Add support for multiple destinations to choose from. (#101)

Signed-off-by: Arjo Chakravarty <[email protected]> Signed-off-by: Michael X. Grey <[email protected]> Co-authored-by: Michael X. Grey <[email protected]>
open-rmf · Dec 21, 2023 · a925af3 · a925af3
1 parent 8d91b71
commit a925af3
Show file tree

Hide file tree

Showing 4 changed files with 261 additions and 51 deletions.
diff --git a/rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp b/rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp
@@ -46,14 +46,26 @@ class GoToPlace::Description : public Event::Description
   /// Make a GoToPlace description using a goal.
   static DescriptionPtr make(Goal goal);
 
+  /// Make a GoToPlace description using a goal.
+  /// This constructor will pick the nearest unoccupied goal and
+  /// head for said goal.
+  static DescriptionPtr make_for_one_of(std::vector<Goal> goal);
+
+  /// Get the possible destinations for this description.
+  const std::vector<Goal>& one_of() const;
+
   /// Get the current goal for this description.
+  [[deprecated("Use one_of() instead. Always assume that multiple destinations are possible.")]]
   const Goal& destination() const;
 
-  /// Set the current goal for this description.
+  /// Set a single goal for this description.
   Description& destination(Goal new_goal);
 
   /// Get the name of the goal. If the goal does not have an explicit name, it
   /// will be referred to as "#x" where "x" is the index number of the waypoint.
+  ///
+  /// If there are multiple goal options, this will be a ` | `-separated list of
+  /// destinations.
   std::string destination_name(const rmf_task::Parameters& parameters) const;
 
   /// Get the expected future destinations that may come after this one.
@@ -69,6 +81,15 @@ class GoToPlace::Description : public Event::Description
   /// be at its destination.
   Description& expected_next_destinations(std::vector<Goal> value);
 
+  /// Check whether a destination on the same map as the robot's initial
+  /// location should always be preferred over any other destinations. If there
+  /// is no destination on the same map then closest will be chosen.
+  bool prefer_same_map() const;
+
+  /// Specify whether or not a destination on the same map as the robot's
+  /// initial location should always be preferred over any other destinations.
+  Description& prefer_same_map(bool choice);
+
   // Documentation inherited
   Activity::ConstModelPtr make_model(
     State invariant_initial_state,

diff --git a/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp b/rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp
@@ -51,7 +51,7 @@ class GoToPlace::Model : public Activity::Model
   static Activity::ConstModelPtr make(
     State invariant_initial_state,
     const Parameters& parameters,
-    Goal goal);
+    const std::vector<Goal>& goal);
 
   // Documentation inherited
   std::optional<Estimate> estimate_finish(
@@ -82,35 +82,49 @@ class GoToPlace::Model : public Activity::Model
 Activity::ConstModelPtr GoToPlace::Model::make(
   State invariant_initial_state,
   const Parameters& parameters,
-  Goal goal)
+  const std::vector<Goal>& goals)
 {
   auto invariant_finish_state = invariant_initial_state;
-  invariant_finish_state.waypoint(goal.waypoint());
-
-  if (goal.orientation())
-    invariant_finish_state.orientation(*goal.orientation());
-  else
-    invariant_finish_state.erase<State::CurrentOrientation>();
 
-  auto invariant_duration = rmf_traffic::Duration(0);
+  auto selected_goal = goals[0];
+  std::optional<rmf_traffic::Duration> shortest_travel_time = std::nullopt;
   if (invariant_initial_state.waypoint().has_value())
   {
-    const auto invariant_duration_opt = estimate_duration(
-      parameters.planner(),
-      invariant_initial_state,
-      goal);
+    for (auto goal: goals)
+    {
+      const auto invariant_duration_opt = estimate_duration(
+        parameters.planner(),
+        invariant_initial_state,
+        goal);
+
+      if (!shortest_travel_time.has_value())
+      {
+        shortest_travel_time = invariant_duration_opt;
+        selected_goal = goal;
+      }
+      else if (shortest_travel_time.value() > invariant_duration_opt.value())
+      {
+        shortest_travel_time = invariant_duration_opt;
+        selected_goal = goal;
+      }
+    }
 
-    if (!invariant_duration_opt.has_value())
+    if (!shortest_travel_time.has_value())
       return nullptr;
-
-    invariant_duration = *invariant_duration_opt;
   }
 
+  invariant_finish_state.waypoint(selected_goal.waypoint());
+
+  if (selected_goal.orientation())
+    invariant_finish_state.orientation(*selected_goal.orientation());
+  else
+    invariant_finish_state.erase<State::CurrentOrientation>();
+
   return std::shared_ptr<Model>(
     new Model(
       std::move(invariant_finish_state),
-      invariant_duration,
-      std::move(goal)));
+      shortest_travel_time.value_or(rmf_traffic::Duration(0)),
+      std::move(selected_goal)));
 }
 
 //==============================================================================
