Add debugging log statement for a common error (#2509)

add debugging informative log statement for a common error Co-authored-by: Peter Mitrano <pmitrano@armstorm>
moveit · May 3, 2021 · e3beb96 · e3beb96
1 parent 8cd6d7b
commit e3beb96
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -1612,8 +1612,14 @@ bool moveit::planning_interface::MoveGroupInterface::setNamedTarget(const std::s
 
 bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget(const std::vector<double>& joint_values)
 {
-  if (joint_values.size() != impl_->getJointModelGroup()->getVariableCount())
+  auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount();
+  if (joint_values.size() != n_group_joints)
+  {
+    ROS_DEBUG_STREAM_NAMED(LOGNAME, "Provided joint value list has length " << joint_values.size() << " but group "
+                                                                            << impl_->getJointModelGroup()->getName()
+                                                                            << " has " << n_group_joints << " joints");
     return false;
+  }
   impl_->setTargetType(JOINT);
   impl_->getJointStateTarget().setJointGroupPositions(impl_->getJointModelGroup(), joint_values);
   return impl_->getJointStateTarget().satisfiesBounds(impl_->getJointModelGroup(), impl_->getGoalJointTolerance());