From 7f8f7798adc7f5eddc50e7e38c9265ff7ed7aaf7 Mon Sep 17 00:00:00 2001 From: "youhei@jsk.imi.i.u-tokyo.ac.jp" Date: Thu, 29 Aug 2013 04:06:41 +0000 Subject: [PATCH] checking the existance all joints in required group on HrpsysJointTrajectoryBridge (ignore not existing joint) --- .../src/HrpsysJointTrajectoryBridge.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index f4544eb40..bc1e82467 100644 --- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp @@ -275,6 +275,19 @@ onJointTrajectory(trajectory_msgs::JointTrajectory trajectory) { duration.length(trajectory.points.size()) ; std::vector joint_names = trajectory.joint_names; + if (joint_names.size() < joint_list.size()) { + ROS_ERROR_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : " + << "required joint_names.size() = " << joint_names.size() + << " < joint_list.size() = " << joint_list.size() ); + return; + } + for (unsigned int i = 0; i < joint_list.size(); i++) { + if (count(joint_names.begin(), joint_names.end(), joint_list[i]) != 1) { + ROS_ERROR_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction / Error : " + << "joint : " << joint_list[i] << " did not exist in the required trajectory."); + return; + } + } ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : trajectory.points.size() " << trajectory.points.size()); for (unsigned int i = 0; i < trajectory.points.size(); i++) {