[Servo] Halt Servo command on Pose Tracking stop

[AdamPettinger]

Description

Small change to make Pose Tracking publish a 0 twist message for Servo when it is stopping. Before this, if you were using a velocity controller and stopped Pose Tracking during a movement, the last published command to Servo would be nonzero and the manipulator would still move, see attached.

It might be worth exposing some kind of halt() function in Servo itself, which I can do here if needed.

Haven't been able to replicate with the Servo demonstrations (position controllers), so attached videos on hardware. In the clip, I use Pose Tracking to point the left arm camera at the right gripper, but call stopMotion() and cancel the pose tracking before it gets there. In the first, the arm holds the last velocity command sent until I E-stop. In the second gif, a 0-twist command is sent to Servo during Pose Tracking stopping.

[codecov]

Codecov Report

Merging #2501 (b3a0058) into master (0e59113) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master    #2501      +/-   ##
==========================================
- Coverage   60.26%   60.25%   -0.00%     
==========================================
  Files         351      351              
  Lines       26476    26483       +7     
==========================================
+ Hits        15952    15956       +4     
- Misses      10524    10527       +3     

Impacted Files	Coverage Δ
.../moveit_servo/include/moveit_servo/pose_tracking.h	100.00% <ø> (ø)
moveit_ros/moveit_servo/src/pose_tracking.cpp	44.10% <0.00%> (-2.00%)	⬇️
.../ompl_interface/src/detail/constrained_sampler.cpp	42.86% <0.00%> (-17.14%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	67.84% <0.00%> (-0.31%)	⬇️
...ipulation/pick_place/src/manipulation_pipeline.cpp	73.41% <0.00%> (+1.07%)	⬆️
...e/src/parameterization/model_based_state_space.cpp	72.80% <0.00%> (+2.40%)	⬆️
moveit_core/robot_model/src/joint_model_group.cpp	65.54% <0.00%> (+2.47%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 0e59113...b3a0058. Read the comment docs.

[AndyZe]

I appreciate the bug fix!

There's already a member function called stopMotion(). How about we move your zero-publication inside stopMotion() like this:

  void stopMotion()
  {
    stop_requested_ = true;
    
    // Send a 0 command to Servo to halt arm motion
    auto msg = moveit::util::make_shared_from_pool<geometry_msgs::TwistStamped>();
    {
      std::lock_guard<std::mutex> lock(target_pose_mtx_);
      msg->header.frame_id = target_pose_.header.frame_id;
    }
    msg->header.stamp = ros::Time::now();
    twist_stamped_pub_.publish(msg);
  }

Then doPostMotionReset() would always call stopMotion()?

void PoseTracking::doPostMotionReset()
{
  stopMotion();
  angular_error_ = boost::none;

  // Reset error integrals and previous errors of PID controllers
  cartesian_position_pids_[0].reset();
  cartesian_position_pids_[1].reset();
  cartesian_position_pids_[2].reset();
  cartesian_orientation_pids_[0].reset();

  stop_requested_ = false;
}

[AndyZe]

The only issue I see with ^ is it sets stop_requested_ = true; then stop_requested_ = false; right afterward. That's a little weird but I guess it won't cause any harm.

[AdamPettinger]

Looks good to me, will do in a second.

What do you think about removing the mutex-protected bit

{
  std::lock_guard<std::mutex> lock(target_pose_mtx_);
  msg->header.frame_id = target_pose_.header.frame_id;
}

As 0 velocity is 0 velocity in any frame and Servo can handle an empty frame command

[AndyZe]

I'd rather keep the mutex to be on the safe side, TBH.

[AdamPettinger]

I'd rather keep the mutex to be on the safe side, TBH.

Works for me. One thing I see about moving the halting message to stopMotion() is that an API call to stopMotion() will result in that function and the 0-twist message being published twice (in the call, and then triggered later in doPostMotionReset().

Is that a problem?

[AndyZe]

i'm OK with it

[henningkayser]

Is this really enough? It seems like this could go in after stop_requested_ so that it has no effect.

[AndyZe]

It looks fine to me. stop_requested_ stops the Pose Tracking loop. Publishing the zero twist stops the Servo loop.