Tweaking API and adding test

Signed-off-by: Michael X. Grey <[email protected]>

rmf_task_sequence/include/rmf_task_sequence/events/GoToPlace.hpp

@@ -49,22 +49,23 @@ class GoToPlace::Description : public Event::Description
   /// Make a GoToPlace description using a goal.
   /// This constructor will pick the nearest unoccupied goal and
   /// head for said goal.
-  static DescriptionPtr make_with_multiple(std::vector<Goal>& goal);
-
-  /// Get the current goal for this description.
-  const bool has_multiple_possible_destinations() const;
+  static DescriptionPtr make_for_one_of(std::vector<Goal> goal);
 
   /// Get the possible destinations for this description.
-  const std::vector<Goal>& destinations() const;
+  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.
@@ -80,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,

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,
-    std::vector<Goal> goal);
+    const std::vector<Goal>& goal);
 
   // Documentation inherited
   std::optional<Estimate> estimate_finish(
@@ -82,7 +82,7 @@ class GoToPlace::Model : public Activity::Model
 Activity::ConstModelPtr GoToPlace::Model::make(
   State invariant_initial_state,
   const Parameters& parameters,
-  std::vector<Goal> goals)
+  const std::vector<Goal>& goals)
 {
   auto invariant_finish_state = invariant_initial_state;
 
@@ -199,8 +199,9 @@ GoToPlace::Model::Model(
 class GoToPlace::Description::Implementation
 {
 public:
-  std::vector<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;
 };
 
 //==============================================================================
@@ -214,7 +215,7 @@ auto GoToPlace::Description::make(Goal goal) -> DescriptionPtr
 }
 
 //==============================================================================
-auto GoToPlace::Description::make_with_multiple(std::vector<Goal>& goal)
+auto GoToPlace::Description::make_for_one_of(std::vector<Goal> goal)
 -> DescriptionPtr
 {
   auto desc = std::shared_ptr<Description>(new Description);
@@ -229,11 +230,28 @@ Activity::ConstModelPtr GoToPlace::Description::make_model(
   State invariant_initial_state,
   const Parameters& parameters) const
 {
-  // TODO(arjo) make invariant return farthest destination.
+  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);
 }
 
 //==============================================================================
@@ -257,7 +275,7 @@ Header GoToPlace::Description::generate_header(
       + "]");
   }
 
-  if (_pimpl->destination.size() == 0)
+  if (_pimpl->one_of.size() == 0)
   {
     utils::fail(fail_header, "No destination was specified");
   }
@@ -266,9 +284,9 @@ Header GoToPlace::Description::generate_header(
 
   std::optional<rmf_traffic::Duration> estimate = std::nullopt;
   std::size_t selected_index = 0;
-  for (std::size_t i = 0; i < _pimpl->destination.size(); i ++)
+  for (std::size_t i = 0; i < _pimpl->one_of.size(); i ++)
   {
-    auto dest = _pimpl->destination[i];
+    auto dest = _pimpl->one_of[i];
 
     if (graph.num_waypoints() <= dest.waypoint())
     {
@@ -296,28 +314,20 @@ Header GoToPlace::Description::generate_header(
     }
   }
 
+  if (!estimate.has_value())
+  {
+    utils::fail(fail_header, "Cannot find a path from ["
+      + 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());
     };
 
-  if (!estimate.has_value())
-  {
-    utils::fail(fail_header, "Cannot find a path from ["
-      + start_name + "] to any of [" + std::accumulate(
-        std::next(_pimpl->destination.begin()),
-        _pimpl->destination.end(),
-        goal_name(_pimpl->destination[0]),
-        [&](std::string a, const rmf_traffic::agv::Plan::Goal& goal)
-        {
-          return std::move(a) + " " + goal_name(goal);
-        }
-      ) + "]");
-  }
-
-  const auto goal_name_ = goal_name(_pimpl->destination[selected_index]);
+  const auto goal_name_ = goal_name(_pimpl->one_of[selected_index]);
 
   return Header(
     "Go to " + goal_name_,
@@ -328,37 +338,47 @@ Header GoToPlace::Description::generate_header(
 //==============================================================================
 auto GoToPlace::Description::destination() const -> const Goal&
 {
-  return _pimpl->destination[0];
-}
-
-//==============================================================================
-auto GoToPlace::Description::has_multiple_possible_destinations() const
--> const bool
-{
-  return _pimpl->destination.size() > 1;
+  return _pimpl->one_of.front();
 }
 
 //==============================================================================
-auto GoToPlace::Description::destinations() const -> const std::vector<Goal>&
+auto GoToPlace::Description::one_of() const -> const std::vector<Goal>&
 {
-  return _pimpl->destination;
+  return _pimpl->one_of;
 }
 
 
 //==============================================================================
 auto GoToPlace::Description::destination(Goal new_goal) -> Description&
 {
-  _pimpl->destination[0] = 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[0].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;
+    });
 }
 
 //==============================================================================
@@ -376,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()
 {

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);
+  }
+}

