Add parameter for deactivating dynamic_joint_states (backport #2064) (#2067)

Author: mergify[bot]

doc/release_notes.rst

@@ -18,6 +18,11 @@ imu_sensor_broadcaster
 *******************************
 * IMU sensor broadcaster is now a chainable controller. It supports a calibration by means of a rotation, defined as euler angles, to its target frame. (`#1833 <https://github.com/ros-controls/ros2_controllers/pull/1833/files>`__).
 
+joint_state_broadcaster
+************************
+* Make all parameters read-only (the never got re-evaluated after initialization anyways). (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
+* Added parameter ``publish_dynamic_joint_states`` to enable/disable publishing of dynamic joint states. (`#2064 <https://github.com/ros-controls/ros2_controllers/pull/2064>`_)
+
 joint_trajectory_controller
 *******************************
 * The controller now supports the new anti-windup strategy of the PID class, which allows for more flexible control of the anti-windup behavior. (`#1759 <https://github.com/ros-controls/ros2_controllers/pull/1759>`__).

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -144,13 +144,15 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
       std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(
         joint_state_publisher_);
 
-    dynamic_joint_state_publisher_ =
-      get_node()->create_publisher<control_msgs::msg::DynamicJointState>(
-        topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
-
-    realtime_dynamic_joint_state_publisher_ =
-      std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(
-        dynamic_joint_state_publisher_);
+    if (params_.publish_dynamic_joint_states)
+    {
+      dynamic_joint_state_publisher_ =
+        get_node()->create_publisher<control_msgs::msg::DynamicJointState>(
+          topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS());
+      realtime_dynamic_joint_state_publisher_ =
+        std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(
+          dynamic_joint_state_publisher_);
+    }
   }
   catch (const std::exception & e)
   {
@@ -202,7 +204,11 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate(
 
   init_auxiliary_data();
   init_joint_state_msg();
-  init_dynamic_joint_state_msg();
+
+  if (params_.publish_dynamic_joint_states)
+  {
+    init_dynamic_joint_state_msg();
+  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -390,22 +396,6 @@ bool JointStateBroadcaster::use_all_available_interfaces() const
   return params_.joints.empty() || params_.interfaces.empty();
 }
 
-double get_value(
-  const std::unordered_map<std::string, std::unordered_map<std::string, double>> & map,
-  const std::string & name, const std::string & interface_name)
-{
-  const auto & interfaces_and_values = map.at(name);
-  const auto interface_and_value = interfaces_and_values.find(interface_name);
-  if (interface_and_value != interfaces_and_values.cend())
-  {
-    return interface_and_value->second;
-  }
-  else
-  {
-    return kUninitializedValue;
-  }
-}
-
 controller_interface::return_type JointStateBroadcaster::update(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {

joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml

@@ -2,11 +2,13 @@ joint_state_broadcaster:
   use_local_topics: {
     type: bool,
     default_value: false,
+    read_only: true,
     description: "Defining if ``joint_states`` and ``dynamic_joint_states`` messages should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``."
   }
   joints: {
     type: string_array,
     default_value: [],
+    read_only: true,
     description: "Parameter to support broadcasting of only specific joints and interfaces.
     It has to be used in combination with the ``interfaces`` parameter.
     If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be
@@ -16,11 +18,13 @@ joint_state_broadcaster:
   extra_joints: {
     type: string_array,
     default_value: [],
+    read_only: true,
     description: "Names of extra joints to be added to ``joint_states`` and ``dynamic_joint_states`` with state set to 0."
   }
   interfaces: {
     type: string_array,
     default_value: [],
+    read_only: true,
     description: "Parameter to support broadcasting of only specific joints and interfaces.
     It has to be used in combination with the ``joints`` parameter.
     If either ``joints`` or ``interfaces`` is left empty, all available state interfaces will be
@@ -30,24 +34,35 @@ joint_state_broadcaster:
     position: {
       type: string,
       default_value: "position",
+      read_only: true,
     }
     velocity: {
       type: string,
       default_value: "velocity",
+      read_only: true,
     }
     effort: {
       type: string,
       default_value: "effort",
+      read_only: true,
     }
   use_urdf_to_filter: {
     type: bool,
     default_value: true,
+    read_only: true,
     description: "Uses the robot_description to filter the ``joint_states`` topic.
     If true, the broadcaster will publish the data of the joints present in the URDF alone.
     If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``."
   }
   frame_id: {
     type: string,
     default_value: "base_link",
+    read_only: true,
     description: "The frame_id to be used in the published joint states. This parameter allows rviz2 to visualize the effort of the joints."
   }
+  publish_dynamic_joint_states: {
+    type: bool,
+    default_value: true,
+    read_only: true,
+    description: "Whether to publish dynamic joint states."
+  }

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -60,28 +60,35 @@ void JointStateBroadcasterTest::SetUp()
 void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); }
 
 void JointStateBroadcasterTest::SetUpStateBroadcaster(
-  const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces)
+  const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces,
+  const std::vector<rclcpp::Parameter> & parameter_overrides)
 {
-  init_broadcaster_and_set_parameters("", joint_names, interfaces);
+  init_broadcaster_and_set_parameters("", joint_names, interfaces, parameter_overrides);
   assign_state_interfaces(joint_names, interfaces);
 }
 
 void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(
   const std::string & robot_description, const std::vector<std::string> & joint_names,
-  const std::vector<std::string> & interfaces)
+  const std::vector<std::string> & interfaces,
+  const std::vector<rclcpp::Parameter> & parameter_overrides)
 {
+  auto local_parameter_overrides = parameter_overrides;
+  local_parameter_overrides.push_back(rclcpp::Parameter("frame_id", frame_id_));
+  local_parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names));
+  local_parameter_overrides.push_back(rclcpp::Parameter("interfaces", interfaces));
+
   controller_interface::ControllerInterfaceParams params;
   params.controller_name = "joint_state_broadcaster";
   params.robot_description = robot_description;
   params.update_rate = 0;
   params.node_namespace = "";
-  params.node_options = state_broadcaster_->define_custom_node_options();
+  params.node_options = state_broadcaster_->define_custom_node_options()
+                          .allow_undeclared_parameters(false)
+                          .parameter_overrides(local_parameter_overrides)
+                          .automatically_declare_parameters_from_overrides(false);
+  ;
   const auto result = state_broadcaster_->init(params);
   ASSERT_EQ(result, controller_interface::return_type::OK);
-
-  state_broadcaster_->get_node()->set_parameter({"joints", joint_names});
-  state_broadcaster_->get_node()->set_parameter({"interfaces", interfaces});
-  state_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
 }
 
 void JointStateBroadcasterTest::assign_state_interfaces(
@@ -205,6 +212,42 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
     ElementsAreArray(interface_names_));
 }
 
+TEST_F(JointStateBroadcasterTest, ActivateEmptyWithoutDynamicJointStatesPublisherTest)
+{
+  // publishers not initialized yet
+  ASSERT_FALSE(state_broadcaster_->joint_state_publisher_);
+  ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_);
+
+  SetUpStateBroadcaster({}, {}, {rclcpp::Parameter("publish_dynamic_joint_states", false)});
+  // configure ok
+  ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  const size_t NUM_JOINTS = joint_names_.size();
+
+  // check interface configuration
+  auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, IsEmpty());
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE);
+  auto state_if_conf = state_broadcaster_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, IsEmpty());
+  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL);
+
+  // publisher initialized
+  ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
+  // dynamic joint states publisher still not initialized
+  ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
+
+  // joint state initialized
+  const auto & joint_state_msg = state_broadcaster_->joint_state_msg_;
+  ASSERT_EQ(joint_state_msg.header.frame_id, frame_id_);
+  ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
+  ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS));
+}
+
 TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest)
 {
   // publishers not initialized yet
@@ -846,10 +889,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping)
 {
   const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
   const std::vector<std::string> IF_NAMES = {custom_interface_name_};
-  SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
-
-  state_broadcaster_->get_node()->set_parameter(
-    {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_});
+  SetUpStateBroadcaster(
+    JOINT_NAMES, IF_NAMES,
+    {rclcpp::Parameter(
+      std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_)});
 
   // configure ok
   ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
@@ -901,10 +944,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate)
 {
   const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
   const std::vector<std::string> IF_NAMES = {custom_interface_name_};
-  SetUpStateBroadcaster(JOINT_NAMES, IF_NAMES);
-
-  state_broadcaster_->get_node()->set_parameter(
-    {std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_});
+  SetUpStateBroadcaster(
+    JOINT_NAMES, IF_NAMES,
+    {rclcpp::Parameter(
+      std::string("map_interface_to_joint_state.") + HW_IF_POSITION, custom_interface_name_)});
 
   sensor_msgs::msg::JointState joint_state_msg;
   activate_and_get_joint_state_message("joint_states", joint_state_msg);
@@ -1164,8 +1207,7 @@ TEST_F(JointStateBroadcasterTest, JointStatePublishTest)
 
 TEST_F(JointStateBroadcasterTest, JointStatePublishTestLocalTopic)
 {
-  SetUpStateBroadcaster();
-  state_broadcaster_->get_node()->set_parameter({"use_local_topics", true});
+  SetUpStateBroadcaster({}, {}, {rclcpp::Parameter("use_local_topics", true)});
 
   test_published_joint_state_message("joint_state_broadcaster/joint_states");
 }
@@ -1245,8 +1287,7 @@ TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTest)
 
 TEST_F(JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic)
 {
-  SetUpStateBroadcaster();
-  state_broadcaster_->get_node()->set_parameter({"use_local_topics", true});
+  SetUpStateBroadcaster({}, {}, {rclcpp::Parameter("use_local_topics", true)});
 
   test_published_dynamic_joint_state_message("joint_state_broadcaster/dynamic_joint_states");
 }
