Allow Traffic Light APIs to update the location of a robot while it is idle (#270)

Author: mxgrey

rmf_fleet_adapter/CHANGELOG.rst

@@ -4,6 +4,7 @@ Changelog for package rmf_fleet_adapter
 
 1.3.0 (2021-XX-XX)
 ------------------
+* Allow Traffic Light APIs to update the location of a robot while it is idle: [#270](https://github.com/osrf/rmf_core/pull/270)
 * Allow TrafficLight and EasyTrafficLight API to update battery level: [#263](https://github.com/osrf/rmf_core/pull/263)
 * Migrating to a task dispatcher framework: [#217](https://github.com/osrf/rmf_core/pull/217)
     * The `rmf_fleet_adapter::agv` component interacts with a dispatcher node over topics with `rmf_task` prefix as specified in `rmf_fleet_adapter/StandardNames.hpp`

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyTrafficLight.hpp

@@ -30,6 +30,9 @@ class EasyTrafficLight : public std::enable_shared_from_this<EasyTrafficLight>
 public:
 
   /// Update the traffic light with a new path for your robot.
+  ///
+  /// \warning This function will throw an exception if there are less than 2
+  /// waypoints in the path.
   void follow_new_path(const std::vector<Waypoint>& new_path);
 
   /// This instruction is given for moving updates. It
@@ -124,6 +127,18 @@ class EasyTrafficLight : public std::enable_shared_from_this<EasyTrafficLight>
   /// Get the last checkpoint that the traffic light knows has been reached.
   std::size_t last_reached() const;
 
+  /// Update the location of the robot while it is idle. This means the robot is
+  /// sitting somewhere without the intention of going anywhere.
+  ///
+  /// \param[in] map_name
+  ///   The name of the reference map where the robot is located.
+  ///
+  /// \param[in] position
+  ///   The (x, y, yaw) coordinates of the robot.
+  EasyTrafficLight& update_idle_location(
+    std::string map_name,
+    Eigen::Vector3d position);
+
   /// Update the current battery level of the robot by specifying its state of
   /// charge as a fraction of its total charge capacity.
   EasyTrafficLight& update_battery_soc(double battery_soc);

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/TrafficLight.hpp

@@ -58,10 +58,25 @@ class TrafficLight
     /// This function should be called any time the robot changes the path that
     /// it intends to follow.
     ///
+    /// \warning This function will throw an exception if there are less than 2
+    /// waypoints in the path.
+    ///
     /// \param[in] new_path
     ///   Submit a new path that the robot intends to follow.
     std::size_t follow_new_path(const std::vector<Waypoint>& new_path);
 
+    /// Update the location of the robot while it is idle. Calling this function
+    /// will clear the last path that was given to follow_new_path.
+    ///
+    /// \param[in] map_name
+    ///   The name of the reference map where the robot is located.
+    ///
+    /// \param[in] position
+    ///   The (x, y, yaw) coordinates of the robot.
+    UpdateHandle& update_idle_location(
+      std::string map_name,
+      Eigen::Vector3d position);
+
     /// Update the current battery level of the robot by specifying its state of
     /// charge as a fraction of its total charge capacity.
     UpdateHandle& update_battery_soc(double battery_soc);

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp

@@ -230,7 +230,17 @@ void EasyTrafficLight::Implementation::deadlock(std::vector<Blocker> blockers)
 void EasyTrafficLight::Implementation::follow_new_path(
     const std::vector<Waypoint>& new_path)
 {
+  clear();
   current_path = new_path;
+  current_checkpoints.resize(new_path.size()-1);
+
+  current_version = update_handle->follow_new_path(new_path);
+}
+
+//==============================================================================
+void EasyTrafficLight::Implementation::clear()
+{
+  current_path.clear();
   last_received_checkpoints.reset();
   last_received_stop_info.reset();
   wait_until->reset();
@@ -239,11 +249,7 @@ void EasyTrafficLight::Implementation::follow_new_path(
   on_standby = nullptr;
   last_departed_checkpoint.reset();
   last_reached = 0;
-
   current_checkpoints.clear();
-  current_checkpoints.resize(new_path.size()-1);
-
-  current_version = update_handle->follow_new_path(new_path);
 }
 
 //==============================================================================
@@ -515,6 +521,13 @@ auto EasyTrafficLight::Implementation::waiting_after(
 //==============================================================================
 void EasyTrafficLight::follow_new_path(const std::vector<Waypoint>& new_path)
 {
+  if (new_path.size() < 2)
+  {
+    throw std::runtime_error(
+      "[EasyTrafficLight::follow_new_path] Invalid number of waypoints given ["
+      + std::to_string(new_path.size()) + "]. Must be at least 2.");
+  }
+
   auto lock = _pimpl->lock();
   _pimpl->follow_new_path(new_path);
 }
@@ -552,6 +565,19 @@ std::size_t EasyTrafficLight::last_reached() const
   return _pimpl->last_reached;
 }
 
+//==============================================================================
+EasyTrafficLight& EasyTrafficLight::update_idle_location(
+  std::string map_name,
+  Eigen::Vector3d position)
+{
+  auto lock = _pimpl->lock();
+  if (!_pimpl->current_path.empty())
+    _pimpl->clear();
+
+  _pimpl->update_handle->update_idle_location(std::move(map_name), position);
+  return *this;
+}
+
 //==============================================================================
 EasyTrafficLight& EasyTrafficLight::update_battery_soc(double battery_soc)
 {

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/TrafficLight.cpp

@@ -78,9 +78,15 @@ class TrafficLight::UpdateHandle::Implementation::Data
   std::vector<rmf_traffic::Route> plan_itinerary;
   std::vector<rmf_traffic::agv::Plan::Waypoint> pending_waypoints;
 
+  struct Location
+  {
+    std::string map;
+    Eigen::Vector3d position;
+  };
+
   // Remember which checkpoint we need to depart from next
   std::optional<std::size_t> last_departed_waypoint;
-  std::optional<Eigen::Vector3d> latest_location;
+  std::optional<Location> last_known_location;
   bool awaiting_confirmation = false;
   bool rejected = false;
   bool approved = false;
@@ -244,21 +250,21 @@ void TrafficLight::UpdateHandle::Implementation::Data::update_path(
   ready_check_timer = nullptr;
   waiting_timer = nullptr;
   last_departed_waypoint.reset();
-  latest_location.reset();
 
   if (new_path.empty())
   {
     // Clear out this robot's itinerary because it is done moving
     RCLCPP_INFO(
       node->get_logger(),
       "Traffic light controlled robot [%s] owned by [%s] is being taken off "
-      "the schedule after being given a null path.",
+      "the schedule because it is idle.",
       itinerary.description().name().c_str(),
       itinerary.description().owner().c_str());
 
     planner = nullptr;
     path.clear();
     departure_timing.clear();
+    blockade.cancel();
     return;
   }
   else if (new_path.size() == 1)
@@ -330,7 +336,11 @@ void TrafficLight::UpdateHandle::Implementation::Data::update_path(
         rmf_traffic::agv::Plan::Configuration(graph, traits),
         rmf_traffic::agv::Plan::Options(nullptr));
 
-  latest_location = new_path.front().position();
+  last_known_location = Location{
+    new_planner->get_configuration().graph().get_waypoint(0).get_map_name(),
+    new_path.front().position()
+  };
+
   rmf_traffic::agv::Plan::Start start{now, 0, new_path.front().position()[2]};
 
   auto approval_cb =
@@ -757,15 +767,15 @@ rmf_traffic::schedule::Negotiation::Alternatives make_reservation_alts(
 
   alts.emplace_back(std::move(preferred_itinerary));
 
-  if (data->latest_location.has_value())
+  if (data->last_known_location.has_value())
   {
     // TODO(MXG): Should we do something about the situation where we don't know
     // the latest location? Maybe just add a stop_and_sit on the first waypoint?
     rmf_traffic::Trajectory stop_and_sit;
     stop_and_sit.insert(
-          now, data->latest_location.value(), Eigen::Vector3d::Zero());
+      now, data->last_known_location->position, Eigen::Vector3d::Zero());
     stop_and_sit.insert(
-          now + 10s, data->latest_location.value(), Eigen::Vector3d::Zero());
+      now + 10s, data->last_known_location->position, Eigen::Vector3d::Zero());
 
     const auto& graph = data->planner->get_configuration().graph();
     const auto& wp = graph.get_waypoint(data->blockade.last_reached());
@@ -1124,7 +1134,11 @@ void TrafficLight::UpdateHandle::Implementation::Data::update_location(
       data->last_departed_waypoint = checkpoint_index;
     }
 
-    data->latest_location = location;
+    data->last_known_location = Location{
+        data->planner->get_configuration().graph()
+          .get_waypoint(checkpoint_index).get_map_name(),
+        location
+    };
     data->blockade.reached(checkpoint_index);
 
     if (plan_version != data->current_plan_version)
@@ -1651,7 +1665,11 @@ void TrafficLight::UpdateHandle::Implementation::Data::reject(
     // Even if the plan version is wrong, it may still be useful to update this
     // information.
     data->last_departed_waypoint = actual_last_departed;
-    data->latest_location = stopped_location;
+    data->last_known_location = Location{
+        data->planner->get_configuration().graph()
+          .get_waypoint(actual_last_departed).get_map_name(),
+        stopped_location
+    };
 
     if (plan_version != data->current_plan_version)
       return;
@@ -1674,13 +1692,13 @@ TrafficLight::UpdateHandle::Implementation::Data::current_location() const
 {
   const auto now = rmf_traffic_ros2::convert(node->now());
 
-  if (last_departed_waypoint.has_value() && latest_location.has_value())
+  if (last_departed_waypoint.has_value() && last_known_location.has_value())
   {
     const auto* lane = planner->get_configuration().graph().lane_from(
           *last_departed_waypoint, *last_departed_waypoint+1);
     assert(lane);
 
-    const Eigen::Vector3d p = *latest_location;
+    const Eigen::Vector3d p = last_known_location->position;
 
     return rmf_traffic::agv::Plan::Start(
           now,
@@ -1700,6 +1718,9 @@ TrafficLight::UpdateHandle::Implementation::Data::current_location() const
 void TrafficLight::UpdateHandle::Implementation::Data
 ::publish_fleet_state() const
 {
+  if (!last_known_location.has_value())
+    return;
+
   auto robot_mode = [&]()
   {
     if (current_range.begin == current_range.end)
@@ -1718,23 +1739,15 @@ ::publish_fleet_state() const
         .mode_request_id(0);
   }();
 
-  auto location = [&]() -> rmf_fleet_msgs::msg::Location
-  {
-    const auto& graph = planner->get_configuration().graph();
-    const auto& l = current_location();
-    const auto& wp = graph.get_waypoint(l.waypoint());
-    const Eigen::Vector2d p = l.location().value_or(wp.get_location());
-
-    return rmf_fleet_msgs::build<rmf_fleet_msgs::msg::Location>()
-      .t(rmf_traffic_ros2::convert(l.time()))
+  const auto& map = last_known_location->map;
+  const auto p = last_known_location->position;
+  auto location = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::Location>()
+      .t(node->now())
       .x(p.x())
       .y(p.y())
-      .yaw(l.orientation())
-      .level_name(wp.get_map_name())
-      // NOTE(MXG): This field is only used by the fleet drivers. For now, we
-      // will just fill it with a zero.
+      .yaw(p[2])
+      .level_name(map)
       .index(0);
-  }();
 
   const auto& fleet_name = itinerary.description().owner();
   auto robot_state = rmf_fleet_msgs::build<rmf_fleet_msgs::msg::RobotState>()
@@ -2005,6 +2018,13 @@ TrafficLight::UpdateHandle::Implementation::make(
 std::size_t TrafficLight::UpdateHandle::follow_new_path(
     const std::vector<Waypoint>& new_path)
 {
+  if (new_path.size() < 2)
+  {
+    throw std::runtime_error(
+      "[TrafficLight::follow_new_path] Invalid number of waypoints given ["
+      + std::to_string(new_path.size()) + "]. Must be at least 2.");
+  }
+
   const std::size_t version = ++_pimpl->received_version;
   _pimpl->data->worker.schedule(
         [version, new_path, data = _pimpl->data](const auto&)
@@ -2015,6 +2035,31 @@ std::size_t TrafficLight::UpdateHandle::follow_new_path(
   return version;
 }
 
+//==============================================================================
+auto TrafficLight::UpdateHandle::update_idle_location(
+  std::string map,
+  Eigen::Vector3d position) -> UpdateHandle&
+{
+  const std::size_t version = ++_pimpl->received_version;
+  _pimpl->data->worker.schedule(
+    [version, map = std::move(map), position, data = _pimpl->data](const auto&)
+  {
+    if (rmf_utils::modular(version).less_than(data->processing_version))
+      return;
+
+    data->last_known_location =
+      Implementation::Data::Location{
+        map,
+        position
+      };
+
+    if (!data->path.empty())
+      data->update_path(version, {});
+  });
+
+  return *this;
+}
+
 //==============================================================================
 auto TrafficLight::UpdateHandle::update_battery_soc(double battery_soc)
 -> UpdateHandle&

rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyTrafficLight.hpp

@@ -108,6 +108,8 @@ class EasyTrafficLight::Implementation
 
   void follow_new_path(const std::vector<Waypoint>& new_path);
 
+  void clear();
+
   void accept_new_checkpoints();
 
   std::optional<MovingInstruction> handle_new_checkpoints_moving(

rmf_traffic/include/rmf_traffic/blockade/Participant.hpp

@@ -61,6 +61,11 @@ class Participant
   /// checkpoint.
   void reached(CheckpointId checkpoint);
 
+  /// Cancel the current path entirely. Note that if a path is canceled while
+  /// the robot is in space that it needs to share with other robots, a
+  /// permanent deadlock could result.
+  void cancel();
+
   /// Get the last checkpoint that this participant said it has reached.
   CheckpointId last_reached() const;
 

rmf_traffic/src/rmf_traffic/blockade/Participant.cpp

@@ -233,6 +233,13 @@ void Participant::reached(CheckpointId checkpoint)
   _pimpl->_send_reached();
 }
 
+//==============================================================================
+void Participant::cancel()
+{
+  if (_pimpl->_reservation_id)
+    _pimpl->_writer->cancel(_pimpl->_id, *_pimpl->_reservation_id);
+}
+
 //==============================================================================
 CheckpointId Participant::last_reached() const
 {