Skip to content

Commit

Permalink
Fix alloc error and some bugs
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
methylDragon committed Jan 10, 2023
1 parent 355e3e1 commit 424beef
Show file tree
Hide file tree
Showing 4 changed files with 11 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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 &&
Expand Down
4 changes: 2 additions & 2 deletions fuse_models/include/fuse_models/parameters/parameter_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,8 @@ inline std::vector<size_t> loadSensorConfig(
fuse_core::node_interfaces::NodeInterfaces<fuse_core::node_interfaces::Parameters> interfaces,
const std::string& name)
{
std::vector<std::string> dimensions =
fuse_core::getParam(interfaces, name, dimensions);
std::vector<std::string> dimensions;
dimensions = fuse_core::getParam(interfaces, name, dimensions);
if (!dimensions.empty())
{
return common::getDimensionIndices<T>(dimensions);
Expand Down
12 changes: 3 additions & 9 deletions fuse_models/src/graph_ignition.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_
);
Expand All @@ -98,7 +98,7 @@ void GraphIgnition::onInit()
set_graph_service_ = rclcpp::create_service<fuse_msgs::srv::SetGraph>(
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(),
Expand Down Expand Up @@ -180,14 +180,8 @@ void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg)
}

auto srv = std::make_shared<std_srvs::srv::Empty::Request>();
// 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
Expand Down
14 changes: 4 additions & 10 deletions fuse_models/src/unicycle_2d_ignition.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_
);
Expand All @@ -123,7 +123,7 @@ void Unicycle2DIgnition::onInit()
set_pose_service_ = rclcpp::create_service<fuse_msgs::srv::SetPose>(
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(),
Expand All @@ -132,7 +132,7 @@ void Unicycle2DIgnition::onInit()
set_pose_deprecated_service_ = rclcpp::create_service<fuse_msgs::srv::SetPoseDeprecated>(
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(),
Expand Down Expand Up @@ -272,14 +272,8 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta
}

auto srv = std::make_shared<std_srvs::srv::Empty::Request>();
// 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
Expand Down

0 comments on commit 424beef

Please sign in to comment.