diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index d7f5890659..8471f642d5 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -7,7 +7,6 @@ export_windows_symbols() set(THIS_PACKAGE_INCLUDE_DEPENDS builtin_interfaces - control_msgs controller_interface generate_parameter_library pluginlib @@ -44,7 +43,6 @@ target_link_libraries(joint_state_broadcaster PUBLIC realtime_tools::realtime_tools urdf::urdf ${sensor_msgs_TARGETS} - ${control_msgs_TARGETS} ${builtin_interfaces_TARGETS}) pluginlib_export_plugin_description_file(controller_interface joint_state_plugin.xml) diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index a66ba98bb0..9f4251d6c0 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -5,7 +5,7 @@ joint_state_broadcaster ======================= -The broadcaster reads all state interfaces and reports them on ``/joint_states`` and ``/dynamic_joint_states``. +The broadcaster reads all state interfaces and reports them on ``/joint_states``. Commands -------- @@ -29,31 +29,11 @@ Published topics and ``effort`` — for joints that provide them. If a joint does not expose a given movement interface, that field is omitted/left empty for that joint in the message. -* ``/dynamic_joint_states`` (``control_msgs/msg/DynamicJointState``): - - Publishes **all available state interfaces** for each joint. This includes the - movement interfaces (position/velocity/effort) *and* any additional or custom - interfaces provided by the hardware (e.g., temperature, voltage, torque sensor - readings, calibration flags). - - The message maps ``joint_names`` to per-joint interface name lists and values. - Example payload:: - - joint_names: [joint1, joint2] - interface_values: - - interface_names: [position, velocity, effort] - values: [1.5708, 0.0, 0.20] - - interface_names: [position, temperature] - values: [0.7854, 42.1] - - Use this topic if you need *every* reported interface, not just movement. - .. note:: - If ``use_local_topics`` is set to ``true``, both topics are published in the - controller’s namespace (e.g., ``/my_state_broadcaster/joint_states`` and - ``/my_state_broadcaster/dynamic_joint_states``). If ``false`` (default), - they are published at the root (e.g., ``/joint_states``). + If ``use_local_topics`` is set to ``true``, the topic is published in the + controller’s namespace (e.g., ``/my_state_broadcaster/joint_states``). If ``false`` (default), + it is published at the root (e.g., ``/joint_states``). Parameters diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index 05d2334d0b..73220f81eb 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -20,7 +20,6 @@ #include #include -#include "control_msgs/msg/dynamic_joint_state.hpp" #include "controller_interface/controller_interface.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" @@ -58,8 +57,6 @@ namespace joint_state_broadcaster * Publishes to: * - \b joint_states (sensor_msgs::msg::JointState): Joint states related to movement * (position, velocity, effort). - * - \b dynamic_joint_states (control_msgs::msg::DynamicJointState): Joint states regardless of - * its interface type. */ class JointStateBroadcaster : public controller_interface::ControllerInterface { @@ -88,9 +85,13 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface bool init_joint_data(); void init_auxiliary_data(); void init_joint_state_msg(); - void init_dynamic_joint_state_msg(); + + [[deprecated( + "use_all_available_interfaces is deprecated. Use use_urdf_joint_interfaces method instead")]] bool use_all_available_interfaces() const; + bool use_urdf_joint_interfaces() const; + protected: // Optional parameters std::shared_ptr param_listener_; @@ -107,14 +108,7 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface realtime_joint_state_publisher_; sensor_msgs::msg::JointState joint_state_msg_; - // For the DynamicJointState format, we use a map to buffer values in for easier lookup - // This allows to preserve whatever order or names/interfaces were initialized. std::unordered_map> name_if_value_mapping_; - std::shared_ptr> - dynamic_joint_state_publisher_; - std::shared_ptr> - realtime_dynamic_joint_state_publisher_; - control_msgs::msg::DynamicJointState dynamic_joint_state_msg_; urdf::Model model_; bool is_model_loaded_ = false; @@ -134,8 +128,6 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface }; std::vector joint_states_data_; - - std::vector> dynamic_joint_states_data_; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 435e3ad178..6b6dc2b195 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -24,7 +24,6 @@ backward_ros builtin_interfaces - control_msgs controller_interface generate_parameter_library pluginlib diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index b24152b0d8..bf07ddb416 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -68,9 +68,22 @@ controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interf { controller_interface::InterfaceConfiguration state_interfaces_config; - if (use_all_available_interfaces()) + if (use_urdf_joint_interfaces()) { - state_interfaces_config.type = controller_interface::interface_configuration_type::ALL; + state_interfaces_config.type = + controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT; + for (const auto & joint : model_.joints_) + { + if ( + joint.second->type == urdf::Joint::CONTINUOUS || + joint.second->type == urdf::Joint::REVOLUTE || joint.second->type == urdf::Joint::PRISMATIC) + { + for (const auto & interface : map_interface_to_joint_state_) + { + state_interfaces_config.names.push_back(joint.first + "/" + interface.first); + } + } + } } else { @@ -92,20 +105,12 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( { params_ = param_listener_->get_params(); - if (params_.publish_dynamic_joint_states) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] The 'publish_dynamic_joint_states' parameter is deprecated and will be removed " - "in future releases. Please update your configuration."); - } - - if (use_all_available_interfaces()) + if (use_urdf_joint_interfaces()) { RCLCPP_INFO( get_node()->get_logger(), - "'joints' or 'interfaces' parameter is empty. " - "All available state interfaces will be published"); + "'joints' or 'interfaces' parameter is empty. Will try to publish all available state " + "interfaces of the URDF joints."); params_.joints.clear(); params_.interfaces.clear(); } @@ -151,16 +156,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( realtime_joint_state_publisher_ = std::make_shared>( joint_state_publisher_); - - if (params_.publish_dynamic_joint_states) - { - dynamic_joint_state_publisher_ = - get_node()->create_publisher( - topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS()); - realtime_dynamic_joint_state_publisher_ = - std::make_shared>( - dynamic_joint_state_publisher_); - } } catch (const std::exception & e) { @@ -174,10 +169,33 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( 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); + if (use_urdf_joint_interfaces()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Robot description could not be loaded. Cannot determine URDF joints to publish. " + "Either provide a valid robot description, or explicitly set both 'joints' and " + "'interfaces' parameters."); + return CallbackReturn::ERROR; + } + else + { + if (params_.use_urdf_to_filter) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'use_urdf_to_filter' parameter is set to true, but robot description could not be " + "parsed. Will publish the joints defined in 'joints' parameter without filtering with " + "URDF. Fix the robot description to filter joints with URDF."); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), + "Failed to parse robot description. Will publish the joints defined in 'joints' " + "parameter along with the defined interfaces in 'interface' parameter."); + } + } } // joint_names reserve space for all joints @@ -213,11 +231,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_activate( init_auxiliary_data(); init_joint_state_msg(); - if (params_.publish_dynamic_joint_states) - { - init_dynamic_joint_state_msg(); - } - return CallbackReturn::SUCCESS; } @@ -286,18 +299,16 @@ bool JointStateBroadcaster::init_joint_data() std::reverse(joint_names_.begin(), joint_names_.end()); if (is_model_loaded_ && params_.use_urdf_to_filter && params_.joints.empty()) { - std::vector joint_names_filtered; - for (const auto & [joint_name, urdf_joint] : model_.joints_) - { - if (urdf_joint && urdf_joint->type != urdf::Joint::FIXED) - { - if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) != joint_names_.end()) + // Preserve the order from the first pass; only remove fixed joints + joint_names_.erase( + std::remove_if( + joint_names_.begin(), joint_names_.end(), + [this](const std::string & name) { - joint_names_filtered.push_back(joint_name); - } - } - } - joint_names_ = joint_names_filtered; + const auto urdf_joint = model_.getJoint(name); + return !urdf_joint || urdf_joint->type == urdf::Joint::FIXED; + }), + joint_names_.end()); } // Add extra joints from parameters, each joint will be added to joint_names_ and @@ -374,44 +385,12 @@ void JointStateBroadcaster::init_joint_state_msg() } } -void JointStateBroadcaster::init_dynamic_joint_state_msg() +bool JointStateBroadcaster::use_all_available_interfaces() const { - dynamic_joint_state_msg_.header.frame_id = frame_id_; - dynamic_joint_state_msg_.joint_names.clear(); - dynamic_joint_state_msg_.interface_values.clear(); - for (const auto & name_ifv : name_if_value_mapping_) - { - const auto & name = name_ifv.first; - const auto & interfaces_and_values = name_ifv.second; - dynamic_joint_state_msg_.joint_names.push_back(name); - control_msgs::msg::InterfaceValue if_value; - for (const auto & interface_and_value : interfaces_and_values) - { - if_value.interface_names.emplace_back(interface_and_value.first); - if_value.values.emplace_back(kUninitializedValue); - } - dynamic_joint_state_msg_.interface_values.emplace_back(if_value); - } - - // save dynamic joint state data - dynamic_joint_states_data_.clear(); - for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) - { - dynamic_joint_states_data_.push_back(std::vector()); - - const auto & name = dynamic_joint_state_msg_.joint_names[ji]; - - for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); - ++ii) - { - const auto & interface_name = - dynamic_joint_state_msg_.interface_values[ji].interface_names[ii]; - dynamic_joint_states_data_[ji].push_back(&name_if_value_mapping_[name][interface_name]); - } - } + return this->use_urdf_joint_interfaces(); } -bool JointStateBroadcaster::use_all_available_interfaces() const +bool JointStateBroadcaster::use_urdf_joint_interfaces() const { return params_.joints.empty() || params_.interfaces.empty(); } @@ -442,7 +421,6 @@ controller_interface::return_type JointStateBroadcaster::update( { joint_state_msg_.header.stamp = time; - // update joint state message and dynamic joint state message for (size_t i = 0; i < joint_names_.size(); ++i) { joint_state_msg_.position[i] = joint_states_data_[i].position_; @@ -452,21 +430,6 @@ controller_interface::return_type JointStateBroadcaster::update( realtime_joint_state_publisher_->try_publish(joint_state_msg_); } - if (realtime_dynamic_joint_state_publisher_) - { - dynamic_joint_state_msg_.header.stamp = time; - for (auto ji = 0u; ji < dynamic_joint_state_msg_.joint_names.size(); ++ji) - { - for (auto ii = 0u; ii < dynamic_joint_state_msg_.interface_values[ji].interface_names.size(); - ++ii) - { - dynamic_joint_state_msg_.interface_values[ji].values[ii] = - *dynamic_joint_states_data_[ji][ii]; - } - } - realtime_dynamic_joint_state_publisher_->try_publish(dynamic_joint_state_msg_); - } - return controller_interface::return_type::OK; } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 2993dddceb..2dac7c613a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -3,7 +3,7 @@ joint_state_broadcaster: 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``." + description: "Defining if ``joint_states`` message should be published into local namespace, e.g., ``/my_state_broadcaster/joint_states``." } joints: { type: string_array, @@ -19,7 +19,7 @@ joint_state_broadcaster: 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." + description: "Names of extra joints to be added to ``joint_states`` with state set to 0." } interfaces: { type: string_array, @@ -60,9 +60,3 @@ joint_state_broadcaster: 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: false, - read_only: true, - description: "[Deprecated] Whether to publish dynamic joint states. This parameter is deprecated and will be removed in future releases." - } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 53c8c0bee0..f5f470a7f8 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -66,7 +66,8 @@ void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces, const std::vector & parameter_overrides) { - init_broadcaster_and_set_parameters("", joint_names, interfaces, parameter_overrides); + init_broadcaster_and_set_parameters( + kThreeJointURDF, joint_names, interfaces, parameter_overrides); assign_state_interfaces(joint_names, interfaces); } @@ -182,8 +183,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) 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); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -215,8 +218,10 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces 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); + // 3 URDF joints × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -244,8 +249,10 @@ TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfaces ASSERT_THAT(cmd_if_conf.names, IsEmpty()); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); 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); + // params are still cleared from configure (URDF mode), so still INDIVIDUAL_BEST_EFFORT + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -277,8 +284,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) 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); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -299,30 +308,8 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) 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); - - // publisher initialized - ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); - - // joint state initialized - const auto & joint_state_msg = state_broadcaster_->joint_state_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)); + // URDF mode (joints empty) with an unparsable robot description must fail at configure + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); } TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription) @@ -349,8 +336,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDes 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); + // URDF loaded: request specific joints with BEST_EFFORT (3 standard interfaces per joint) + ASSERT_THAT(state_if_conf.names, SizeIs(joint_in_urdf.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -386,8 +375,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) 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); + // URDF loaded: request specific joints with BEST_EFFORT (3 standard interfaces per joint) + ASSERT_THAT(state_if_conf.names, SizeIs(joint_in_urdf.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -454,8 +445,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) 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); + // 3 URDF joints (joint1, joint2, joint3) × 3 standard interfaces + ASSERT_THAT(state_if_conf.names, SizeIs(joint_names_.size() * 3)); + EXPECT_EQ( + state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL_BEST_EFFORT); // publisher initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -751,9 +744,25 @@ TEST_F(JointStateBroadcasterTest, UpdateTest) TEST_F(JointStateBroadcasterTest, UpdatePerformanceTest) { + // Generate a URDF with all 29 test joints (joint_1 ... joint_29) + std::string perf_urdf = + ""; + for (auto joint = 1u; joint < 30; ++joint) + { + const auto jname = "joint_" + std::to_string(joint); + const auto parent = joint == 1u ? "base_link" : "link_" + std::to_string(joint - 1); + const auto child = "link_" + std::to_string(joint); + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + perf_urdf += ""; + } + perf_urdf += ""; + controller_interface::ControllerInterfaceParams params; params.controller_name = "joint_state_broadcaster"; - params.robot_description = ""; + params.robot_description = perf_urdf; params.update_rate = 0; params.node_namespace = ""; params.node_options = state_broadcaster_->define_custom_node_options(); diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 4a4e9345e9..74f2ecfdb4 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -54,6 +54,40 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, CorrectMappingWhenInterfaceReadFailsTest); }; +// Minimal 3-joint URDF covering the joint_names_ used in tests +constexpr char * kThreeJointURDF = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + class JointStateBroadcasterTest : public ::testing::Test { public: