[feature] Low latency mode (moveit#2401)

tylerjw · AndyZe · web-flow · commit a15af07ee83b · 2020-11-16T11:43:01.000-07:00
Co-authored-by: AndyZe &lt;andyz@utexas.edu&gt;
diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
@@ -175,6 +175,16 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_LIBRARIES}
   )
 
+  # low_latency
+  add_rostest_gtest(servo_low_latency_test
+    test/servo_low_latency_test.test
+    test/servo_low_latency_test.cpp
+  )
+  target_link_libraries(servo_low_latency_test
+    ${SERVO_LIB_NAME}
+    ${catkin_LIBRARIES}
+  )
+
   # pose_tracking
   add_rostest_gtest(pose_tracking_test
     test/pose_tracking_test.test
diff --git a/moveit_ros/moveit_servo/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml
@@ -15,6 +15,7 @@ low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the
 
 ## Properties of outgoing commands
 publish_period: 0.008  # 1/Nominal publish rate [seconds]
+low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
@@ -39,7 +39,9 @@
 #pragma once
 
 // C++
+#include <condition_variable>
 #include <mutex>
+#include <thread>
 
 // ROS
 #include <control_msgs/JointJog.h>
@@ -69,10 +71,7 @@ class ServoCalcs
   ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters,
              const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
 
-  ~ServoCalcs()
-  {
-    timer_.stop();
-  }
+  ~ServoCalcs();
 
   /** \brief Start the timer where we do work and publish outputs */
   void start();
@@ -106,8 +105,14 @@ class ServoCalcs
   void changeRobotLinkCommandFrame(const std::string& new_command_frame);
 
 private:
-  /** \brief Timer method */
-  void run(const ros::TimerEvent& timer_event);
+  /** \brief Run the main calculation loop */
+  void mainCalcLoop();
+
+  /** \brief Do calculations for a single iteration. Publish one outgoing command */
+  void calculateSingleIteration();
+
+  /** \brief Stop the currently running thread */
+  void stop();
 
   /** \brief Do servoing calculations for Cartesian twist commands. */
   bool cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory);
@@ -257,8 +262,6 @@ class ServoCalcs
   trajectory_msgs::JointTrajectoryConstPtr last_sent_command_;
 
   // ROS
-  ros::Timer timer_;
-  ros::Duration period_;
   ros::Subscriber joint_state_sub_;
   ros::Subscriber twist_stamped_sub_;
   ros::Subscriber joint_cmd_sub_;
@@ -270,6 +273,10 @@ class ServoCalcs
   ros::ServiceServer control_dimensions_server_;
   ros::ServiceServer reset_servo_status_;
 
+  // Main tracking / result publisher loop
+  std::thread thread_;
+  bool stop_requested_;
+
   // Status
   StatusCode status_ = StatusCode::NO_WARNING;
   bool paused_ = false;
@@ -292,8 +299,8 @@ class ServoCalcs
   // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw]
   std::array<bool, 6> control_dimensions_ = { { true, true, true, true, true, true } };
 
-  // latest_state_mutex_ is used to protect the state below it
-  mutable std::mutex latest_state_mutex_;
+  // input_mutex_ is used to protect the state below it
+  mutable std::mutex input_mutex_;
   Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_;
   Eigen::Isometry3d tf_moveit_to_ee_frame_;
   geometry_msgs::TwistStampedConstPtr latest_twist_stamped_;
@@ -302,5 +309,9 @@ class ServoCalcs
   ros::Time latest_joint_command_stamp_ = ros::Time(0.);
   bool latest_nonzero_twist_stamped_ = false;
   bool latest_nonzero_joint_cmd_ = false;
+
+  // input condition variable used for low latency mode
+  std::condition_variable input_cv_;
+  bool new_input_cmd_ = false;
 };
 }  // namespace moveit_servo
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
@@ -71,6 +71,7 @@ struct ServoParameters
   bool publish_joint_positions;
   bool publish_joint_velocities;
   bool publish_joint_accelerations;
+  bool low_latency_mode;
   // Collision checking
   bool check_collisions;
   std::string collision_check_type;
diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
@@ -136,7 +136,7 @@ bool Servo::readParameters()
   // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity
   // thresholds
   // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling
