diff --git a/.github/workflows/asan.yaml b/.github/workflows/asan.yaml index 00eae666..3a7d6630 100644 --- a/.github/workflows/asan.yaml +++ b/.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/setup-ros@v0.2 - 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 diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 6ae74109..cd890674 100644 --- a/.github/workflows/build.yaml +++ b/.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 diff --git a/.github/workflows/tsan.yaml b/.github/workflows/tsan.yaml index 1d36e9d1..16356ffc 100644 --- a/.github/workflows/tsan.yaml +++ b/.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/setup-ros@v0.2 - 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/action-ros-ci@v0.2 - id: tsan_build_test env: CC: clang CXX: clang++ + uses: ros-tooling/action-ros-ci@0.3.5 + 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 diff --git a/rmf_traffic/QUALITY_DECLARATION.md b/rmf_traffic/QUALITY_DECLARATION.md index 70234309..71139c8d 100644 --- a/rmf_traffic/QUALITY_DECLARATION.md +++ b/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] diff --git a/rmf_traffic/include/rmf_traffic/Trajectory.hpp b/rmf_traffic/include/rmf_traffic/Trajectory.hpp index b5968160..0cea4ae3 100644 --- a/rmf_traffic/include/rmf_traffic/Trajectory.hpp +++ b/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 diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index 2e7b97aa..2b1d2b82 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -27,7 +27,9 @@ #include #include +#include #include +#include 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 _pimpl; + }; + using LiftPropertiesPtr = std::shared_ptr; + + 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 _pimpl; + }; + using DoorPropertiesPtr = std::shared_ptr; + /// 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 merge_radius() const; + + /// Set the merge radius specific to this waypoint. + Waypoint& set_merge_radius(std::optional valeu); + class Implementation; private: Waypoint(); @@ -371,8 +469,8 @@ class Graph template DerivedExecutor& execute(DerivedExecutor& executor) const { - return static_cast(execute( - static_cast(executor))); + return static_cast( + execute(static_cast(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 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 _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 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 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 _pimpl; diff --git a/rmf_traffic/include/rmf_traffic/schedule/Participant.hpp b/rmf_traffic/include/rmf_traffic/schedule/Participant.hpp index bae9b2e5..950e3de8 100644 --- a/rmf_traffic/include/rmf_traffic/schedule/Participant.hpp +++ b/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; diff --git a/rmf_traffic/src/rmf_traffic/Trajectory.cpp b/rmf_traffic/src/rmf_traffic/Trajectory.cpp index 79a325b9..3f29c7d2 100644 --- a/rmf_traffic/src/rmf_traffic/Trajectory.cpp +++ b/rmf_traffic/src/rmf_traffic/Trajectory.cpp @@ -690,6 +690,38 @@ auto Trajectory::base_iterator::operator--(int) -> base_iterator return _pimpl->post_decrement(); } +//============================================================================== +template +auto Trajectory::base_iterator::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 +auto Trajectory::base_iterator::operator-(int offset) const +-> base_iterator +{ + return *this + (-offset); +} + //============================================================================== #define DEFINE_BASIC_ITERATOR_OP(op) \ template \ diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp index f97b25b4..82f5a892 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp @@ -25,6 +25,215 @@ namespace rmf_traffic { namespace agv { +//============================================================================== +class Graph::LiftProperties::Implementation +{ +public: + std::string name; + Eigen::Vector2d location; + double orientation; + Eigen::Vector2d half_dimensions; + Eigen::Isometry2d tf_inv; + + static void update( + LiftProperties& original, + const LiftProperties& incoming) + { + *original._pimpl = *incoming._pimpl; + } +}; + +//============================================================================== +const std::string& Graph::LiftProperties::name() const +{ + return _pimpl->name; +} + +//============================================================================== +Eigen::Vector2d Graph::LiftProperties::location() const +{ + return _pimpl->location; +} + +//============================================================================== +double Graph::LiftProperties::orientation() const +{ + return _pimpl->orientation; +} + +//============================================================================== +Eigen::Vector2d Graph::LiftProperties::dimensions() const +{ + return 2.0 * _pimpl->half_dimensions; +} + +//============================================================================== +bool Graph::LiftProperties::is_in_lift( + Eigen::Vector2d position, + double envelope) const +{ + Eigen::Vector2d p_local = _pimpl->tf_inv * position; + for (int i = 0; i < 2; ++i) + { + if (p_local[i] < -_pimpl->half_dimensions[i] - envelope) + return false; + + if (_pimpl->half_dimensions[i] + envelope < p_local[i]) + return false; + } + + return true; +} + +//============================================================================== +Eigen::Isometry2d make_lift_tf_inv(Eigen::Vector2d location, double orientation) +{ + Eigen::Isometry2d tf = Eigen::Isometry2d::Identity(); + tf.translate(location); + tf.rotate(orientation); + return tf.inverse(); +} + +//============================================================================== +Graph::LiftProperties::LiftProperties( + std::string name, + Eigen::Vector2d location, + double orientation, + Eigen::Vector2d dimensions) +: _pimpl(rmf_utils::make_impl( + Implementation { + std::move(name), + location, + orientation, + dimensions / 2.0, + make_lift_tf_inv(location, orientation) + })) +{ + // Do nothing +} + +//============================================================================== +class Graph::DoorProperties::Implementation +{ +public: + std::string name; + Eigen::Vector2d start; + Eigen::Vector2d end; + std::string map; +}; + +//============================================================================== +const std::string& Graph::DoorProperties::name() const +{ + return _pimpl->name; +} + +//============================================================================== +Eigen::Vector2d Graph::DoorProperties::start() const +{ + return _pimpl->start; +} + +//============================================================================== +Eigen::Vector2d Graph::DoorProperties::end() const +{ + return _pimpl->end; +} + +//============================================================================== +const std::string& Graph::DoorProperties::map() const +{ + return _pimpl->map; +} + +//============================================================================== +namespace { +double distance_from_point_to_segment( + Eigen::Vector2d q, + Eigen::Vector2d p0, + Eigen::Vector2d p1) +{ + const double endpoint_distance = std::min((q - p0).norm(), (q - p1).norm()); + const auto L = (p1 - p0).norm(); + if (L < 1e-3) + { + return endpoint_distance; + } + + const Eigen::Vector2d n = (p1 - p0) / L; + const Eigen::Vector2d v = q - p0; + const Eigen::Vector2d q_proj = (v.dot(n)) * n; + const double ortho_distance = (q - q_proj).norm(); + return std::min(ortho_distance, endpoint_distance); +} +} // anonymous namespace + +//============================================================================== +bool Graph::DoorProperties::intersects( + Eigen::Vector2d p0, + Eigen::Vector2d p1, + double envelope) const +{ + const auto q0 = _pimpl->start; + const auto q1 = _pimpl->end; + for (const auto test : std::vector>{ + [&]{ return distance_from_point_to_segment(p0, q0, q1); }, + [&]{ return distance_from_point_to_segment(p1, q0, q1); }, + [&]{ return distance_from_point_to_segment(q0, p0, p1); }, + [&]{ return distance_from_point_to_segment(_pimpl->end, p0, p1); } + }) + { + const double distance = test(); + if (distance <= envelope) + return true; + } + + // If none of the endpoints are within range of the other lines, then the only + // way for an intersection to exist is if the lines truly cross each other. + const double det = (p0.x() - p1.x()) * (q0.y() - q1.y()) + - (p0.y() - p1.y()) * (q0.x() - q1.x()); + + if (std::abs(det) < 1e-8) + { + // The lines are essentially parallel and their endpoints aren't close + // enough, so there is no intersection. + return false; + } + + const double t = ( (p0.x() - q0.x()) * (q0.y() - q1.y()) + - (p0.y() - q0.y()) * (q0.x() - q1.x()) ) + / det; + + if (t < 0.0 || 1.0 < t) + return false; + + const double u = ( (p0.x() - q0.x()) * (p0.y() - p1.y()) + - (p0.y() - q0.y()) * (p0.x() - p1.x()) ) + / det; + + if (u < 0.0 || 1.0 < u) + return false; + + return true; +} + +//============================================================================== +Graph::DoorProperties::DoorProperties( + std::string name, + Eigen::Vector2d start, + Eigen::Vector2d end, + std::string map) +: _pimpl(rmf_utils::make_impl( + Implementation { + std::move(name), + start, + end, + std::move(map) + })) +{ + // Do nothing +} + //============================================================================== class Graph::Waypoint::Implementation { @@ -46,6 +255,12 @@ class Graph::Waypoint::Implementation bool charger = false; + LiftPropertiesPtr in_lift = nullptr; + + std::string mutex_group = ""; + + std::optional merge_radius = std::nullopt; + template static Waypoint make(Args&& ... args) { @@ -140,6 +355,19 @@ auto Graph::Waypoint::set_charger(bool _is_charger) -> Waypoint& return *this; } +//============================================================================== +auto Graph::Waypoint::in_lift() const -> LiftPropertiesPtr +{ + return _pimpl->in_lift; +} + +//============================================================================== +auto Graph::Waypoint::set_in_lift(LiftPropertiesPtr lift) -> Waypoint& +{ + _pimpl->in_lift = lift; + return *this; +} + //============================================================================== std::size_t Graph::Waypoint::index() const { @@ -180,6 +408,32 @@ std::string Graph::Waypoint::name_or_index( + index_format.substr(it+2); } +//============================================================================== +const std::string& Graph::Waypoint::in_mutex_group() const +{ + return _pimpl->mutex_group; +} + +//============================================================================== +auto Graph::Waypoint::set_in_mutex_group(std::string group_name) -> Waypoint& +{ + _pimpl->mutex_group = std::move(group_name); + return *this; +} + +//============================================================================== +std::optional Graph::Waypoint::merge_radius() const +{ + return _pimpl->merge_radius; +} + +//============================================================================== +auto Graph::Waypoint::set_merge_radius(std::optional value) -> Waypoint& +{ + _pimpl->merge_radius = value; + return *this; +} + //============================================================================== Graph::Waypoint::Waypoint() { @@ -676,6 +930,7 @@ class Graph::Lane::Properties::Implementation public: std::optional speed_limit; + std::string mutex_group; }; @@ -700,6 +955,20 @@ auto Graph::Lane::Properties::speed_limit(std::optional value) return *this; } +//============================================================================== +const std::string& Graph::Lane::Properties::in_mutex_group() const +{ + return _pimpl->mutex_group; +} + +//============================================================================== +auto Graph::Lane::Properties::set_in_mutex_group(std::string group_name) +-> Properties& +{ + _pimpl->mutex_group = std::move(group_name); + return *this; +} + //============================================================================== class Graph::Lane::Implementation { @@ -956,5 +1225,83 @@ auto Graph::lane_from(std::size_t from_wp, std::size_t to_wp) const return const_cast(*this).lane_from(from_wp, to_wp); } +//============================================================================== +auto Graph::set_known_lift(LiftProperties lift) -> LiftPropertiesPtr +{ + const auto [l_it, inserted] = _pimpl->lifts.insert({lift.name(), nullptr}); + if (inserted) + { + l_it->second = std::make_shared(std::move(lift)); + } + else + { + *l_it->second = std::move(lift); + } + + return l_it->second; +} + +//============================================================================== +auto Graph::all_known_lifts() const -> std::vector +{ + std::vector lifts; + lifts.reserve(_pimpl->lifts.size()); + for (const auto& [_, lift] : _pimpl->lifts) + { + lifts.push_back(lift); + } + + return lifts; +} + +//============================================================================== +auto Graph::find_known_lift(const std::string& name) const -> LiftPropertiesPtr +{ + const auto l_it = _pimpl->lifts.find(name); + if (l_it == _pimpl->lifts.end()) + return nullptr; + + return l_it->second; +} + +//============================================================================== +auto Graph::set_known_door(DoorProperties door) -> DoorPropertiesPtr +{ + const auto [d_it, inserted] = _pimpl->doors.insert({door.name(), nullptr}); + if (inserted) + { + d_it->second = std::make_shared(std::move(door)); + } + else + { + *d_it->second = std::move(door); + } + + return d_it->second; +} + +//============================================================================== +auto Graph::all_known_doors() const -> std::vector +{ + std::vector doors; + doors.reserve(_pimpl->doors.size()); + for (const auto& [_, door] : _pimpl->doors) + { + doors.push_back(door); + } + + return doors; +} + +//============================================================================== +auto Graph::find_known_door(const std::string& name) const -> DoorPropertiesPtr +{ + const auto d_it = _pimpl->doors.find(name); + if (d_it == _pimpl->doors.end()) + return nullptr; + + return d_it->second; +} + } // namespace avg } // namespace rmf_traffic diff --git a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp index 769e3b35..5bd91d46 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Planner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Planner.cpp @@ -1185,21 +1185,38 @@ std::vector compute_plan_starts( { const auto& lane = graph.get_lane(i); const auto& wp0 = graph.get_waypoint(lane.entry().waypoint_index()); - if (wp0.get_map_name() != map_name) - continue; - const auto& wp1 = graph.get_waypoint(lane.exit().waypoint_index()); - if (wp1.get_map_name() != map_name) + if (wp0.get_map_name() != map_name && wp1.get_map_name() != map_name) continue; + const std::optional wp0_merge_radius = wp0.merge_radius(); + const std::optional wp1_merge_radius = wp1.merge_radius(); + const Eigen::Vector2d p0 = wp0.get_location(); const Eigen::Vector2d p1 = wp1.get_location(); const double lane_length = (p1 - p0).norm(); - // This "lane" is effectively a single point, so we'll skip it + // This "lane" is either two points stacked very close or is moving + // vertically through a lift. + const double merge_dist = std::max( + wp0_merge_radius.value_or(max_merge_lane_distance), + wp1_merge_radius.value_or(max_merge_lane_distance)); + if (lane_length < min_lane_length) + { + const double dp0 = (p_location - p0).norm(); + const double dp1 = (p_location - p1).norm(); + if (dp0 < merge_dist || dp1 < merge_dist) + { + starts.emplace_back( + Plan::Start( + start_time, lane.exit().waypoint_index(), + start_yaw, p_location, i)); + } + continue; + } const Eigen::Vector2d pn = (p1 - p0) / lane_length; const Eigen::Vector2d p_l = p_location - p0; @@ -1211,7 +1228,9 @@ std::vector compute_plan_starts( const double dist_to_entry = p_l.norm(); const std::size_t entry_waypoint_index = lane.entry().waypoint_index(); - if (dist_to_entry < max_merge_lane_distance) + const double merge_dist = + wp0_merge_radius.value_or(max_merge_lane_distance); + if (dist_to_entry < merge_dist) { if (!raw_starts.insert(entry_waypoint_index).second) continue; @@ -1228,7 +1247,9 @@ std::vector compute_plan_starts( const double dist_to_exit = (p_location - p1).norm(); const std::size_t exit_waypoint_index = lane.exit().waypoint_index(); - if (dist_to_exit < max_merge_lane_distance) + const double merge_dist = + wp1_merge_radius.value_or(max_merge_lane_distance); + if (dist_to_exit < merge_dist) { if (!raw_starts.insert(exit_waypoint_index).second) continue; @@ -1244,8 +1265,11 @@ std::vector compute_plan_starts( { const double lane_dist = (p_l - p_l_projection*pn).norm(); const std::size_t exit_waypoint_index = lane.exit().waypoint_index(); + double merge_dist = max_merge_lane_distance; + if (wp0_merge_radius.has_value() && wp1_merge_radius.has_value()) + merge_dist = std::max(*wp0_merge_radius, *wp1_merge_radius); - if (lane_dist < max_merge_lane_distance) + if (lane_dist < merge_dist) { starts.emplace_back( Plan::Start( diff --git a/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp b/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp index d1c2385f..0e3372a3 100644 --- a/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp +++ b/rmf_traffic/src/rmf_traffic/agv/internal_Graph.hpp @@ -31,6 +31,8 @@ class Graph::Implementation std::vector waypoints; std::vector lanes; std::unordered_map keys; + std::unordered_map lifts; + std::unordered_map doors; // A map from a waypoint index to the set of lanes that can exit from it std::vector> lanes_from; diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp index 0c285aa2..36e3ec5e 100644 --- a/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp @@ -45,6 +45,7 @@ std::vector reconstruct_nodes(const NodePtr& finish_node) } std::reverse(node_sequence.begin(), node_sequence.end()); + return node_sequence; } @@ -114,8 +115,6 @@ struct OrientationTimeMap const auto& node_low = it_low->second; const auto& node_high = it_high->second; - assert(node_low->route_from_parent.back().map() - == node_high->route_from_parent.back().map()); const auto& start_wp = node_low->route_from_parent.back().trajectory().back(); @@ -187,6 +186,20 @@ std::vector reconstruct_nodes( const double rotational_threshold) { auto node_sequence = reconstruct_nodes(finish_node); + std::optional start; + if (!node_sequence.empty()) + { + start = node_sequence.front()->start; + if (!start.has_value()) + { + // *INDENT-OFF* + throw std::runtime_error( + "[rmf_traffic::agv::Planner::plan][1] The root node of a solved plan " + "is missing its Start information. This should not happen. Please " + "report this critical bug to the maintainers of rmf_traffic."); + // *INDENT-ON* + } + } // Remove "cruft" from plans. This means making sure vehicles don't do any // unnecessary motions. @@ -235,7 +248,12 @@ std::vector reconstruct_nodes( duplicate.squash(validator); } - return reconstruct_nodes(finish_node); + auto final_node_sequence = reconstruct_nodes(finish_node); + if (!final_node_sequence.empty()) + { + final_node_sequence.front()->start = start; + } + return final_node_sequence; } //============================================================================== @@ -636,8 +654,47 @@ reconstruct_waypoints( } else { - itinerary.push_back(next_route); - extended_route = &itinerary.back(); + bool merged = false; + if (last_route.trajectory().size() < 2) + { + // The last itinerary did not have enough waypoints, so we should + // discard it. + const std::size_t remove = itinerary.size() - 1; + itinerary.pop_back(); + for (auto& candidate : candidates) + { + const auto r_it = std::remove_if( + candidate.waypoint.arrival.begin(), + candidate.waypoint.arrival.end(), + [remove](const Plan::Checkpoint& c) + { + return c.route_id == remove; + }); + candidate.waypoint.arrival.erase( + r_it, + candidate.waypoint.arrival.end()); + } + + if (!itinerary.empty()) + { + Route& last_route = itinerary.back(); + if (next_route.map() == last_route.map()) + { + merged = true; + extended_route = &last_route; + for (const auto& waypoint : next_route.trajectory()) + { + last_route.trajectory().insert(waypoint); + } + } + } + } + + if (!merged) + { + itinerary.push_back(next_route); + extended_route = &itinerary.back(); + } } if (!node->approach_lanes.empty()) @@ -662,6 +719,41 @@ reconstruct_waypoints( } } + std::vector removals; + for (std::size_t i=0; i < itinerary.size(); ++i) + { + if (itinerary[i].trajectory().size() < 2) + { + removals.push_back(i); + } + } + + for (auto r_it = removals.rbegin(); r_it != removals.rend(); ++r_it) + { + std::size_t remove = *r_it; + itinerary.erase(itinerary.begin() + remove); + for (WaypointCandidate& candidate : candidates) + { + auto c_it = candidate.waypoint.arrival.begin(); + while (c_it != candidate.waypoint.arrival.end()) + { + Plan::Checkpoint& c = *c_it; + if (c.route_id == remove) + { + candidate.waypoint.arrival.erase(c_it); + continue; + } + + if (c.route_id > remove) + { + --c.route_id; + } + + ++c_it; + } + } + } + auto plan_waypoints = find_dependencies( itinerary, candidates, validator, dependency_window, dependency_resolution); @@ -680,8 +772,8 @@ Plan::Start find_start(NodePtr node) { // *INDENT-OFF* throw std::runtime_error( - "[rmf_traffic::agv::Planner::plan] The root node of a solved plan is " - "missing its Start information. This should not happen. Please report " + "[rmf_traffic::agv::Planner::plan][2] The root node of a processed plan " + "is missing its Start information. This should not happen. Please report " "this critical bug to the maintainers of rmf_traffic."); // *INDENT-ON* } @@ -725,6 +817,7 @@ class ScheduledDifferentialDriveExpander double current_cost; std::optional start; SearchNodePtr parent; + std::size_t line; double get_total_cost_estimate() const { @@ -756,7 +849,8 @@ class ScheduledDifferentialDriveExpander Graph::Lane::EventPtr event_, double current_cost_, std::optional start_, - SearchNodePtr parent_) + SearchNodePtr parent_, + std::size_t line_) : entry(entry_), waypoint(waypoint_), approach_lanes(std::move(approach_lanes_)), @@ -768,7 +862,8 @@ class ScheduledDifferentialDriveExpander event(event_), current_cost(current_cost_), start(std::move(start_)), - parent(std::move(parent_)) + parent(std::move(parent_)), + line(line_) { assert(!route_from_parent.empty()); @@ -920,11 +1015,57 @@ class ScheduledDifferentialDriveExpander const auto& wp = _supergraph->original().waypoints[target_waypoint_index]; const Eigen::Vector2d wp_location = wp.get_location(); + SearchNodePtr parent = top; + if (start.lane().has_value() && start.location().has_value()) + { + const auto lane_index = start.lane().value(); + const Eigen::Vector2d p = start.location().value(); + const auto& lane = _supergraph->original().lanes[lane_index]; + if (lane.entry().event()) + { + Graph::Lane::EventPtr entry_event = lane.entry().event()->clone(); + const double event_cost = time::to_seconds(entry_event->duration()); + Trajectory hold; + Eigen::Vector3d p_start = {p.x(), p.y(), start.orientation()}; + const auto zero = Eigen::Vector3d::Zero(); + hold.insert(parent->time, p_start, zero); + hold.insert(parent->time + entry_event->duration(), p_start, zero); + Route route{wp.get_map_name(), std::move(hold)}; + if (_validator && !is_valid(parent, route)) + { + // If we cannot wait for the entry event to happen then this is not a + // feasible start. + return; + } + + parent = std::make_shared( + SearchNode{ + std::nullopt, + top->waypoint, + {}, + p, + start.orientation(), + parent->time + entry_event->duration(), + parent->remaining_cost_estimate, + {std::move(route)}, + std::move(entry_event), + parent->current_cost + event_cost, + std::nullopt, + parent, + __LINE__ + }); + } + } + + Graph::Lane::EventPtr entry_event; + double entry_event_cost = 0.0; + Duration entry_event_duration = Duration(0); + // If this start node did not have a waypoint, then it must have a location assert(start.location().has_value()); const auto approach_info = make_start_approach_trajectories( - top->start.value(), top->current_cost); + top->start.value(), parent->current_cost); if (approach_info.trajectories.empty()) { @@ -974,7 +1115,7 @@ class ScheduledDifferentialDriveExpander for (const auto& map : map_names) { Route route{map, approach_trajectory}; - if (_validator && !is_valid(top, route)) + if (_validator && !is_valid(parent, route)) { all_valid = false; break; @@ -998,7 +1139,7 @@ class ScheduledDifferentialDriveExpander for (const auto& map : map_names) { Route route{map, hold}; - if (_validator && !is_valid(top, route)) + if (_validator && !is_valid(parent, route)) { all_valid = false; break; @@ -1026,12 +1167,13 @@ class ScheduledDifferentialDriveExpander wp_location, approach_yaw, approach_time, - top->remaining_cost_estimate - approach_cost, + parent->remaining_cost_estimate - approach_cost, std::move(approach_routes), exit_event, - top->current_cost + approach_cost, + parent->current_cost + approach_cost, std::nullopt, - top + parent, + __LINE__ }); if (exit_event) @@ -1049,7 +1191,8 @@ class ScheduledDifferentialDriveExpander nullptr, node->current_cost + exit_event_cost, std::nullopt, - node + node, + __LINE__ }); } @@ -1081,7 +1224,7 @@ class ScheduledDifferentialDriveExpander std::make_shared( SearchNode{ std::nullopt, - std::nullopt, + top->waypoint, {}, p, yaw, @@ -1091,7 +1234,8 @@ class ScheduledDifferentialDriveExpander nullptr, top->current_cost + hold_cost, start, - top + top, + __LINE__ })); } @@ -1137,7 +1281,8 @@ class ScheduledDifferentialDriveExpander nullptr, top->current_cost + cost, std::nullopt, - top + top, + __LINE__ }); } @@ -1193,7 +1338,8 @@ class ScheduledDifferentialDriveExpander nullptr, top->current_cost + cost, std::nullopt, - top + top, + __LINE__ }); } @@ -1425,6 +1571,26 @@ class ScheduledDifferentialDriveExpander const double cost = calculate_cost(approach_route.trajectory()); const double yaw = approach_wp.position()[2]; const auto time = approach_wp.time(); + auto parent = node; + std::vector route_from_parent; + std::vector approach_lanes; + if (approach_route.trajectory().size() < 2) + { + // This is just an entry event so we will skip the unnecessary + // intermediate node + parent = node->parent; + approach_lanes = node->approach_lanes; + if (!parent) + { + // If the top node is a root node, then don't skip it + parent = node; + } + route_from_parent = node->route_from_parent; + } + else + { + route_from_parent = {std::move(approach_route)}; + } node = std::make_shared( SearchNode{ @@ -1434,17 +1600,18 @@ class ScheduledDifferentialDriveExpander Side::Start }, initial_waypoint_index, - {}, + approach_lanes, p0, yaw, time, *remaining_cost_estimate + entry_event_cost + alt->cost + exit_event_cost, - {std::move(approach_route)}, + route_from_parent, traversal.entry_event, node->current_cost + cost, std::nullopt, - node + parent, + __LINE__ }); } @@ -1483,7 +1650,8 @@ class ScheduledDifferentialDriveExpander traversal.exit_event, node->current_cost + entry_event_cost + alt->cost, std::nullopt, - node + node, + __LINE__ }); if (traversal.exit_event && exit_event_route.trajectory().size() >= 2) @@ -1501,7 +1669,8 @@ class ScheduledDifferentialDriveExpander nullptr, node->current_cost + exit_event_cost, std::nullopt, - node + node, + __LINE__ }); } @@ -1551,7 +1720,8 @@ class ScheduledDifferentialDriveExpander solution_root->info.event, search_node->current_cost + approach_info.cost, std::nullopt, - search_node + search_node, + __LINE__ }); } @@ -1599,7 +1769,8 @@ class ScheduledDifferentialDriveExpander solution_node->info.event, search_node->current_cost + solution_node->info.cost_from_parent, std::nullopt, - search_node + search_node, + __LINE__ }); solution_node = solution_node->child; @@ -1626,7 +1797,15 @@ class ScheduledDifferentialDriveExpander if (!top->waypoint.has_value()) { // If the node does not have a waypoint, then it must be a start node. - assert(top->start.has_value()); + if (!top->start.has_value()) + { + throw std::runtime_error( + "[rmf_traffic::agv::planning::DifferentialDrivePlanner::expand] " + "Node has no waypoint and also no start information. It was produced " + "on line [" + std::to_string(top->line) + "]. This should not be " + "possible. Please report this critical bug to the maintainers of " + "rmf_traffic."); + } expand_start(top, queue); return; } @@ -1902,7 +2081,8 @@ class ScheduledDifferentialDriveExpander nullptr, 0.0, start, - nullptr + nullptr, + __LINE__ }); } @@ -1941,7 +2121,7 @@ class ScheduledDifferentialDriveExpander for (const auto& void_node : nodes) { bool skip = false; - const auto original_node = + auto original_node = std::static_pointer_cast(void_node.first); const auto original_t = void_node.second; @@ -1976,6 +2156,14 @@ class ScheduledDifferentialDriveExpander if (skip) continue; + while (original_node && !original_node->waypoint.has_value() && !original_node->start.has_value()) + { + original_node = original_node->parent; + } + + if (!original_node) + continue; + rollout_queue.emplace_back( RolloutEntry{ original_t, diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/Supergraph.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/Supergraph.cpp index 69b7b78e..5f5f8eb6 100644 --- a/rmf_traffic/src/rmf_traffic/agv/planning/Supergraph.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/planning/Supergraph.cpp @@ -80,6 +80,8 @@ struct TraversalNode Graph::Lane::EventPtr entry_event; Graph::Lane::EventPtr exit_event; + std::string mutex_group; + // TODO(MXG): Replace this with a more intelligent way of handling speed limit // changes that may occur when traversing multiple consecutive lanes std::optional lowest_speed_limit; @@ -299,6 +301,7 @@ void perform_traversal( node.initial_waypoint_index = wp_index_0; node.finish_waypoint_index = wp_index_1; node.lowest_speed_limit = lane.properties().speed_limit(); + node.mutex_group = lane.properties().in_mutex_group(); if (parent) { @@ -309,6 +312,31 @@ void perform_traversal( return; } + const auto& wp0_mutex = wp0.in_mutex_group(); + if (!parent->mutex_group.empty() && wp0_mutex.empty()) + { + // We are exiting a mutex group, so we should do a quick stop here to make + // sure that we release the mutex. + return; + } + + if (!node.mutex_group.empty() && parent->mutex_group != node.mutex_group) + { + // The lane belongs to a different mutex group than the parent, so we + // cannot continuously traverse. + return; + } + + const auto& wp1_mutex = wp1.in_mutex_group(); + if (!wp1_mutex.empty() && wp1_mutex != node.mutex_group) + { + // The end waypoint of this lane belongs to a different mutex group than + // the lane leading up to it. The waypoint's mutex must be locked before + // traversing this lane, so we should stop before traversing this lane + // instead of continuing from a parent. + return; + } + node.initial_lane_index = parent->initial_lane_index; node.initial_waypoint_index = parent->initial_waypoint_index; node.initial_p = parent->initial_p; diff --git a/rmf_traffic/src/rmf_traffic/schedule/Database.cpp b/rmf_traffic/src/rmf_traffic/schedule/Database.cpp index 35db7448..3f6dd48c 100644 --- a/rmf_traffic/src/rmf_traffic/schedule/Database.cpp +++ b/rmf_traffic/src/rmf_traffic/schedule/Database.cpp @@ -1488,10 +1488,10 @@ auto Database::changes( } std::vector part_patches; - for (const auto& p : changes) + for (const auto p : _pimpl->participant_ids) { - const auto& changeset = p.second; - const auto& state = _pimpl->states.at(p.first); + const auto& changeset = changes[p]; + const auto& state = _pimpl->states.at(p); std::vector delays; for (const auto& d : changeset.delays) @@ -1502,15 +1502,6 @@ auto Database::changes( }); } - if (changeset.erasures.empty() - && delays.empty() - && changeset.additions.empty()) - { - // There aren't actually any changes for this participant, so we will - // leave it out of the patch. - continue; - } - std::optional progress; if (state.schedule_version_of_progress.has_value()) { @@ -1522,13 +1513,23 @@ auto Database::changes( } } + if (changeset.erasures.empty() + && delays.empty() + && changeset.additions.empty() + && !progress.has_value()) + { + // There aren't actually any changes for this participant, so we will + // leave it out of the patch. + continue; + } + part_patches.emplace_back( Patch::Participant{ - p.first, + p, state.tracker->last_known_version(), - Change::Erase(std::move(p.second.erasures)), + Change::Erase(std::move(changeset.erasures)), std::move(delays), - Change::Add(state.latest_plan_id, std::move(p.second.additions)), + Change::Add(state.latest_plan_id, std::move(changeset.additions)), std::move(progress) }); } diff --git a/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp b/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp index 2743a792..ffdd4695 100644 --- a/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp +++ b/rmf_traffic/src/rmf_traffic/schedule/Participant.cpp @@ -482,6 +482,12 @@ ItineraryVersion Participant::version() const return _pimpl->_shared->_version; } +//============================================================================== +ProgressVersion Participant::progress_version() const +{ + return _pimpl->_shared->_progress.version; +} + //============================================================================== const ParticipantDescription& Participant::description() const { diff --git a/rmf_traffic/test/unit/schedule/test_Database.cpp b/rmf_traffic/test/unit/schedule/test_Database.cpp index 622d73a5..64bb2cb9 100644 --- a/rmf_traffic/test/unit/schedule/test_Database.cpp +++ b/rmf_traffic/test/unit/schedule/test_Database.cpp @@ -200,7 +200,8 @@ SCENARIO("Test Database Conflicts") // query from the start changes = db.changes(query_all, rmf_utils::nullopt); - CHECK(changes.size() == 0); + // Progress will always be reported for each participant in a full update + CHECK(changes.size() == 1); CHECK_FALSE(changes.cull()); CHECK(changes.latest_version() == db.latest_version()); @@ -230,7 +231,8 @@ SCENARIO("Test Database Conflicts") // query from the start changes = db.changes(query_all, rmf_utils::nullopt); - CHECK(changes.size() == 0); + // Progress will always be reported for each participant in a full update + CHECK(changes.size() == 1); CHECK_FALSE(changes.cull()); CHECK(changes.latest_version() == db.latest_version()); diff --git a/rmf_traffic/test/unit/schedule/test_Mirror.cpp b/rmf_traffic/test/unit/schedule/test_Mirror.cpp index d0e6d712..62b3c5bc 100644 --- a/rmf_traffic/test/unit/schedule/test_Mirror.cpp +++ b/rmf_traffic/test/unit/schedule/test_Mirror.cpp @@ -327,10 +327,18 @@ SCENARIO("Testing specialized mirrors") db.changes(query, rmf_utils::nullopt); REQUIRE(changes.size() > 0); - CHECK(changes.size() == 1); - CHECK(changes.begin()->participant_id() == p1.id()); - REQUIRE(changes.begin()->additions().items().size() == 1); - CHECK(changes.begin()->additions().items().begin()->storage_id == 0); + // Progress will always be updated when a full update is asked for + CHECK(changes.size() == 2); + + rmf_traffic::schedule::Patch::const_iterator p_it = changes.begin(); + for (; p_it != changes.end(); ++p_it) + { + if (p_it->participant_id() == p1.id()) + break; + } + REQUIRE(p_it != changes.end()); + REQUIRE(p_it->additions().items().size() == 1); + CHECK(p_it->additions().items().begin()->storage_id == 0); } GIVEN("Query patch with spacetime region overlapping with t2") @@ -356,10 +364,18 @@ SCENARIO("Testing specialized mirrors") db.changes(query, rmf_utils::nullopt); REQUIRE(changes.size() > 0); - CHECK(changes.size() == 1); - CHECK(changes.begin()->participant_id() == p1.id()); - REQUIRE(changes.begin()->additions().items().size() == 1); - CHECK(changes.begin()->additions().items().begin()->storage_id == 1); + // Progress will always be updated when a full update is asked for + CHECK(changes.size() == 2); + + rmf_traffic::schedule::Patch::const_iterator p_it = changes.begin(); + for (; p_it != changes.end(); ++p_it) + { + if (p_it->participant_id() == p1.id()) + break; + } + REQUIRE(p_it != changes.end()); + REQUIRE(p_it->additions().items().size() == 1); + CHECK(p_it->additions().items().begin()->storage_id == 1); } // // COMMENTED DUE TO NON-DETERMINISTIC BEHAVIOR OF FCL @@ -413,7 +429,8 @@ SCENARIO("Testing specialized mirrors") rmf_traffic::schedule::Patch changes = db.changes(query, rmf_utils::nullopt); REQUIRE(changes.size() > 0); - CHECK(changes.size() == 1); + // Progress will always be updated when a full update is asked for + CHECK(changes.size() == 2); CHECK(changes.begin()->participant_id() == p2.id()); REQUIRE(changes.begin()->additions().items().size() == 1); CHECK(changes.begin()->additions().items().begin()->storage_id == 0);