Introducing new graph elements and properties (#103)

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

.github/workflows/asan.yaml

@@ -7,21 +7,24 @@ on:
 jobs:
   asan:
     name: asan
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
+    container:
+      image: osrf/ros:iron-desktop-jammy
     steps:
       - name: create_blacklist
-        run: echo "fun:*Eigen*" > /home/runner/work/blacklist.txt
-      - name: deps
-        uses: ros-tooling/[email protected]
-        with:
-          required-ros-distributions: foxy
+        run: |
+          mkdir -p ${{ github.workspace }}/
+          touch ${{ github.workspace }}/blacklist.txt
+          echo "fun:*Eigen*" > ${{ github.workspace }}/blacklist.txt
+      - name: install_clang_and_tools
+        run: sudo apt update && sudo apt install -y clang clang-tools lld wget python3-pip python3-colcon-coveragepy-result python3-colcon-lcov-result lcov
       - name: build_and_test
-        uses: ros-tooling/action-ros-ci@v0.2
+        uses: ros-tooling/action-ros-ci@0.3.5
         env:
-          CC: clang -fsanitize-blacklist=/home/runner/work/blacklist.txt
-          CXX: clang++ -fsanitize-blacklist=/home/runner/work/blacklist.txt
+          CC: clang -fsanitize-blacklist=${{ github.workspace }}/blacklist.txt
+          CXX: clang++ -fsanitize-blacklist=${{ github.workspace }}/blacklist.txt
         with:
-          target-ros2-distro: foxy
+          target-ros2-distro: iron
           # build all packages listed in the meta package
           package-name: |
             rmf_traffic

.github/workflows/build.yaml

@@ -12,28 +12,33 @@ jobs:
     strategy:
       matrix:
         ros_distribution:
-          - galactic
+          - humble
+          - iron
           - rolling
         include:
-          # Galactic Geochelone (May 2021 - November 2022)
-          - docker_image: ubuntu:focal
-            ros_distribution: galactic
+          # Humble Hawksbill (May 2022 - May 2027)
+          - ubuntu_distribution: jammy
+            ros_distribution: humble
+            ros_version: 2
+          # Iron Irwini (May 2023 - November 2024)
+          - ubuntu_distribution: jammy
+            ros_distribution: iron
             ros_version: 2
           # Rolling Ridley (No End-Of-Life)
-          - docker_image: ubuntu:jammy
+          - ubuntu_distribution: jammy
             ros_distribution: rolling
             ros_version: 2
     container:
-      image: ${{ matrix.docker_image }}
+      image: osrf/ros:${{ matrix.ros_distribution }}-desktop-${{ matrix.ubuntu_distribution }}
     steps:
       - name: pwd
         run: pwd
       - name: setup ROS environment
-        uses: ros-tooling/setup-ros@v0.3
+        uses: ros-tooling/setup-ros@0.7.1
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
       - name: build
-        uses: ros-tooling/action-ros-ci@v0.2
+        uses: ros-tooling/action-ros-ci@0.3.5
         with:
           target-ros2-distro: ${{ matrix.ros_distribution }}
           # build all packages listed in the meta package

.github/workflows/tsan.yaml

@@ -7,20 +7,20 @@ on:
 jobs:
   tsan:
     name: tsan
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
+    container:
+      image: osrf/ros:iron-desktop-jammy
     steps:
-      - name: deps
-        uses: ros-tooling/[email protected]
-        with:
-          required-ros-distributions: foxy
+      - name: install_clang_and_tools
+        run: sudo apt update && sudo apt install -y clang clang-tools lld wget python3-pip python3-colcon-coveragepy-result python3-colcon-lcov-result lcov
       - name: tsan_build_test
-        uses: ros-tooling/[email protected]
-        id: tsan_build_test
         env:
           CC: clang
           CXX: clang++
+        uses: ros-tooling/[email protected]
+        id: tsan_build_test
         with:
-          target-ros2-distro: foxy
+          target-ros2-distro: iron
           # build all packages listed in the meta package
           package-name: |
             rmf_traffic

rmf_traffic/QUALITY_DECLARATION.md

@@ -158,7 +158,7 @@ This is assumed to be **Quality Level 4** due to its provided feature documentat
 ### Target platforms [6.i]
 
 `rmf_traffic` does not support all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers).
-`rmf_traffic` supports ROS Foxy.
+`rmf_traffic` supports ROS Iron.
 
 ## Security [7]
 

rmf_traffic/include/rmf_traffic/Trajectory.hpp

@@ -367,6 +367,13 @@ class Trajectory::base_iterator
   /// \return a copy of the iterator before it was decremented
   base_iterator operator--(int);
 
