[Joint State Broadcaster] Add option to publish joint states to local topics (ros-controls#218)

* Use local topics for publishing joint states

* Parameterize local namespace topics.

* Unify test structure.

* Use get_node() instead of node_ everywhere.

* Add missing return in init method.

* Update userdoc.rst

Author: destogl

joint_state_broadcaster/doc/userdoc.rst

@@ -18,5 +18,8 @@ All available *joint state interfaces* are used by this broadcaster.
 Parameters
 ----------
 
-extra_joints (optional; string array; default: empty)
-  Names of extra joints to be added to ``/joint_states`` and ``/dynamic_joint_states`` with state set to 0.
+``use_local_topics``
+  Optional parameter (boolean; default: ``False``) defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``.
+
+``extra_joints``
+  Optional parameter (string array) with names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0.

joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp

@@ -36,6 +36,10 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   JOINT_STATE_BROADCASTER_PUBLIC
   JointStateBroadcaster();
 
+  JOINT_STATE_BROADCASTER_PUBLIC
+  controller_interface::return_type
+  init(const std::string & controller_name) override;
+
   JOINT_STATE_BROADCASTER_PUBLIC
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
 
@@ -64,6 +68,8 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface
   void init_dynamic_joint_state_msg();
 
 protected:
+  bool use_local_topics_;
+
   //  For the JointState message,
   //  we store the name of joints with compatible interfaces
   std::vector<std::string> joint_names_;

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -48,6 +48,24 @@ using hardware_interface::HW_IF_EFFORT;
 JointStateBroadcaster::JointStateBroadcaster()
 {}
 
+controller_interface::return_type
+JointStateBroadcaster::init(const std::string & controller_name)
+{
+  auto ret = ControllerInterface::init(controller_name);
+  if (ret != controller_interface::return_type::OK) {
+    return ret;
+  }
+
+  try {
+    get_node()->declare_parameter<bool>("use_local_topics", false);
+  } catch (const std::exception & e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return controller_interface::return_type::ERROR;
+  }
+
+  return controller_interface::return_type::OK;
+}
+
 controller_interface::InterfaceConfiguration
 JointStateBroadcaster::command_interface_configuration() const
 {
@@ -65,13 +83,17 @@ const
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 JointStateBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
+  use_local_topics_ = get_node()->get_parameter("use_local_topics").as_bool();
+
   try {
+    const std::string topic_name_prefix = use_local_topics_ ? "~/" : "";
+
     joint_state_publisher_ = get_node()->create_publisher<sensor_msgs::msg::JointState>(
-      "joint_states", rclcpp::SystemDefaultsQoS());
+      topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS());
 
     dynamic_joint_state_publisher_ =
       get_node()->create_publisher<control_msgs::msg::DynamicJointState>(
-      "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
+      topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
   } catch (const std::exception & e) {
     // get_node() may throw, logging raw here
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
@@ -209,8 +231,8 @@ JointStateBroadcaster::update()
       state_interface.get_value());
   }
 
-  joint_state_msg_.header.stamp = node_->get_clock()->now();
-  dynamic_joint_state_msg_.header.stamp = node_->get_clock()->now();
+  joint_state_msg_.header.stamp = get_node()->get_clock()->now();
+  dynamic_joint_state_msg_.header.stamp = get_node()->get_clock()->now();
 
   // update joint state message and dynamic joint state message
   for (auto i = 0ul; i < joint_names_.size(); ++i) {

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -115,7 +115,7 @@ TEST_F(JointStateBroadcasterTest, ConfigureErrorTest)
   ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_);
 
   // configure failed
-  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR);
+  ASSERT_THROW(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), std::exception);
 
   SetUpStateBroadcaster();
   // check state remains unchanged
@@ -198,10 +198,8 @@ TEST_F(JointStateBroadcasterTest, UpdateTest)
   ASSERT_EQ(state_broadcaster_->update(), controller_interface::return_type::OK);
 }
 
-TEST_F(JointStateBroadcasterTest, JointStatePublishTest)
+void JointStateBroadcasterTest::test_published_joint_state_message(const std::string & topic)
 {
-  SetUpStateBroadcaster();
-
   auto node_state = state_broadcaster_->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
@@ -213,7 +211,7 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest)
     {
     };
   auto subscription = test_node.create_subscription<sensor_msgs::msg::JointState>(
-    "joint_states",
+    topic,
     10,
     subs_callback);
 
@@ -238,10 +236,24 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest)
   ASSERT_THAT(joint_state_msg.effort, ElementsAreArray(joint_state_msg.position));
 }
 
-TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest)
+TEST_F(JointStateBroadcasterTest, JointStatePublishTest)
+{
+  SetUpStateBroadcaster();
+
+  test_published_joint_state_message("joint_states");
+}
+
+TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic)
 {
   SetUpStateBroadcaster();
+  state_broadcaster_->get_node()->set_parameter({"use_local_topics", true});
+
+  test_published_joint_state_message("joint_state_broadcaster/joint_states");
+}
 
+void
+JointStateBroadcasterTest::test_published_dynamic_joint_state_message(const std::string & topic)
+{
   auto node_state = state_broadcaster_->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
 
@@ -253,7 +265,7 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest)
     {
     };
   auto subscription = test_node.create_subscription<control_msgs::msg::DynamicJointState>(
-    "dynamic_joint_states",
+    topic,
     10,
     subs_callback);
 
@@ -290,6 +302,21 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest)
   }
 }
 
+TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest)
+{
+  SetUpStateBroadcaster();
+
+  test_published_dynamic_joint_state_message("dynamic_joint_states");
+}
+
+TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic)
+{
+  SetUpStateBroadcaster();
+  state_broadcaster_->get_node()->set_parameter({"use_local_topics", true});
+
+  test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states");
+}
+
 TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
 {
   // joint state not initialized yet
@@ -309,10 +336,8 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
   SetUpStateBroadcaster();
 
   // Add extra joints as parameters
-  auto broadcaster_node = state_broadcaster_->get_node();
   const std::vector<std::string> extra_joint_names = {"extra1", "extra2", "extra3"};
-  const rclcpp::Parameter extra_joints_parameters("extra_joints", extra_joint_names);
-  broadcaster_node->set_parameter(extra_joints_parameters);
+  state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names});
 
   // configure ok
   ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -42,6 +42,10 @@ class JointStateBroadcasterTest : public ::testing::Test
 
   void SetUpStateBroadcaster();
 
+  void test_published_joint_state_message(const std::string & topic);
+
+  void test_published_dynamic_joint_state_message(const std::string & topic);
+
 protected:
   // dummy joint state values used for tests
   const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};