diff --git a/joint_state_broadcaster/doc/userdoc.rst b/joint_state_broadcaster/doc/userdoc.rst index c7bf4fa9a1..996284d51a 100644 --- a/joint_state_broadcaster/doc/userdoc.rst +++ b/joint_state_broadcaster/doc/userdoc.rst @@ -38,3 +38,25 @@ An example parameter file .. generate_parameter_library_default:: ../src/joint_state_broadcaster_parameters.yaml + + +Order of the joints in the message +---------------------------------- + +The order of the joints in the message can determined by 3 different parameter settings: + +1. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``false``: + The order of the joints in the message is the same as the order of the joints' state interfaces registered in the resource manager. This is typically the order in which the hardware components are loaded and configured. + +2. No defined ``joints`` parameter and ``use_urdf_to_filter`` set to ``true``: + The order of the joints in the message is the same as the order of the joints in the URDF file, which is inherited from the loaded URDF model and independent of the order in the `ros2_control` tag. + +3. Defined ``joints`` parameter along with ``interfaces`` parameter: + The order of the joints in the message is the same as the order of the joints in the ``joints`` parameter. + + If the ``joints`` parameter is a subset of the total available joints in the URDF (or) the total available state interfaces, then only the joints in the ``joints`` parameter are published in the message. + + If any of the combinations of the defined ``joints`` parameter and ``interfaces`` parameter are not in the available state interfaces, the controller will fail to activate. + +..note:: + If the ``extra_joints`` parameter is set, the joints in the ``extra_joints`` parameter are appended to the end of the joint names in the message. diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5f4e92d857..2890617f50 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -170,6 +170,17 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); } + // joint_names reserve space for all joints + const auto max_joints_size = + (params_.joints.empty() ? model_.joints_.size() : params_.joints.size()) + + params_.extra_joints.size(); + joint_names_.reserve(max_joints_size); + auto & joint_state_msg = realtime_joint_state_publisher_->msg_; + joint_state_msg.name.reserve(max_joints_size); + joint_state_msg.position.reserve(max_joints_size); + joint_state_msg.velocity.reserve(max_joints_size); + joint_state_msg.effort.reserve(max_joints_size); + return CallbackReturn::SUCCESS; } @@ -199,23 +210,6 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( return CallbackReturn::SUCCESS; } -template -bool has_any_key( - const std::unordered_map & map, const std::vector & keys) -{ - bool found_key = false; - for (const auto & key_item : map) - { - const auto & key = key_item.first; - if (std::find(keys.cbegin(), keys.cend(), key) != keys.cend()) - { - found_key = true; - break; - } - } - return found_key; -} - bool JointStateBroadcaster::init_joint_data() { joint_names_.clear(); @@ -226,12 +220,15 @@ bool JointStateBroadcaster::init_joint_data() } // loop in reverse order, this maintains the order of values at retrieval time + const std::vector joint_state_interfaces = { + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT}; for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++) { + const std::string prefix_name = si->get_prefix_name(); // initialize map if name is new - if (name_if_value_mapping_.count(si->get_prefix_name()) == 0) + if (name_if_value_mapping_.count(prefix_name) == 0) { - name_if_value_mapping_[si->get_prefix_name()] = {}; + name_if_value_mapping_[prefix_name] = {}; } // add interface name std::string interface_name = si->get_interface_name(); @@ -239,39 +236,51 @@ bool JointStateBroadcaster::init_joint_data() { interface_name = map_interface_to_joint_state_[interface_name]; } - name_if_value_mapping_[si->get_prefix_name()][interface_name] = kUninitializedValue; - } + name_if_value_mapping_[prefix_name][interface_name] = kUninitializedValue; - // 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})) + // filter state interfaces that have at least one of the joint_states fields, + // the rest will be ignored for this message + if ( + std::find(joint_state_interfaces.begin(), joint_state_interfaces.end(), interface_name) != + joint_state_interfaces.end()) { if ( !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || - model_.getJoint(name_ifv.first)) + model_.getJoint(prefix_name)) { - joint_names_.push_back(name_ifv.first); + if (std::find(joint_names_.begin(), joint_names_.end(), prefix_name) == joint_names_.end()) + { + joint_names_.push_back(prefix_name); + } } } } + 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()) + { + joint_names_filtered.push_back(joint_name); + } + } + } + joint_names_ = joint_names_filtered; + } // Add extra joints from parameters, each joint will be added to joint_names_ and // name_if_value_mapping_ if it is not already there - rclcpp::Parameter extra_joints; - if (get_node()->get_parameter("extra_joints", extra_joints)) + for (const auto & extra_joint_name : params_.extra_joints) { - const std::vector & extra_joints_names = extra_joints.as_string_array(); - for (const auto & extra_joint_name : extra_joints_names) + if (name_if_value_mapping_.count(extra_joint_name) == 0) { - if (name_if_value_mapping_.count(extra_joint_name) == 0) - { - name_if_value_mapping_[extra_joint_name] = { - {HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; - joint_names_.push_back(extra_joint_name); - } + name_if_value_mapping_[extra_joint_name] = { + {HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}}; + joint_names_.push_back(extra_joint_name); } }