当前位置：首页 > news >正文

关于ros2_control中的joint_state_broadcaster，监听/joint_states,关节轨迹乱序问题。

news 2025/9/13 13:27:45

ros2_controllers.yaml配置文件中关于joint_state_broadcaster的部分：

joint_state_broadcaster:ros__parameters:joints:  # 按预期顺序排列- joint_1- joint_2- joint_3- joint_4- joint_5- joint_6interfaces:  # 修正参数名（原为 state_interfaces）- position- velocity- effortpublish_rate: 50.0use_urdf_to_filter: true

监听到的内容：

header:stamp:sec: 1757659337nanosec: 364778494frame_id: base_link
name:
- joint_1
- joint_2
- joint_4
- joint_5
- joint_3
- joint_6
position:
- -0.0003235740724449876
- -8.38895743378032e-05
- -0.0002636529479183025
- -0.0011504855909141298
- -0.00033555829735010256
- -0.001222390940346152
velocity:
- -25.0
- 95.0
- 152.0
- -53.0
- -191.0
- 127.0
effort:
- 10.0
- 62.0
- -20.0
- -67.0
- 118.0
- -44.0
---

可见关节顺序是：1 2 4 5 3 6,已经乱序。

源码中joint_state_broadcaster.cpp的bool JointStateBroadcaster::init_joint_data()函数：

bool JointStateBroadcaster::init_joint_data()
{joint_names_.clear();if (state_interfaces_.empty()){return false;}// loop in reverse order, this maintains the order of values at retrieval timefor (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++){// initialize map if name is newif (name_if_value_mapping_.count(si->get_prefix_name()) == 0){name_if_value_mapping_[si->get_prefix_name()] = {};}// add interface namestd::string interface_name = si->get_interface_name();if (map_interface_to_joint_state_.count(interface_name) > 0){interface_name = map_interface_to_joint_state_[interface_name];}name_if_value_mapping_[si->get_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 messagefor (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);}}// Add extra joints from parameters, each joint will be added to joint_names_ and// name_if_value_mapping_ if it is not already thererclcpp::Parameter extra_joints;if (get_node()->get_parameter("extra_joints", extra_joints)){const std::vector<std::string> & 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){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);}}}return true;
}

修改后是这样的：

bool JointStateBroadcaster::init_joint_data()
{joint_names_.clear();if (state_interfaces_.empty()){return false;}// 1. 先按硬件接口更新 name_if_value_mapping_（保留原逻辑）for (auto si = state_interfaces_.begin(); si != state_interfaces_.end(); si++)  // 正向迭代{std::string joint_name = si->get_prefix_name();std::string interface_name = si->get_interface_name();if (map_interface_to_joint_state_.count(interface_name) > 0){interface_name = map_interface_to_joint_state_[interface_name];}// 初始化或更新映射表name_if_value_mapping_[joint_name][interface_name] = kUninitializedValue;}// 2. 强制 joint_names_ 按配置的 params_.joints 顺序排列（关键修改）if (!params_.joints.empty())  // 若用户指定了 joints，则按其顺序{joint_names_ = params_.joints;}else  // 未指定则按硬件接口顺序（兼容默认情况）{for (const auto & name_ifv : name_if_value_mapping_){joint_names_.push_back(name_ifv.first);}}// 处理额外关节（保持原逻辑）rclcpp::Parameter extra_joints;if (get_node()->get_parameter("extra_joints", extra_joints)){const std::vector<std::string> & 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){name_if_value_mapping_[extra_joint_name] = {{HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}};// 若用户指定了 joints，额外关节加在末尾；否则按原逻辑if (!params_.joints.empty()){joint_names_.push_back(extra_joint_name);}}}}return true;
}

主要修改了 init_joint_data() 函数，核心是强制关节顺序与配置中的 joints 列表一致，具体有两处关键变更：

1. 遍历硬件接口的迭代方向修改

将原代码中反向迭代硬件接口（state_interfaces_）的逻辑，改为正向迭代，避免因反向遍历导致的顺序颠倒。

原代码：

// 反向迭代：从最后一个接口开始遍历，导致关节顺序逆序
for (auto si = state_interfaces_.crbegin(); si != state_interfaces_.crend(); si++)

修改后：

// 正向迭代：按硬件接口原生顺序遍历
for (auto si = state_interfaces_.begin(); si != state_interfaces_.end(); si++)

2. 关节名称列表（`joint_names_`）的赋值逻辑修改

原代码中 joint_names_ 是从硬件接口中自动收集的（依赖硬件顺序），修改后改为直接使用配置中 joints 列表的顺序，彻底摆脱对硬件接口顺序的依赖。

原代码（关节顺序依赖硬件接口）：

// 从硬件接口映射表中收集关节名，顺序由硬件决定
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);}
}

修改后（关节顺序强制与配置一致）：

// 若用户配置了 joints，则直接使用配置的顺序
if (!params_.joints.empty())  
{joint_names_ = params_.joints;  // 关键修改：强制使用配置顺序
}
else  // 未配置时仍兼容原逻辑（按硬件顺序）
{for (const auto & name_ifv : name_if_value_mapping_){joint_names_.push_back(name_ifv.first);}
}