@@ -185,13 +199,24 @@ GoToPlace::Model::Model(
 class GoToPlace::Description::Implementation
 {
 public:
-
-  rmf_traffic::agv::Plan::Goal destination;
+  std::vector<rmf_traffic::agv::Plan::Goal> one_of;
   std::vector<rmf_traffic::agv::Plan::Goal> expected_next_destinations;
+  bool prefer_same_map = false;
 };
 
 //==============================================================================
 auto GoToPlace::Description::make(Goal goal) -> DescriptionPtr
+{
+  auto desc = std::shared_ptr<Description>(new Description);
+  desc->_pimpl = rmf_utils::make_impl<Implementation>(
+    Implementation{{std::move(goal)}, {}});
+
+  return desc;
+}
+
+//==============================================================================
+auto GoToPlace::Description::make_for_one_of(std::vector<Goal> goal)
+-> DescriptionPtr
 {
   auto desc = std::shared_ptr<Description>(new Description);
   desc->_pimpl = rmf_utils::make_impl<Implementation>(
@@ -205,10 +230,28 @@ Activity::ConstModelPtr GoToPlace::Description::make_model(
   State invariant_initial_state,
   const Parameters& parameters) const
 {
+  if (_pimpl->prefer_same_map && invariant_initial_state.waypoint().has_value())
+  {
+    const std::size_t wp = invariant_initial_state.waypoint().value();
+    const auto& graph = parameters.planner()->get_configuration().graph();
+    const auto& map = graph.get_waypoint(wp).get_map_name();
+    std::vector<Goal> goals;
+    for (const auto& g : _pimpl->one_of)
+    {
+      const auto& goal_map = graph.get_waypoint(g.waypoint()).get_map_name();
+      if (goal_map == map)
+        goals.push_back(g);
+    }
+
+    const auto model = Model::make(invariant_initial_state, parameters, goals);
+    if (model)
+      return model;
+  }
+
   return Model::make(
     std::move(invariant_initial_state),
     parameters,
-    _pimpl->destination);
+    _pimpl->one_of);
 }
 
 //==============================================================================
@@ -232,27 +275,60 @@ Header GoToPlace::Description::generate_header(
       + "]");
   }
 
+  if (_pimpl->one_of.size() == 0)
+  {
+    utils::fail(fail_header, "No destination was specified");
+  }
+
   const auto start_name = rmf_task::standard_waypoint_name(graph, start_wp);
 
-  if (graph.num_waypoints() <= _pimpl->destination.waypoint())
+  std::optional<rmf_traffic::Duration> estimate = std::nullopt;
+  std::size_t selected_index = 0;
+  for (std::size_t i = 0; i < _pimpl->one_of.size(); i ++)
   {
-    utils::fail(fail_header, "Destination waypoint ["
-      + std::to_string(_pimpl->destination.waypoint())
-      + "] is outside the graph [" + std::to_string(graph.num_waypoints())
-      + "]");
-  }
+    auto dest = _pimpl->one_of[i];
 
-  const auto goal_name_ = destination_name(parameters);
+    if (graph.num_waypoints() <= dest.waypoint())
+    {
+      utils::fail(fail_header, "Destination waypoint ["
+        + std::to_string(dest.waypoint())
+        + "] is outside the graph [" + std::to_string(graph.num_waypoints())
+        + "]");
+    }
 
-  const auto estimate = estimate_duration(
-    parameters.planner(), initial_state, _pimpl->destination);
+    if (estimate.has_value())
+    {
+      auto curr_est = estimate_duration(
+        parameters.planner(), initial_state, dest);
+      if (curr_est.has_value() && curr_est.value() < estimate)
+      {
+        estimate = curr_est;
+        selected_index = i;
+      }
+    }
+    else
+    {
+      estimate = estimate_duration(
+        parameters.planner(), initial_state, dest);
+      selected_index = i;
+    }
+  }
 
   if (!estimate.has_value())
   {
     utils::fail(fail_header, "Cannot find a path from ["
-      + start_name + "] to [" + goal_name_ + "]");
+      + start_name + "] to any of [" + destination_name(parameters) + "]");
   }
 