rmf_task_sequence/test/unit/utils.hpp

@@ -76,21 +76,34 @@ std::shared_ptr<rmf_task::Parameters> make_test_parameters(
   using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink;
   using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink;
 
-  const std::string test_map_name = "test_map";
+  const std::string L1 = "test_map_L1";
+  const std::string B1 = "test_map_B1";
   rmf_traffic::agv::Graph graph;
-  graph.add_waypoint(test_map_name, {-5, -5}).set_passthrough_point(true); // 0
-  graph.add_waypoint(test_map_name, { 0, -5}).set_passthrough_point(true); // 1
-  graph.add_waypoint(test_map_name, { 5, -5}).set_passthrough_point(true); // 2
-  graph.add_waypoint(test_map_name, {10, -5}).set_passthrough_point(true); // 3
-  graph.add_waypoint(test_map_name, {-5, 0}); // 4
-  graph.add_waypoint(test_map_name, { 0, 0}); // 5
-  graph.add_waypoint(test_map_name, { 5, 0}); // 6
-  graph.add_waypoint(test_map_name, {10, 0}).set_passthrough_point(true); // 7
-  graph.add_waypoint(test_map_name, {10, 4}).set_passthrough_point(true); // 8
-  graph.add_waypoint(test_map_name, { 0, 8}).set_passthrough_point(true); // 9
-  graph.add_waypoint(test_map_name, { 5, 8}).set_passthrough_point(true); // 10
-  graph.add_waypoint(test_map_name, {10, 12}).set_passthrough_point(true); // 11
-  graph.add_waypoint(test_map_name, {12, 12}).set_passthrough_point(true); // 12
+  /*
+  *
+  *               11--12
+  *              /
+  *      9---10-/
+  *      |    |    8
+  *      |    |    |
+  * 4----5    6    7
+  *      |         |
+  *      |         |
+  * 0----1----2----3
+  */
+  graph.add_waypoint(B1, {-5, -5}).set_passthrough_point(true); // 0
+  graph.add_waypoint(L1, { 0, -5}).set_passthrough_point(true); // 1
+  graph.add_waypoint(L1, { 5, -5}).set_passthrough_point(true); // 2
+  graph.add_waypoint(L1, {10, -5}).set_passthrough_point(true); // 3
+  graph.add_waypoint(L1, {-5, 0}); // 4
+  graph.add_waypoint(L1, { 0, 0}); // 5
+  graph.add_waypoint(L1, { 5, 0}); // 6
+  graph.add_waypoint(L1, {10, 0}).set_passthrough_point(true); // 7
+  graph.add_waypoint(L1, {10, 4}).set_passthrough_point(true); // 8
+  graph.add_waypoint(L1, { 0, 8}).set_passthrough_point(true); // 9
+  graph.add_waypoint(L1, { 5, 8}).set_passthrough_point(true); // 10
+  graph.add_waypoint(L1, {10, 12}).set_passthrough_point(true); // 11
+  graph.add_waypoint(L1, {12, 12}).set_passthrough_point(true); // 12
   REQUIRE(graph.num_waypoints() == 13);
 
   auto add_bidir_lane = [&](const std::size_t w0, const std::size_t w1)
@@ -107,8 +120,10 @@ std::shared_ptr<rmf_task::Parameters> make_test_parameters(
   add_bidir_lane(4, 5);
   add_bidir_lane(6, 10);
   add_bidir_lane(7, 8);
+  add_bidir_lane(5, 9);
   add_bidir_lane(9, 10);
   add_bidir_lane(10, 11);
+  add_bidir_lane(11, 12);
 
   const auto shape = rmf_traffic::geometry::make_final_convex<
     rmf_traffic::geometry::Circle>(1.0);