From ff7a07de01d738f874ce3a8d30beae9b8a368db7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christian=20J=C3=BClg?= Date: Fri, 10 Jun 2022 16:04:15 +0200 Subject: [PATCH] jointStateCB: compare joint names, reject updates with differing names or number of joints fixes https://github.com/ros-industrial/motoman/issues/326 based on https://github.com/gavanderhoorn/motoman/commit/e369ddc88e4a04adf847d9415ae94a0103ab9fd8 --- .../joint_trajectory_interface.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 69223609..2f5f086f 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp @@ -518,7 +518,6 @@ bool JointTrajectoryInterface::calc_velocity(const motoman_msgs::DynamicJointsGr return true; } - robot_groups_[pt.group_number].get_joint_names(); for (size_t i = 0; i < robot_groups_[pt.group_number].get_joint_names().size(); ++i) { const std::string &jnt_name = robot_groups_[pt.group_number].get_joint_names()[i]; @@ -727,12 +726,29 @@ bool JointTrajectoryInterface::is_valid(const motoman_msgs::DynamicJointTrajecto void JointTrajectoryInterface::jointStateCB( const sensor_msgs::JointStateConstPtr &msg) { + if (msg->name.size() != this->all_joint_names_.size()) + return; + + // compare all names, reject message if a name is not in both lists + for (size_t i=0; i < msg->name.size(); i++) + if (std::find(msg->name.begin(), msg->name.end(), this->all_joint_names_[i]) == msg->name.end()) + return; + this->cur_joint_pos_ = *msg; } void JointTrajectoryInterface::jointStateCB( const sensor_msgs::JointStateConstPtr &msg, int robot_id) { + if (msg->name.size() != this->robot_groups_[robot_id].get_joint_names().size()) + return; + + // compare all names, reject message if a name is not in both lists + for (size_t i=0; i < msg->name.size(); i++) + if (std::find(msg->name.begin(), msg->name.end(), this->robot_groups_[robot_id].get_joint_names()[i]) + == msg->name.end()) + return; + this->cur_joint_pos_map_[robot_id] = *msg; }