diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h index 60736b7fd..8bfc757dd 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h @@ -100,8 +100,8 @@ struct Odometry2DPublisherParams : public ParameterBase map_frame_id = fuse_core::getParam(interfaces, ns + "map_frame_id", map_frame_id); odom_frame_id = fuse_core::getParam(interfaces, ns + "odom_frame_id", odom_frame_id); base_link_frame_id = fuse_core::getParam(interfaces, ns + "base_link_frame_id", base_link_frame_id); - base_link_frame_id = fuse_core::getParam(interfaces, ns + "base_link_output_frame_id", base_link_frame_id); - odom_frame_id = fuse_core::getParam(interfaces, ns + "world_frame_id", odom_frame_id); + base_link_output_frame_id = fuse_core::getParam(interfaces, ns + "base_link_output_frame_id", base_link_output_frame_id); + world_frame_id = fuse_core::getParam(interfaces, ns + "world_frame_id", world_frame_id); const bool frames_valid = map_frame_id != odom_frame_id && diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.h b/fuse_models/include/fuse_models/parameters/parameter_base.h index fd0dc4714..5e3442e21 100644 --- a/fuse_models/include/fuse_models/parameters/parameter_base.h +++ b/fuse_models/include/fuse_models/parameters/parameter_base.h @@ -88,8 +88,8 @@ inline std::vector loadSensorConfig( fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { - std::vector dimensions = - fuse_core::getParam(interfaces, name, dimensions); + std::vector dimensions; + dimensions = fuse_core::getParam(interfaces, name, dimensions); if (!dimensions.empty()) { return common::getDimensionIndices(dimensions); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index b77fcbb43..5a5bc7915 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -78,7 +78,7 @@ void GraphIgnition::onInit() interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.reset_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), rclcpp::ServicesQoS(), cb_group_ ); @@ -98,7 +98,7 @@ void GraphIgnition::onInit() set_graph_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_graph_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_graph_service), std::bind( &GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS(), @@ -180,14 +180,8 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg) } auto srv = std::make_shared(); + // No need to spin since node is optimizer node, which should be spinning auto result_future = reset_client_->async_send_request(srv); - if (rclcpp::spin_until_future_complete( - interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) - != rclcpp::FutureReturnCode::SUCCESS) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); - } } // Now that the optimizer has been reset, actually send the initial state constraints to the optimizer diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index e01a727fb..f41131f67 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -103,7 +103,7 @@ void Unicycle2DIgnition::onInit() interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.reset_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), rclcpp::ServicesQoS(), cb_group_ ); @@ -123,7 +123,7 @@ void Unicycle2DIgnition::onInit() set_pose_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_pose_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), std::bind( &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS(), @@ -132,7 +132,7 @@ void Unicycle2DIgnition::onInit() set_pose_deprecated_service_ = rclcpp::create_service( interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), - interfaces_.get_node_services_interface()->resolve_service_name(params_.set_pose_deprecated_service), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), std::bind( &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS(), @@ -272,14 +272,8 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta } auto srv = std::make_shared(); + // No need to spin since node is optimizer node, which should be spinning auto result_future = reset_client_->async_send_request(srv); - if (rclcpp::spin_until_future_complete( - interfaces_.get_node_base_interface(), result_future, std::chrono::seconds(10)) - != rclcpp::FutureReturnCode::SUCCESS) - { - // The reset() service failed. Propagate that failure to the caller of this service. - throw std::runtime_error("Failed to call the '" + std::string(reset_client_->get_service_name()) + "' service."); - } } // Now that the pose has been validated and the optimizer has been reset, actually send the initial state constraints