+  /// Get the iterator that would be offset from this one by the specified
+  /// amount
+  base_iterator operator+(int offset) const;
+
+  /// Get the iterator that would be offset from this one by the specified
+  /// amount in the opposite direction.
+  base_iterator operator-(int offset) const;
 
   // TODO(MXG): Consider the spaceship operator when we can use C++20
 

rmf_traffic/include/rmf_traffic/agv/Graph.hpp

@@ -27,7 +27,9 @@
 
 #include <vector>
 #include <unordered_map>
+#include <unordered_set>
 #include <optional>
+#include <iostream>
 
 namespace rmf_traffic {
 namespace agv {
@@ -37,10 +39,80 @@ class Graph
 {
 public:
 
-  class Waypoint
+  /// Properties related to lifts (elevators) that exist in the graph
+  class LiftProperties
+  {
+  public:
+    /// Get the name of the lift.
+    const std::string& name() const;
+
+    /// Get the (x, y) location of the lift in RMF canonical coordinates.
+    Eigen::Vector2d location() const;
+
+    /// Get the orientation (in radians) of the lift in RMF canonical
+    /// coordinates.
+    double orientation() const;
+
+    /// Get the dimensions of the lift, aligned with the lift's local (x, y)
+    /// coordinates.
+    Eigen::Vector2d dimensions() const;
+
+    /// Get whether the specified position, given in RMF canonical coordinates,
+    /// is inside the lift. The envelope will expand the footprint of the lift
+    /// that is used in the calculation.
+    bool is_in_lift(Eigen::Vector2d position, double envelope = 0.0) const;
+
+    /// Constructor
+    LiftProperties(
+      std::string name,
+      Eigen::Vector2d location,
+      double orientations,
+      Eigen::Vector2d dimensions);
+
+    class Implementation;
+  private:
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+  using LiftPropertiesPtr = std::shared_ptr<LiftProperties>;
+
+  class DoorProperties
   {
   public:
+    /// Get the name of the door.
+    const std::string& name() const;
+
+    /// Get the start position of the door.
+    Eigen::Vector2d start() const;
+
+    /// Get the end position of the door.
+    Eigen::Vector2d end() const;
+
+    /// Get the name of the map that this door is on.
+    const std::string& map() const;
+
+    /// Check if the line formed by p0 -> p1 intersects this door.
+    bool intersects(
+      Eigen::Vector2d p0,
+      Eigen::Vector2d p1,
+      double envelope = 0.0) const;
+
+    /// Constructor
+    DoorProperties(
+      std::string name,
+      Eigen::Vector2d start,
+      Eigen::Vector2d end,
+      std::string map);
+
+    class Implementation;
+  private:
+    rmf_utils::impl_ptr<Implementation> _pimpl;
+  };
+  using DoorPropertiesPtr = std::shared_ptr<DoorProperties>;
 
+  /// Properties assigned to each waypoint (vertex) in the graph
+  class Waypoint
+  {
+  public:
     /// Get the name of the map that this Waypoint exists on.
     const std::string& get_map_name() const;
 
@@ -79,7 +151,6 @@ class Graph
     /// Set this Waypoint to be a parking spot.
     Waypoint& set_parking_spot(bool _is_parking_spot);
 
-
     /// Returns true if this Waypoint is a charger spot. Robots are routed to
     /// these spots when their batteries charge levels drop below the threshold
     /// value.
@@ -88,6 +159,14 @@ class Graph
     /// Set this Waypoint to be a parking spot.
     Waypoint& set_charger(bool _is_charger);
 
+    /// If this waypoint is inside the lift then this will return a pointer to
+    /// the properties of the lift. Otherwise this will be a nullptr.
+    LiftPropertiesPtr in_lift() const;
+
+    /// Set the properties of the lift that the waypoint is inside of, or
+    /// provide a nullptr if it is not inside a lift.
+    Waypoint& set_in_lift(LiftPropertiesPtr properties);
+
     /// The index of this waypoint within the Graph. This cannot be changed
     /// after the waypoint is created.
     std::size_t index() const;
@@ -119,6 +198,25 @@ class Graph
       const std::string& name_format = "%s",
       const std::string& index_format = "#%d") const;
 
+    /// Get the mutex group that this waypoint is associated with. An empty
+    /// string implies that it is not associated with any mutex group.
+    ///
+    /// Only one robot at a time is allowed to occupy any waypoint or lane
+    /// associated with a particular mutex group.
+    const std::string& in_mutex_group() const;
+
+    /// Set what mutex group this waypoint is associated with. Passing in an
+    /// empty string will disasscoiate the waypoint from any mutex group.
+    Waypoint& set_in_mutex_group(std::string group_name);
+
+    /// Get a merge radius specific to this waypoint, if it has one. The radius
+    /// indicates that any robot within this distance of the waypoint can merge
+    /// onto this waypoint.
+    std::optional<double> merge_radius() const;
+
+    /// Set the merge radius specific to this waypoint.
+    Waypoint& set_merge_radius(std::optional<double> valeu);
+
     class Implementation;
   private:
     Waypoint();
@@ -371,8 +469,8 @@ class Graph
       template<typename DerivedExecutor>
       DerivedExecutor& execute(DerivedExecutor& executor) const
       {
-        return static_cast<DerivedExecutor&>(execute(
-            static_cast<Executor&>(executor)));
+        return static_cast<DerivedExecutor&>(
+          execute(static_cast<Executor&>(executor)));
       }
 
       /// Execute this event
@@ -468,6 +566,7 @@ class Graph
 
       /// Construct a default set of properties
       /// * speed_limit: nullopt
+      /// * mutex_group: ""
       Properties();
 
       /// Get the speed limit along this lane. If a std::nullopt is returned,
@@ -478,6 +577,17 @@ class Graph
       /// indicates that there is no speed limit for the lane.
       Properties& speed_limit(std::optional<double> value);
 
+      /// Get the mutex group that this lane is associated with. An empty string
+      /// implies that it is not associated with any mutex group.
+      ///
+      /// Only one robot at a time is allowed to occupy any waypoint or lane
+      /// associated with a particular mutex group.
+      const std::string& in_mutex_group() const;
+
+      /// Set what mutex group this lane is associated with. Passing in an
+      /// empty string will disassociate the lane from any mutex group.
+      Properties& set_in_mutex_group(std::string group_name);
+
       class Implementation;
     private:
       rmf_utils::impl_ptr<Implementation> _pimpl;
@@ -589,6 +699,28 @@ class Graph
   /// const-qualified lane_from()
   const Lane* lane_from(std::size_t from_wp, std::size_t to_wp) const;
 
+  /// Add a known lift to the graph. If this lift has the same name as one
+  /// previously added, we will continue to use the same pointer as the original
+  /// and override the properties because lift names are expected to be unique.
+  LiftPropertiesPtr set_known_lift(LiftProperties lift);
+
+  /// Get all the known lifts.
+  std::vector<LiftPropertiesPtr> all_known_lifts() const;
+
+  /// Find a known lift based on its name.
+  LiftPropertiesPtr find_known_lift(const std::string& name) const;
+
+  /// Add a known door to the graph. If this door has the same name as one
+  /// previously added, we will continue to use the same pointer as the original
+  /// and override the properties because door names are expected to be unique.
+  DoorPropertiesPtr set_known_door(DoorProperties door);
+
+  /// Get all the known doors.
+  std::vector<DoorPropertiesPtr> all_known_doors() const;
+
+  /// Find a known door based on its name.
+  DoorPropertiesPtr find_known_door(const std::string& name) const;
+
   class Implementation;
 private:
   rmf_utils::impl_ptr<Implementation> _pimpl;

rmf_traffic/include/rmf_traffic/schedule/Participant.hpp

@@ -100,6 +100,9 @@ class Participant
   // TODO(MXG): This function needs to be unit tested.
   ItineraryVersion version() const;
 
+  /// Get the current progress version for this participant.
+  ProgressVersion progress_version() const;
+
   /// Get the description of this participant.
   const ParticipantDescription& description() const;
 

rmf_traffic/src/rmf_traffic/Trajectory.cpp

@@ -690,6 +690,38 @@ auto Trajectory::base_iterator<SegT>::operator--(int) -> base_iterator
   return _pimpl->post_decrement<SegT>();
 }
 
+//==============================================================================
+template<typename SegT>
+auto Trajectory::base_iterator<SegT>::operator+(int offset) const
+-> base_iterator
+{
+  const std::size_t N = std::abs(offset);
+  const bool negative = offset < 0;
+
+  base_iterator result = *this;
+  for (std::size_t i=0; i < N; ++i)
+  {
+    if (negative)
+    {
+      --result;
+    }
+    else
+    {
+      ++result;
+    }
+  }
+
+  return result;
+}
+
+//==============================================================================
+template<typename SegT>
+auto Trajectory::base_iterator<SegT>::operator-(int offset) const
+-> base_iterator
+{
+  return *this + (-offset);
+}
+
 //==============================================================================
 #define DEFINE_BASIC_ITERATOR_OP(op) \
   template<typename SegT> \