Fix emergency stop (#82)

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Luca Della Vedova <luca@openrobotics.org>

rmf_robot_sim_common/CHANGELOG.rst

@@ -2,6 +2,10 @@
 Changelog for package rmf_robot_sim_common
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.0.0 (2022-XX-XX)
+------------------
+* Fix spurious emergency stop behavior when robot is moving backwards (`https://github.com/open-rmf/rmf_simulation/pull/82`)
+
 1.3.0 (2021-09-01)
 ------------------
 * support reversible slotcar (`#38 <https://github.com/open-rmf/rmf_simulation/issues/38>`_)

rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp

@@ -157,8 +157,6 @@ class SlotcarCommon
 
   rmf_fleet_msgs::msg::PauseRequest pause_request;
 
-  std::vector<rclcpp::Time> _hold_times;
-
   std::mutex _mutex;
 
   std::string _model_name;

rmf_robot_sim_common/src/slotcar_common.cpp

@@ -240,16 +240,19 @@ bool SlotcarCommon::path_request_valid(
   if (msg->task_id == _current_task_id)
   {
     RCLCPP_INFO(
-      logger(), "%s already received task [%s] -- continuing as normal",
-      _current_task_id.c_str(), _model_name.c_str());
+      logger(), "[%s] already received request [%s] -- continuing as normal",
+      _model_name.c_str(), _current_task_id.c_str());
     return false;
   }
 
   // Empty task request
   if (msg->path.size() == 0)
   {
-    RCLCPP_WARN(logger(), "%s received a path with no waypoints",
-      _model_name.c_str());
+    RCLCPP_WARN(
+      logger(),
+      "[%s] received a path [%s] with no waypoints",
+      _model_name.c_str(),
+      msg->task_id.c_str());
     return false;
   }
   return true;
@@ -266,12 +269,11 @@ void SlotcarCommon::path_request_cb(
 
   RCLCPP_DEBUG(
     logger(),
-    "%s received a path request with %d waypoints",
-    _model_name.c_str(), (int)msg->path.size());
+    "[%s] received a path request [%s] with %d waypoints",
+    _model_name.c_str(), msg->task_id.c_str(), (int)msg->path.size());
 
   // Reset this if we aren't at the final waypoint
   trajectory.resize(msg->path.size());
-  _hold_times.resize(msg->path.size());
   for (size_t i = 0; i < msg->path.size(); ++i)
   {
     Eigen::Vector3d v3(
@@ -294,7 +296,6 @@ void SlotcarCommon::path_request_cb(
       trajectory.at(i).approach_speed_limit = msg->path[i].approach_speed_limit;
     }
 
-    _hold_times.at(i) = msg->path[i].t;
   }
   _remaining_path = msg->path;
   _traj_wp_idx = 0;
@@ -311,9 +312,6 @@ void SlotcarCommon::path_request_cb(
     trajectory.clear();
     trajectory.push_back(_pose);
 
-    _hold_times.clear();
-    _hold_times.push_back(rclcpp::Time((int64_t)0, RCL_ROS_TIME));
-
     // We'll stick with the old path when an adapter error happens so that the
     // fleet adapter knows where the robot currently is along its previous path.
     _remaining_path = old_path;
@@ -323,7 +321,6 @@ void SlotcarCommon::path_request_cb(
   else
   {
     trajectory.erase(trajectory.begin());
-    _hold_times.erase(_hold_times.begin());
     _remaining_path.erase(_remaining_path.begin());
   }
 
@@ -527,21 +524,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
     const Eigen::Vector3d dpos = compute_dpos(
       trajectory.at(_traj_wp_idx).pose, _pose);
 
-    if (_hold_times.size() != trajectory.size())
-    {
-      throw std::runtime_error(
-              "Mismatch between trajectory size ["
-              + std::to_string(trajectory.size()) + "] and holding time size ["
-              + std::to_string(_hold_times.size()) + "]");
-    }
-
     auto dpos_mag = dpos.norm();
-    // TODO(MXG): Some kind of crazy nonsense bug is somehow altering the
-    // clock type value for the _hold_times. I don't know where this could
-    // possibly be happening, but I suspect it must be caused by undefined
-    // behavior. For now we deal with this by explicitly setting the clock type.
-    const auto hold_time =
-      rclcpp::Time(_hold_times.at(_traj_wp_idx), RCL_ROS_TIME);
 
     bool close_enough = (dpos_mag < 0.02);
     if (_was_rotating) // Accomodate slight drift when rotating on the spot
@@ -554,9 +537,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
       pause_request.type == pause_request.TYPE_PAUSE_IMMEDIATELY;
     const bool pause = checkpoint_pause || immediate_pause;
 
-    const bool hold = now < hold_time;
-
-    const bool rotate_towards_next_target = close_enough && (hold || pause);
+    const bool rotate_towards_next_target = close_enough && pause;
     _was_rotating = rotate_towards_next_target;
 
     if (rotate_towards_next_target)
@@ -642,9 +623,15 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
   const bool immediate_pause =
     pause_request.type == pause_request.TYPE_PAUSE_IMMEDIATELY;
 
-  const bool stop =
-    immediate_pause || emergency_stop(obstacle_positions, current_heading);
+  const bool e_stop = [&]() -> bool
+    {
+      if (result.target_linear_speed_now > 0.0)
+        return emergency_stop(obstacle_positions, current_heading);
+
+      return false;
+    } ();
 
+  const bool stop = immediate_pause || e_stop;
   if (immediate_pause)
   {
     // If we are required to immediately pause, report that we are in paused