From 4a6456fb6877c10494ddea78b57e23053142c38e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 15 Aug 2024 13:02:01 +0200 Subject: [PATCH] [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (#1233) --- joint_state_broadcaster/CMakeLists.txt | 2 + joint_state_broadcaster/package.xml | 1 + .../src/joint_state_broadcaster.cpp | 19 +- .../joint_state_broadcaster_parameters.yaml | 7 + .../test/test_joint_state_broadcaster.cpp | 219 +++++++++++++++++- .../test/test_joint_state_broadcaster.hpp | 8 +- 6 files changed, 248 insertions(+), 8 deletions(-) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 5c383897cc..cc8dc18bf6 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/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() diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 336c04f2c7..aa66a9641e 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -22,6 +22,7 @@ rcutils realtime_tools sensor_msgs + urdf ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5eb0a9b9b6..209b44e557 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/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,6 +243,17 @@ 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_) @@ -249,7 +261,12 @@ bool JointStateBroadcaster::init_joint_data() 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); + } } } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 8f0d4da6c5..c8ca928a12 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/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``." + } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 7400d6bbb9..877d199419 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/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 & joint_names, const std::vector & 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 & joint_names, const std::vector & interfaces) + const std::string & robot_description, const std::vector & joint_names, + const std::vector & 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 JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + init_broadcaster_and_set_parameters("", 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 JOINT_NAMES = {}; + const std::vector 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 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 JOINT_NAMES = {"joint1"}; + const std::vector 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 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 JOINT_NAMES = {"joint1"}; + const std::vector 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 JOINT_NAMES = {"joint1"}; @@ -426,7 +635,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector 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 JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector 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 state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 3e54116d5c..0b4c50e89e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/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 & interfaces = {}); void init_broadcaster_and_set_parameters( - const std::vector & joint_names = {}, - const std::vector & interfaces = {}); + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces); void assign_state_interfaces( const std::vector & joint_names = {},