Fix jog_arm segfault (moveit#1692)

Co-Authored-By: Robert Haschke <rhaschke@users.noreply.github.com>
PickNikRobotics · May 12, 2020 · 339a220 · 339a220
1 parent 8170ab1
commit 339a220
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp
@@ -568,15 +568,17 @@ void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_chan
 bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj)
 {
   bool halting = false;
+
   for (auto joint : joint_model_group_->getJointModels())
   {
     if (!kinematic_state_->satisfiesVelocityBounds(joint))
     {
       ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " "
                                                                            << " close to a "
                                                                               " velocity limit. Enforcing limit.");
+
       kinematic_state_->enforceVelocityBounds(joint);
-      for (std::size_t c = 0; c < num_joints_; ++c)
+      for (std::size_t c = 0; c < new_jt_traj.joint_names.size(); ++c)
       {
         if (new_jt_traj.joint_names[c] == joint->getName())
         {
@@ -588,15 +590,14 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n
 
     // Halt if we're past a joint margin and joint velocity is moving even farther past
     double joint_angle = 0;
-    for (std::size_t c = 0; c < num_joints_; ++c)
+    for (std::size_t c = 0; c < original_jt_state_.name.size(); ++c)
     {
       if (original_jt_state_.name[c] == joint->getName())
       {
         joint_angle = original_jt_state_.position.at(c);
         break;
       }
     }
-
     if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin))
     {
       const std::vector<moveit_msgs::JointLimits> limits = joint->getVariableBoundsMsg();