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

…in the URDF (ros-controls#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 = {},