Re-add moveit#497

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -152,12 +152,13 @@ class ServoCalcs
    * Is handled differently for position vs. velocity control.
    */
   void suddenHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const;
+  void suddenHalt(sensor_msgs::msg::JointState& joint_state) const;
 
   /** \brief  Scale the delta theta to match joint velocity/acceleration limits */
   void enforceVelLimits(Eigen::ArrayXd& delta_theta);
 
   /** \brief Avoid overshooting joint limits */
-  bool enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const;
+  bool enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const;
 
   /** \brief Possibly calculate a velocity scaling factor, due to proximity of
    * singularity and direction of motion

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -578,17 +578,17 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
   // Mark the lowpass filters as updated for this cycle
   updated_filters_ = true;
 
-  // compose outgoing message
-  composeJointTrajMessage(internal_joint_state_, joint_trajectory);
-
   // Enforce SRDF position limits, might halt if needed, set prev_vel to 0
-  if (!enforcePositionLimits(joint_trajectory))
+  if (!enforcePositionLimits(internal_joint_state_))
   {
-    suddenHalt(joint_trajectory);
+    suddenHalt(internal_joint_state_);
     status_ = StatusCode::JOINT_BOUND;
     prev_joint_velocity_.setZero();
   }
 
+  // compose outgoing message
+  composeJointTrajMessage(internal_joint_state_, joint_trajectory);
+
   // Modify the output message if we are using gazebo
   if (parameters_->use_gazebo)
   {
@@ -777,39 +777,38 @@ void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta)
   delta_theta = velocity_scaling_factor * velocity * parameters_->publish_period;
 }
 
-bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const
+bool ServoCalcs::enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) const
 {
   // Halt if we're past a joint margin and joint velocity is moving even farther past
   bool halting = false;
   double joint_angle = 0;
 
   for (auto joint : joint_model_group_->getActiveJointModels())
   {
-    for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c)
+    for (std::size_t c = 0; c < joint_state.name.size(); ++c)
     {
       // Use the most recent robot joint state
-      if (original_joint_state_.name[c] == joint->getName())
+      if (joint_state.name[c] == joint->getName())
       {
-        joint_angle = original_joint_state_.position.at(c);
+        joint_angle = joint_state.position.at(c);
         break;
       }
     }
 
-    if (!current_state_->satisfiesPositionBounds(joint, -parameters_->joint_limit_margin))
+    if (!joint->satisfiesPositionBounds(&joint_angle, -parameters_->joint_limit_margin))
     {
       const std::vector<moveit_msgs::msg::JointLimits> limits = joint->getVariableBoundsMsg();
 
       // Joint limits are not defined for some joints. Skip them.
       if (!limits.empty())
       {
         // Check if pending velocity command is moving in the right direction
-        auto joint_itr =
-            std::find(joint_trajectory.joint_names.begin(), joint_trajectory.joint_names.end(), joint->getName());
-        auto joint_idx = std::distance(joint_trajectory.joint_names.begin(), joint_itr);
+        auto joint_itr = std::find(joint_state.name.begin(), joint_state.name.end(), joint->getName());
+        auto joint_idx = std::distance(joint_state.name.begin(), joint_itr);
 
-        if ((joint_trajectory.points[0].velocities[joint_idx] < 0 &&
+        if ((joint_state.velocity.at(joint_idx) < 0 &&
              (joint_angle < (limits[0].min_position + parameters_->joint_limit_margin))) ||
-            (joint_trajectory.points[0].velocities[joint_idx] > 0 &&
+            (joint_state.velocity.at(joint_idx) > 0 &&
              (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin))))
         {
           rclcpp::Clock& clock = *node_->get_clock();
@@ -862,6 +861,13 @@ void ServoCalcs::suddenHalt(trajectory_msgs::msg::JointTrajectory& joint_traject
   }
 }
 
+void ServoCalcs::suddenHalt(sensor_msgs::msg::JointState& joint_state) const
+{
+  // Set the position to the original position, and velocity to 0
+  joint_state.position = original_joint_state_.position;
+  joint_state.velocity.assign(joint_state.position.size(), 0.0);
+}
+
 // Parse the incoming joint msg for the joints of our MoveGroup
 void ServoCalcs::updateJoints()
 {