+  auto goal_name = [&](const rmf_traffic::agv::Plan::Goal& goal)
+    {
+      return rmf_task::standard_waypoint_name(
+        parameters.planner()->get_configuration().graph(),
+        goal.waypoint());
+    };
+
+  const auto goal_name_ = goal_name(_pimpl->one_of[selected_index]);
+
   return Header(
     "Go to " + goal_name_,
     "Moving the robot from " + start_name + " to " + goal_name_,
@@ -262,23 +338,47 @@ Header GoToPlace::Description::generate_header(
 //==============================================================================
 auto GoToPlace::Description::destination() const -> const Goal&
 {
-  return _pimpl->destination;
+  return _pimpl->one_of.front();
+}
+
+//==============================================================================
+auto GoToPlace::Description::one_of() const -> const std::vector<Goal>&
+{
+  return _pimpl->one_of;
 }
 
+
 //==============================================================================
 auto GoToPlace::Description::destination(Goal new_goal) -> Description&
 {
-  _pimpl->destination = std::move(new_goal);
+  _pimpl->one_of.resize(1, new_goal);
   return *this;
 }
 
 //==============================================================================
 std::string GoToPlace::Description::destination_name(
   const Parameters& parameters) const
 {
-  return rmf_task::standard_waypoint_name(
-    parameters.planner()->get_configuration().graph(),
-    _pimpl->destination.waypoint());
+  if (_pimpl->one_of.empty())
+    return "<none>";
+
+  auto goal_name = [&](const rmf_traffic::agv::Plan::Goal& goal)
+    {
+      return rmf_task::standard_waypoint_name(
+        parameters.planner()->get_configuration().graph(),
+        goal.waypoint());
+    };
+
+  return std::accumulate(
+    std::next(_pimpl->one_of.begin()),
+    _pimpl->one_of.end(),
+    goal_name(_pimpl->one_of.front()),
+    [&](std::string a, const rmf_traffic::agv::Plan::Goal& goal)
+    {
+      a += " | ";
+      a += goal_name(goal);
+      return a;
+    });
 }
 
 //==============================================================================
@@ -296,6 +396,19 @@ auto GoToPlace::Description::expected_next_destinations(std::vector<Goal> value)
   return *this;
 }
 
+//==============================================================================
+bool GoToPlace::Description::prefer_same_map() const
+{
+  return _pimpl->prefer_same_map;
+}
+
+//==============================================================================
+auto GoToPlace::Description::prefer_same_map(bool choice) -> Description&
+{
+  _pimpl->prefer_same_map = choice;
+  return *this;
+}
+
 //==============================================================================
 GoToPlace::Description::Description()
 {

diff --git a/rmf_task_sequence/test/unit/events/test_GoToPlace.cpp b/rmf_task_sequence/test/unit/events/test_GoToPlace.cpp
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <rmf_utils/catch.hpp>
+
+#include <rmf_task_sequence/events/GoToPlace.hpp>
+
+#include "../utils.hpp"
+
+SCENARIO("Test GoToPlace")
+{
+  using GoToPlace = rmf_task_sequence::events::GoToPlace;
+
+  auto description = GoToPlace::Description::make_for_one_of({0, 8, 12});
+  const auto parameters = make_test_parameters();
+  const auto constraints = make_test_constraints();
+  const auto now = std::chrono::steady_clock::now();
+  rmf_task::State initial_state;
+  initial_state.waypoint(1)
+  .orientation(0.0)
+  .time(now)
+  .dedicated_charging_waypoint(0)
+  .battery_soc(1.0);
+
+  const auto travel_estimator = rmf_task::TravelEstimator(*parameters);
+
+  WHEN("Not constrained to any map")
+  {
+    const auto model = description->make_model(initial_state, *parameters);
+    const auto finish = model->estimate_finish(
+      initial_state, now, *constraints, travel_estimator);
+
+    REQUIRE(finish.has_value());
+    CHECK(finish->finish_state().waypoint() == 0);
+  }
+
+  WHEN("Constrained to the same map")
+  {
+    description->prefer_same_map(true);
+    const auto model = description->make_model(initial_state, *parameters);
+    const auto finish = model->estimate_finish(
+      initial_state, now, *constraints, travel_estimator);
+
+    REQUIRE(finish.has_value());
+    CHECK(finish->finish_state().waypoint() == 8);
+  }
+}