-  if (nh_.hasParam("collision_proximity_threshold") &&
+  if (nh.hasParam("collision_proximity_threshold") &&
       rosparam_shortcuts::get(LOGNAME, nh, "collision_proximity_threshold", collision_proximity_threshold))
   {
     ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate"
@@ -170,6 +170,17 @@ bool Servo::readParameters()
     error += !rosparam_shortcuts::get(LOGNAME, nh, "warning_topic", parameters_.status_topic);
   }
 
+  if (nh.hasParam("low_latency_mode"))
+  {
+    error += !rosparam_shortcuts::get(LOGNAME, nh, "low_latency_mode", parameters_.low_latency_mode);
+  }
+  else
+  {
+    ROS_WARN_NAMED(LOGNAME, "'low_latency_mode' is a new parameter that runs servo calc immediately after receiving "
+                            "input.  Setting to the default value of false.");
+    parameters_.low_latency_mode = false;
+  }
+
   rosparam_shortcuts::shutdownIfError(LOGNAME, error);
 
   // Input checking
diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -86,7 +86,7 @@ geometry_msgs::TransformStamped convertIsometryToTransform(const Eigen::Isometry
 // Constructor for the class that handles servoing calculations
 ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters,
                        const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
-  : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), period_(parameters.publish_period)
+  : nh_(nh), parameters_(parameters), planning_scene_monitor_(planning_scene_monitor), stop_requested_(true)
 {
   // MoveIt Setup
   current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
@@ -150,8 +150,16 @@ ServoCalcs::ServoCalcs(ros::NodeHandle& nh, ServoParameters& parameters,
   tf_moveit_to_robot_cmd_frame_ = empty_matrix;
 }
 
+ServoCalcs::~ServoCalcs()
+{
+  stop();
+}
+
 void ServoCalcs::start()
 {
+  // Stop the thread if we are currently running
+  stop();
+
   // We will update last_sent_command_ every time we start servo
   auto initial_joint_trajectory = moveit::util::make_shared_from_pool<trajectory_msgs::JointTrajectory>();
 
@@ -188,19 +196,78 @@ void ServoCalcs::start()
   tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *
                                   current_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame);
 
-  timer_ = nh_.createTimer(period_, &ServoCalcs::run, this);
+  stop_requested_ = false;
+  thread_ = std::thread([this] { mainCalcLoop(); });
+  new_input_cmd_ = false;
 }
 
-void ServoCalcs::run(const ros::TimerEvent& timer_event)
+void ServoCalcs::stop()
 {
-  // Log warning when the last loop duration was longer than the period
-  if (timer_event.profile.last_duration.toSec() > period_.toSec())
+  // Request stop
+  stop_requested_ = true;
+
+  // Notify condition variable in case the thread is blocked on it
   {
-    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
-                                   "last_duration: " << timer_event.profile.last_duration.toSec() << " ("
-                                                     << period_.toSec() << ")");
+    // scope so the mutex is unlocked after so the thread can continue
+    // and therefore be joinable
+    const std::lock_guard<std::mutex> lock(input_mutex_);
+    new_input_cmd_ = false;
+    input_cv_.notify_all();
+  }
+
+  // Join the thread
+  if (thread_.joinable())
+  {
+    thread_.join();
+  }
+}
+
+void ServoCalcs::mainCalcLoop()
+{
+  ros::Rate rate(1.0 / parameters_.publish_period);
+
+  while (ros::ok() && !stop_requested_)
+  {
+    // lock the input state mutex
+    std::unique_lock<std::mutex> input_lock(input_mutex_);
+
+    // low latency mode
+    if (parameters_.low_latency_mode)
+    {
+      input_cv_.wait(input_lock, [this] { return (new_input_cmd_ || stop_requested_); });
+
+      // break out of the loop if stop was requested
+      if (stop_requested_)
+        break;
+    }
+
+    // reset new_input_cmd_ flag
+    new_input_cmd_ = false;
+
+    // run servo calcs
+    const auto start_time = ros::Time::now();
+    calculateSingleIteration();
+    const auto run_duration = ros::Time::now() - start_time;
+
+    // Log warning when the run duration was longer than the period
+    if (run_duration.toSec() > parameters_.publish_period)
+    {
+      ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
+                                     "run_duration: " << run_duration.toSec() << " (" << parameters_.publish_period
+                                                      << ")");
+    }
+
+    // normal mode, unlock input mutex and wait for the period of the loop
+    if (!parameters_.low_latency_mode)
+    {
+      input_lock.unlock();
+      rate.sleep();
+    }
   }
