diff --git a/fuse_core/include/fuse_core/parameter.hpp b/fuse_core/include/fuse_core/parameter.hpp index 507def3d5..bab6bcecf 100644 --- a/fuse_core/include/fuse_core/parameter.hpp +++ b/fuse_core/include/fuse_core/parameter.hpp @@ -49,6 +49,9 @@ namespace fuse_core { +// Helper function to get a namespace string with a '.' suffix, but only if not empty +std::string joinParameterName(const std::string & left, const std::string & right); + // NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces /** diff --git a/fuse_core/src/parameter.cpp b/fuse_core/src/parameter.cpp index aa272c50c..a2d01a2d7 100644 --- a/fuse_core/src/parameter.cpp +++ b/fuse_core/src/parameter.cpp @@ -83,4 +83,21 @@ detail::list_parameter_override_prefixes( } return output_names; } + +std::string joinParameterName(const std::string & left, const std::string & right) +{ + if (left.empty()) { + return right; + } + if (right.empty()) { + return left; + } + if ('.' != left.back() && '.' != right.front() ) { + return left + '.' + right; + } + if ('.' == left.back() && '.' == right.front()) { + return left + right.substr(1); + } + return left + right; +} } // namespace fuse_core diff --git a/fuse_core/test/test_parameter.cpp b/fuse_core/test/test_parameter.cpp index cce8db2c9..bc334ede5 100644 --- a/fuse_core/test/test_parameter.cpp +++ b/fuse_core/test/test_parameter.cpp @@ -95,3 +95,13 @@ TEST(parameter, list_parameter_override_prefixes) EXPECT_NE(matches.end(), matches.find("baz")); } } + +TEST(parameter, joinParameterName) +{ + EXPECT_EQ("foo.bar", fuse_core::joinParameterName("foo", "bar")); + EXPECT_EQ("foo.bar", fuse_core::joinParameterName("foo.", "bar")); + EXPECT_EQ("foo.bar", fuse_core::joinParameterName("foo", ".bar")); + EXPECT_EQ("foo.bar", fuse_core::joinParameterName("foo.", ".bar")); + EXPECT_EQ("foo", fuse_core::joinParameterName("foo", "")); + EXPECT_EQ("bar", fuse_core::joinParameterName("", "bar")); +} diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 174c5adb6..153bb9eb2 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -69,17 +69,27 @@ void SerializedPublisher::initialize( void SerializedPublisher::onInit() { // Configure the publisher - frame_id_ = fuse_core::getParam(interfaces_, "frame_id", frame_id_); + frame_id_ = fuse_core::getParam( + interfaces_, + fuse_core::joinParameterName(name_, "frame_id"), frame_id_); bool latch = false; - latch = fuse_core::getParam(interfaces_, "latch", latch); + latch = fuse_core::getParam( + interfaces_, + fuse_core::joinParameterName(name_, "latch"), latch); rclcpp::Duration graph_throttle_period{0, 0}; - fuse_core::getPositiveParam(interfaces_, "graph_throttle_period", graph_throttle_period, false); + fuse_core::getPositiveParam( + interfaces_, + fuse_core::joinParameterName(name_, "graph_throttle_period"), + graph_throttle_period, false); bool graph_throttle_use_wall_time{false}; graph_throttle_use_wall_time = - fuse_core::getParam(interfaces_, "graph_throttle_use_wall_time", graph_throttle_use_wall_time); + fuse_core::getParam( + interfaces_, + fuse_core::joinParameterName(name_, "graph_throttle_use_wall_time"), + graph_throttle_use_wall_time); graph_publisher_throttled_callback_.setThrottlePeriod(graph_throttle_period); diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp index b4c37d2a1..484e727a6 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp @@ -221,12 +221,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object void updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index); void updateOrientationVisibility(); - Ogre::SceneNode * root_node_; - Ogre::SceneNode * fixed_orientation_node_; - Ogre::SceneNode * position_scale_node_; - Ogre::SceneNode * position_node_; + Ogre::SceneNode * root_node_ = nullptr; + Ogre::SceneNode * fixed_orientation_node_ = nullptr; + Ogre::SceneNode * position_scale_node_ = nullptr; + Ogre::SceneNode * position_node_ = nullptr; - Ogre::SceneNode * orientation_root_node_; + Ogre::SceneNode * orientation_root_node_ = nullptr; Ogre::SceneNode * orientation_offset_node_[kNumOriShapes]; rviz_rendering::Shape * position_shape_; //!< Ellipse used for the position diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp index 4acefe0ae..dc240f7ad 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp @@ -150,10 +150,10 @@ class Pose2DStampedVisual : public rviz_rendering::Object private: void setPose2DStamped(const Ogre::Vector3 & position, const Ogre::Quaternion & orientation); - Ogre::SceneNode * root_node_; - Ogre::SceneNode * sphere_node_; - Ogre::SceneNode * axes_node_; - Ogre::SceneNode * text_node_; + Ogre::SceneNode * root_node_ = nullptr; + Ogre::SceneNode * sphere_node_ = nullptr; + Ogre::SceneNode * axes_node_ = nullptr; + Ogre::SceneNode * text_node_ = nullptr; bool visible_; diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp index 0c40aca16..c13785839 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp @@ -194,11 +194,11 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object } private: - Ogre::SceneNode * root_node_; - Ogre::SceneNode * relative_pose_line_node_; - Ogre::SceneNode * error_line_node_; - Ogre::SceneNode * relative_pose_axes_node_; - Ogre::SceneNode * text_node_; + Ogre::SceneNode * root_node_ = nullptr; + Ogre::SceneNode * relative_pose_line_node_ = nullptr; + Ogre::SceneNode * error_line_node_ = nullptr; + Ogre::SceneNode * relative_pose_axes_node_ = nullptr; + Ogre::SceneNode * text_node_ = nullptr; std::shared_ptr relative_pose_line_; std::shared_ptr error_line_; diff --git a/fuse_viz/src/mapped_covariance_visual.cpp b/fuse_viz/src/mapped_covariance_visual.cpp index fe3123607..95105693c 100644 --- a/fuse_viz/src/mapped_covariance_visual.cpp +++ b/fuse_viz/src/mapped_covariance_visual.cpp @@ -315,16 +315,16 @@ MappedCovarianceVisual::MappedCovarianceVisual( MappedCovarianceVisual::~MappedCovarianceVisual() { delete position_shape_; - scene_manager_->destroySceneNode(position_node_->getName()); + scene_manager_->destroySceneNode(position_node_); for (int i = 0; i < kNumOriShapes; i++) { delete orientation_shape_[i]; - scene_manager_->destroySceneNode(orientation_offset_node_[i]->getName()); + scene_manager_->destroySceneNode(orientation_offset_node_[i]); } - scene_manager_->destroySceneNode(position_scale_node_->getName()); - scene_manager_->destroySceneNode(fixed_orientation_node_->getName()); - scene_manager_->destroySceneNode(root_node_->getName()); + scene_manager_->destroySceneNode(position_scale_node_); + scene_manager_->destroySceneNode(fixed_orientation_node_); + scene_manager_->destroySceneNode(root_node_); } void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose) diff --git a/fuse_viz/src/pose_2d_stamped_visual.cpp b/fuse_viz/src/pose_2d_stamped_visual.cpp index 372ae33ea..19f4a1773 100644 --- a/fuse_viz/src/pose_2d_stamped_visual.cpp +++ b/fuse_viz/src/pose_2d_stamped_visual.cpp @@ -90,10 +90,10 @@ Pose2DStampedVisual::Pose2DStampedVisual( Pose2DStampedVisual::~Pose2DStampedVisual() { delete text_; - scene_manager_->destroySceneNode(sphere_node_->getName()); - scene_manager_->destroySceneNode(axes_node_->getName()); - scene_manager_->destroySceneNode(text_node_->getName()); - scene_manager_->destroySceneNode(root_node_->getName()); + scene_manager_->destroySceneNode(sphere_node_); + scene_manager_->destroySceneNode(axes_node_); + scene_manager_->destroySceneNode(text_node_); + scene_manager_->destroySceneNode(root_node_); } void Pose2DStampedVisual::setPose2DStamped( diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp index c83c1371f..49ee0b91b 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp @@ -117,11 +117,11 @@ RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( RelativePose2DStampedConstraintVisual::~RelativePose2DStampedConstraintVisual() { delete text_; - scene_manager_->destroySceneNode(relative_pose_line_node_->getName()); - scene_manager_->destroySceneNode(error_line_node_->getName()); - scene_manager_->destroySceneNode(relative_pose_axes_node_->getName()); - scene_manager_->destroySceneNode(text_node_->getName()); - scene_manager_->destroySceneNode(root_node_->getName()); + scene_manager_->destroySceneNode(relative_pose_line_node_); + scene_manager_->destroySceneNode(error_line_node_); + scene_manager_->destroySceneNode(relative_pose_axes_node_); + scene_manager_->destroySceneNode(text_node_); + scene_manager_->destroySceneNode(root_node_); } void RelativePose2DStampedConstraintVisual::setConstraint(