[Joint State Broadcaster] Publish the joint_states of joints present …

…in the URDF (#1233)

joint_state_broadcaster/CMakeLists.txt

@@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   realtime_tools
   sensor_msgs
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)
@@ -72,6 +73,7 @@ if(BUILD_TESTING)
   )
   ament_target_dependencies(test_joint_state_broadcaster
     hardware_interface
+    ros2_control_test_assets
   )
 endif()
 

joint_state_broadcaster/package.xml

@@ -22,6 +22,7 @@
   <depend>rcutils</depend>
   <depend>realtime_tools</depend>
   <depend>sensor_msgs</depend>
+  <depend>urdf</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -25,6 +25,7 @@
 #include "rclcpp/qos.hpp"
 #include "rclcpp/time.hpp"
 #include "std_msgs/msg/header.hpp"
+#include "urdf/model.h"
 
 namespace rclcpp_lifecycle
 {
@@ -242,14 +243,30 @@ bool JointStateBroadcaster::init_joint_data()
     name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue;
   }
 
+  const std::string & urdf = get_robot_description();
+
+  urdf::Model model;
+  const bool is_model_loaded = !urdf.empty() && model.initString(urdf);
+  if (!is_model_loaded)
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(),
+      "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'",
+      HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT);
+  }
   // filter state interfaces that have at least one of the joint_states fields,
   // the rest will be ignored for this message
   for (const auto & name_ifv : name_if_value_mapping_)
   {
     const auto & interfaces_and_values = name_ifv.second;
     if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}))
     {
-      joint_names_.push_back(name_ifv.first);
+      if (
+        !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded ||
+        model.getJoint(name_ifv.first))
+      {
+        joint_names_.push_back(name_ifv.first);
+      }
     }
   }
 

joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml

@@ -39,3 +39,10 @@ joint_state_broadcaster:
       type: string,
       default_value: "effort",
     }
+  use_urdf_to_filter: {
+    type: bool,
+    default_value: 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``."
+  }

joint_state_broadcaster/test/test_joint_state_broadcaster.cpp

@@ -28,6 +28,7 @@
 #include "rclcpp/executor.hpp"
 #include "rclcpp/executors.hpp"
 #include "rclcpp/utilities.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
 #include "test_joint_state_broadcaster.hpp"
 
 using hardware_interface::HW_IF_EFFORT;
@@ -61,15 +62,17 @@ void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr);
 void JointStateBroadcasterTest::SetUpStateBroadcaster(
   const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces)
 {
-  init_broadcaster_and_set_parameters(joint_names, interfaces);
+  init_broadcaster_and_set_parameters("", joint_names, interfaces);
   assign_state_interfaces(joint_names, interfaces);
 }
 
 void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(
-  const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces)
+  const std::string & robot_description, const std::vector<std::string> & joint_names,
+  const std::vector<std::string> & interfaces)
 {
   const auto result = state_broadcaster_->init(
-    "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options());
+    "joint_state_broadcaster", robot_description, 0, "",
+    state_broadcaster_->define_custom_node_options());
   ASSERT_EQ(result, controller_interface::return_type::OK);
 
   state_broadcaster_->get_node()->set_parameter({"joints", joint_names});
@@ -262,6 +265,212 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
     ElementsAreArray(interface_names_));
 }
 
+TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF)
+{
+  const std::vector<std::string> JOINT_NAMES = {};
+  const std::vector<std::string> IF_NAMES = {interface_names_[0]};
+  init_broadcaster_and_set_parameters("<invalid_urdf></invalid_urdf>", JOINT_NAMES, IF_NAMES);
+  assign_state_interfaces(JOINT_NAMES, IF_NAMES);
+
+  // 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);
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
+
+  // joint state initialized
+  const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
+  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));
+
+  // dynamic joint state initialized
+  const auto & dynamic_joint_state_msg =
+    state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[0].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[1].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[2].interface_names,
+    ElementsAreArray(interface_names_));
+}
+
+TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription)
+{
+  const std::vector<std::string> JOINT_NAMES = {};
+  const std::vector<std::string> IF_NAMES = {interface_names_[0]};
+
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) +
+    ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail;
+  const std::vector<std::string> joint_in_urdf({"joint1", "joint2"});
+  init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES);
+  assign_state_interfaces(JOINT_NAMES, IF_NAMES);
+
+  // 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_in_urdf.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);
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
+
+  // joint state initialized
+  const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
+  ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf));
+  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));
+
+  // dynamic joint state initialized and it will have the data of all the interfaces
+  const auto & dynamic_joint_state_msg =
+    state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size()));
+  ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size()));
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[0].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[1].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[2].interface_names,
+    ElementsAreArray(interface_names_));
+}
+
+TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces)
+{
+  const std::vector<std::string> JOINT_NAMES = {"joint1"};
+  const std::vector<std::string> IF_NAMES = {};
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) +
+    ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail;
+  const std::vector<std::string> joint_in_urdf({"joint1", "joint2"});
+  init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES);
+  assign_state_interfaces(JOINT_NAMES, IF_NAMES);
+
+  // 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_in_urdf.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);
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
+
+  // joint state initialized
+  const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
+  ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_in_urdf));
+  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));
+
+  // dynamic joint state initialized and it will have the data of all the interfaces
+  const auto & dynamic_joint_state_msg =
+    state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size()));
+  ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size()));
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[0].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[1].interface_names,
+    ElementsAreArray(interface_names_));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[2].interface_names,
+    ElementsAreArray(interface_names_));
+}
+
+TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces)
+{
+  const std::vector<std::string> JOINT_NAMES = {"joint1"};
+  const std::vector<std::string> IF_NAMES = interface_names_;
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) +
+    ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail;
+  init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES);
+  assign_state_interfaces(JOINT_NAMES, IF_NAMES);
+
+  // 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, SizeIs(JOINT_NAMES.size() * IF_NAMES.size()));
+  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+
+  // publishers initialized
+  ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
+  ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);
+
+  // joint state initialized
+  const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
+  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));
+
+  // dynamic joint state initialized and it will have the data of all the interfaces
+  const auto & dynamic_joint_state_msg =
+    state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
+  ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
+  ASSERT_THAT(
+    dynamic_joint_state_msg.interface_values[0].interface_names,
+    ElementsAreArray(interface_names_));
+}
+
 TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter)
 {
   const std::vector<std::string> JOINT_NAMES = {"joint1"};
@@ -426,7 +635,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing)
   const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
   const std::vector<std::string> IF_NAMES = {interface_names_[0], interface_names_[1]};
 
-  init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]});
+  init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]});
 
   // assign state with interfaces which are not set in parameters --> We should actually not assign
   // anything because CM will also not do that
@@ -444,7 +653,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing)
   const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
   const std::vector<std::string> IF_NAMES = {interface_names_[0], interface_names_[1]};
 
-  init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]});
+  init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]});
 
   // Manually assign existing interfaces --> one we need is missing
   std::vector<LoanedStateInterface> state_ifs;

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -35,6 +35,10 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
   FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces);
+  FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface);
   FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces);
@@ -60,8 +64,8 @@ class JointStateBroadcasterTest : public ::testing::Test
     const std::vector<std::string> & interfaces = {});
 
   void init_broadcaster_and_set_parameters(
-    const std::vector<std::string> & joint_names = {},
-    const std::vector<std::string> & interfaces = {});
+    const std::string & robot_description, const std::vector<std::string> & joint_names,
+    const std::vector<std::string> & interfaces);
 
   void assign_state_interfaces(
     const std::vector<std::string> & joint_names = {},