Fix alloc error and some bugs

Signed-off-by: methylDragon <methylDragon@gmail.com>

Author: methylDragon

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 &&

fuse_models/include/fuse_models/parameters/parameter_base.h

@@ -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);

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<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(),

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<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(),
@@ -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(),