[fix] prevent xarm_joint_plan service in xarm_planner returning true …

…when given dimension is OFF
xArm-Developer · Feb 3, 2024 · a048a17 · a048a17
1 parent 9e2e847
commit a048a17
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/xarm_planner/src/xarm_simple_planner.cpp b/xarm_planner/src/xarm_simple_planner.cpp
@@ -160,7 +160,11 @@ bool XArmSimplePlanner::do_single_cartesian_plan(xarm_planner::single_straight_p
 bool XArmSimplePlanner::do_joint_plan(xarm_planner::joint_plan::Request &req, xarm_planner::joint_plan::Response &res)
 {
   ROS_INFO("move_group_planner received new plan Request");
-  group.setJointValueTarget(req.target);
+  if(!group.setJointValueTarget(req.target))
+  {
+    ROS_ERROR("setJointValueTarget() Failed! Please check the dimension and range of given joint target.");
+    return false;
+  }
 
   bool success = (group.plan(my_xarm_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
   res.success = success;