Merge pull request #1121 from Naoki-Hiraoka/PR-jointname

[HrpsysSeqStateROSBridge.cpp] avoid segmentation fault when unkown joint name is given to /fullbody_controller/follow_joint_trajectory_action/goal
start-jsk · Jun 9, 2022 · 07f7e19 · 07f7e19
2 parents ee21bf5 + ae1800b
commit 07f7e19
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 2 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -457,7 +457,9 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
     for (unsigned int j = 0; j < joint_names.size(); j++)
     {
-      parent->body->link(joint_names[j])->q = point.positions[j];
+      hrp::Link* l = parent->body->link(joint_names[j]);
+      if(l) l->q = point.positions[j];
+      else ROS_WARN_STREAM_ONCE("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : joint named " << joint_names[j] << " is not found. Skipping ...");
     }
 
     parent->body->calcForwardKinematics();

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -188,7 +188,9 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
 
     trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i];
     for (unsigned int j=0; j < trajectory.joint_names.size(); j++ ) {
-      body->link(joint_names[j])->q = point.positions[j];
+      hrp::Link* l = body->link(joint_names[j]);
+      if(l) l->q = point.positions[j];
+      else ROS_WARN_STREAM_ONCE("[" << getInstanceName() << "] @onJointTrajectoryAction : joint named " << joint_names[j] << " is not found. Skipping ...");
     }
 
     body->calcForwardKinematics();