From 298bb425a46431c0de465c34e082927e784318d3 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Fri, 15 Apr 2022 18:20:04 +0900 Subject: [PATCH 1/3] [HrpsysSeqStateROSBridge.cpp] avoid segmentation fault when unkown joint name is given to /fullbody_controller/follow_joint_trajectory_action/goal --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index bebd13460..43f9682e3 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -188,7 +188,8 @@ 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]; } body->calcForwardKinematics(); From 713767989dc1b5eea6b40432a90e66e8c14e9a84 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 17 Apr 2022 17:41:57 +0900 Subject: [PATCH 2/3] [HrpsysSeqStateROSBridge.cpp] add warning message --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 43f9682e3..11fa347ee 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -190,6 +190,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory for (unsigned int j=0; j < trajectory.joint_names.size(); 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(); From ae1800b1f5bb14e5f78f65709103842436f67d24 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 17 Apr 2022 17:42:23 +0900 Subject: [PATCH 3/3] [HrpsysJointTrajectoryBridge.cpp] avoid segmentation fault when unkown joint name is given --- hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp index 1f28aa8c4..688b8d8a2 100644 --- 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();