@@ -1257,11 +1298,9 @@ TEST_F(JointStateBroadcasterTest, ExtraJointStatePublishTest)
   ASSERT_FALSE(state_broadcaster_->realtime_joint_state_publisher_);
   ASSERT_FALSE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
 
-  SetUpStateBroadcaster();
-
   // Add extra joints as parameters
   const std::vector<std::string> extra_joint_names = {"extra1", "extra2", "extra3"};
-  state_broadcaster_->get_node()->set_parameter({"extra_joints", extra_joint_names});
+  SetUpStateBroadcaster({}, {}, {rclcpp::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

@@ -34,6 +34,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
 {
   FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyWithoutDynamicJointStatesPublisherTest);
   FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF);
@@ -62,11 +63,13 @@ class JointStateBroadcasterTest : public ::testing::Test
 
   void SetUpStateBroadcaster(
     const std::vector<std::string> & joint_names = {},
-    const std::vector<std::string> & interfaces = {});
+    const std::vector<std::string> & interfaces = {},
+    const std::vector<rclcpp::Parameter> & parameter_overrides = {});
 
   void init_broadcaster_and_set_parameters(
     const std::string & robot_description, const std::vector<std::string> & joint_names,
-    const std::vector<std::string> & interfaces);
+    const std::vector<std::string> & interfaces,
+    const std::vector<rclcpp::Parameter> & parameter_overrides = {});
 
   void assign_state_interfaces(
     const std::vector<std::string> & joint_names = {},