Make planner better about changing floors

rmf_traffic/src/rmf_traffic/DetectConflict.cpp

@@ -39,14 +39,17 @@ class invalid_trajectory_error::Implementation
   std::string what;
 
   static invalid_trajectory_error make_segment_num_error(
-    std::size_t num_segments)
+    std::size_t num_segments,
+    std::size_t line,
+    std::string function)
   {
     invalid_trajectory_error error;
     error._pimpl->what = std::string()
       + "[rmf_traffic::invalid_trajectory_error] Attempted to check a "
       + "conflict with a Trajectory that has [" + std::to_string(num_segments)
       + "] segments. This is not supported. Trajectories must have at least "
-      + "2 segments to check them for conflicts.";
+      + "2 segments to check them for conflicts. "
+        + function + ":" + std::to_string(line);
     return error;
   }
 
@@ -710,12 +713,18 @@ rmf_utils::optional<rmf_traffic::Time> DetectConflict::Implementation::between(
   Interpolate /*interpolation*/,
   std::vector<Conflict>* output_conflicts)
 {
-  const std::size_t min_size =
-    std::min(trajectory_a.size(), trajectory_b.size());
-  if (min_size < 2)
+  if (trajectory_a.size() < 2)
   {
     throw invalid_trajectory_error::Implementation
-          ::make_segment_num_error(min_size);
+        ::make_segment_num_error(
+          trajectory_a.size(), __LINE__, __FUNCTION__);
+  }
+
+  if (trajectory_b.size() < 2)
+  {
+    throw invalid_trajectory_error::Implementation
+        ::make_segment_num_error(
+          trajectory_b.size(), __LINE__, __FUNCTION__);
   }
 
   const Profile::Implementation profile_a = convert_profile(input_profile_a);

rmf_traffic/src/rmf_traffic/agv/internal_planning.cpp

@@ -1448,26 +1448,54 @@ struct DifferentialDriveExpander
 
       if (const auto* event = lane.exit().event())
       {
-        if (!is_valid(route, initial_parent))
-          continue;
+        NodePtr parent_to_event;
+        if (route.trajectory.size() > 1)
+        {
+          if (!is_valid(route, initial_parent))
+            continue;
 
-        auto parent_to_event = std::make_shared<Node>(
-          Node{
-            _context.heuristic.estimate_remaining_cost(
-              _context, exit_waypoint_index),
-            compute_current_cost(initial_parent, trajectory),
-            exit_waypoint_index,
-            orientation,
-            std::move(route),
-            nullptr,
-            initial_parent
-          });
+          parent_to_event = std::make_shared<Node>(
+            Node{
+              _context.heuristic.estimate_remaining_cost(
+                _context, exit_waypoint_index),
+              compute_current_cost(initial_parent, trajectory),
+              exit_waypoint_index,
+              orientation,
+              std::move(route),
+              nullptr,
+              initial_parent
+            });
+        }
+        else
+        {
+          parent_to_event = initial_parent;
+        }
 
         event->execute(_executor.update(parent_to_event))
-        .add_if_valid(this, queue);
+          .add_if_valid(this, queue);
 
         continue;
       }
+      else if (route.trajectory.size() < 2)
+      {
+        // This should only happen when the map name has changed.
+        RouteData new_map_route;
+        new_map_route.map = exit_waypoint.get_map_name();
+        assert(new_map_route.map != map_name);
+
+        // FIXME TODO(MXG): This route generation is a hack that assumes that
+        // directly vertical lifts are the only things that connect two maps
+        // together. It also assumes that the parent route is simply a
+        // stationary route on the previous map.
+        //
+        // We should make map transitions more meaningful and require fewer
+        // assumptions.
+        new_map_route.trajectory = initial_parent->route_from_parent.trajectory;
+
+        add_if_valid(exit_waypoint_index, orientation, initial_parent,
+                     new_map_route, queue);
+        continue;
+      }
       // NOTE(MXG): We cannot move the trajectory in this function call, because
       // we may need to copy the trajectory later when we expand further down
       // other lanes.