+}
 
+void ServoCalcs::calculateSingleIteration()
+{
   // Publish status each loop iteration
   auto status_msg = moveit::util::make_shared_from_pool<std_msgs::Int8>();
   status_msg->data = static_cast<int8_t>(status_);
@@ -213,22 +280,20 @@ void ServoCalcs::run(const ros::TimerEvent& timer_event)
 
   // Update from latest state
   current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
-  {
-    const std::lock_guard<std::mutex> lock(latest_state_mutex_);
-    if (latest_twist_stamped_)
-      twist_stamped_cmd_ = *latest_twist_stamped_;
-    if (latest_joint_cmd_)
-      joint_servo_cmd_ = *latest_joint_cmd_;
-
-    // Check for stale cmds
-    twist_command_is_stale_ =
-        ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout));
-    joint_command_is_stale_ =
-        ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout));
-
-    have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_;
-    have_nonzero_joint_command_ = latest_nonzero_joint_cmd_;
-  }
+
+  if (latest_twist_stamped_)
+    twist_stamped_cmd_ = *latest_twist_stamped_;
+  if (latest_joint_cmd_)
+    joint_servo_cmd_ = *latest_joint_cmd_;
+
+  // Check for stale cmds
+  twist_command_is_stale_ =
+      ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout));
+  joint_command_is_stale_ =
+      ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout));
+
+  have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_;
+  have_nonzero_joint_command_ = latest_nonzero_joint_cmd_;
 
   // Get the transform from MoveIt planning frame to servoing command frame
   // Calculate this transform to ensure it is available via C++ API
@@ -980,7 +1045,7 @@ void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& del
 
 bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   transform = tf_moveit_to_robot_cmd_frame_;
 
   // All zeros means the transform wasn't initialized, so return false
@@ -989,7 +1054,7 @@ bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform)
 
 bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& transform)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   // All zeros means the transform wasn't initialized, so return false
   if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0))
   {
@@ -1003,7 +1068,7 @@ bool ServoCalcs::getCommandFrameTransform(geometry_msgs::TransformStamped& trans
 
 bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   transform = tf_moveit_to_ee_frame_;
 
   // All zeros means the transform wasn't initialized, so return false
@@ -1012,7 +1077,7 @@ bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform)
 
 bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   // All zeros means the transform wasn't initialized, so return false
   if (tf_moveit_to_ee_frame_.matrix().isZero(0))
   {
@@ -1025,22 +1090,30 @@ bool ServoCalcs::getEEFrameTransform(geometry_msgs::TransformStamped& transform)
 
 void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   latest_twist_stamped_ = msg;
   latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_);
 
   if (msg->header.stamp != ros::Time(0.))
     latest_twist_command_stamp_ = msg->header.stamp;
+
+  // notify that we have a new input
+  new_input_cmd_ = true;
+  input_cv_.notify_all();
 }
 
 void ServoCalcs::jointCmdCB(const control_msgs::JointJogConstPtr& msg)
 {
-  const std::lock_guard<std::mutex> lock(latest_state_mutex_);
+  const std::lock_guard<std::mutex> lock(input_mutex_);
   latest_joint_cmd_ = msg;
   latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_);
 
   if (msg->header.stamp != ros::Time(0.))
     latest_joint_command_stamp_ = msg->header.stamp;
+
+  // notify that we have a new input
+  new_input_cmd_ = true;
+  input_cv_.notify_all();
 }
 
 void ServoCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg)
diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml
@@ -14,6 +14,7 @@ scale:
 low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## Properties of outgoing commands
+low_latency_mode: false  # Set this to true to tie the output rate to the input rate
 publish_period: 0.01  # 1/Nominal publish rate [seconds]
 
 # What type of topic does your robot driver expect?
diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml
diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp b/moveit_ros/moveit_servo/test/servo_low_latency_test.cpp
diff --git a/moveit_ros/moveit_servo/test/servo_low_latency_test.test b/moveit_ros/moveit_servo/test/servo_low_latency_test.test
