diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst new file mode 100644 index 000000000..bbd8f97d9 --- /dev/null +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmf_charging_schedule +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.2 (XXXX-YY-ZZ) +------------------ +* First release diff --git a/rmf_charging_schedule/README.md b/rmf_charging_schedule/README.md new file mode 100644 index 000000000..3a14c818b --- /dev/null +++ b/rmf_charging_schedule/README.md @@ -0,0 +1,39 @@ +A simple node that takes in a yaml file that describes a schedule for charger +usage in an RMF scenario. The node will watch the clock and publish the +appropriate commands to change the chargers of the robots. + +The format for the schedule looks like this: +``` +"my_fleet_name": + "00:00": { "robot_1": "charger_A", "robot_2": "charger_B", "robot_3": "queue_A" } + "01:55": { "robot_1": "queue_B" } + "02:00": { "robot_3": "charger_A", "robot_1": "queue_A" } + "03:55": { "robot_2": "queue_B" } + "04:00": { "robot_1": "charger_B", "robot_2": "queue_A" } +``` + +The time format is `"HH:MM"` where `HH` ranges from `00` to `23` and `MM` ranges +from `00` to `59`. Note that quotes are important because otherwise the yaml +format may confuse the meaning of the colon `:`. + +The schedule will cycle every 24 hours. + +For each timestamp, only robots that are explicitly mentioned will have their +dedicated charger changed. **It is the responsibility of the schedule file author +to make sure that two robots are never assigned the same charger at the same +time.** Failing to ensure this may cause traffic and task management to misbehave. + +When run in simulation mode (`--ros-args --use-sim-time`), the time `00:00` in +the schedule will correspond to `t=0.0` in simulation time. + +When run without sim time on, the hours and minutes will correspond to the local +timezone of the machine that the node is run on. To choose a specific timezone +instead of using the system's local timzeone, use the `--timezone` argument and +provide the desired [TZ identifier](https://en.wikipedia.org/wiki/List_of_tz_database_time_zones) +string. + +It is advisable that you always put a `00:00` entry that indicates **all** of +the intended charger assignments at midnight. When the node is launched, it will +move through the schedule from the earliest entry up until the last relevant one +and issue an initial charger assignment message based on what the assignments +would have been if the schedule had run from `00:00`. diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml new file mode 100644 index 000000000..f53610f8c --- /dev/null +++ b/rmf_charging_schedule/package.xml @@ -0,0 +1,16 @@ + + + + rmf_charging_schedule + 2.3.2 + Node for a fixed 24-hour rotating charger usage schedule + Grey + Apache License 2.0 + + rclpy + rmf_fleet_msgs + + + ament_python + + diff --git a/rmf_charging_schedule/resource/rmf_charging_schedule b/rmf_charging_schedule/resource/rmf_charging_schedule new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_charging_schedule/rmf_charging_schedule/__init__.py b/rmf_charging_schedule/rmf_charging_schedule/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/rmf_charging_schedule/rmf_charging_schedule/main.py b/rmf_charging_schedule/rmf_charging_schedule/main.py new file mode 100644 index 000000000..9d90062c7 --- /dev/null +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -0,0 +1,240 @@ +import sys +import yaml +import datetime +import argparse +from collections import OrderedDict +import bisect +from functools import total_ordering +from zoneinfo import ZoneInfo + +from icecream import ic + +import rclpy +from rclpy.node import Node, Publisher +from rclpy.qos import ( + QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy +) + +from rmf_fleet_msgs.msg import ChargingAssignment, ChargingAssignments + +@total_ordering +class ScheduleTimePoint: + hour: int + minute: int + + def __init__(self, hour: int, minute: int): + self.hour = hour + self.minute = minute + + def parse(text: str): + segments = text.split(":") + assert len(segments) == 2, ( + f'Time point text [{text}] does not have the correct HH:MM format' + ) + hh = int(segments[0]) + mm = int(segments[1]) + assert 0 <= hh and hh < 24, ( + f'Time point text [{text}] has an hour value which is outside the ' + f'valid range of 0 -> 23.' + ) + assert 0 <= mm and mm < 60, ( + f'Time point text [{text}] has a minute value which is outside the ' + f'valid range of 0 -> 59' + ) + return ScheduleTimePoint(hh, mm) + + def __eq__(self, other): + return self.hour == other.hour and self.minute == other.minute + + def __lt__(self, other): + if self.hour < other.hour: + return True + elif self.hour > other.hour: + return False + return self.minute < other.minute + + def __hash__(self): + return hash((self.hour, self.minute)) + + +class Assignment: + fleet: str + robot: str + charger: str + + def __init__(self, fleet, robot, charger): + self.fleet = fleet + self.robot = robot + self.charger = charger + + +def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): + for fleet, robots in assignments.items(): + msg = ChargingAssignments() + msg.fleet_name = fleet + for robot, charger in robots.items(): + assignment = ChargingAssignment() + assignment.robot_name = robot + assignment.waypoint_name = charger + # The mode isn't actually used yet, so it doesn't matter what we set + # it to. + assignment.mode = ChargingAssignment.MODE_CHARGE + msg.assignments.append(assignment) + + publisher.publish(msg) + +def update_assignments( + last_update_index: int | None, + next_update_index: int, + sorted_times: list, + schedule: dict, + assignments: dict, + publisher: Publisher, + node: Node, +): + for key in sorted_times[last_update_index:next_update_index]: + changes: list[Assignment] = schedule[key] + for change in changes: + assignments.setdefault(change.fleet, {})[change.robot] = change.charger + node.get_logger().info( + f'Sending {change.fleet}/{change.robot} to {change.charger} at ' + f'{key.hour:02d}:{key.minute:02d}' + ) + publish_assignments(publisher, assignments) + + +def simulation_time(node: Node) -> ScheduleTimePoint: + seconds, _ = node.get_clock().now().seconds_nanoseconds() + minutes: int = int(seconds/60) + hour: int = int((minutes/60) % 24) + minute = minutes % 60 + return ScheduleTimePoint(hour, minute) + + +def real_time(node: Node, timezone: ZoneInfo) -> ScheduleTimePoint: + nanoseconds = float(node.get_clock().now().nanoseconds) + seconds = nanoseconds / 1e9 + dt = datetime.datetime.fromtimestamp(seconds, timezone) + return ScheduleTimePoint(dt.hour, dt.minute) + + +def main(argv=sys.argv): + rclpy.init(args=argv) + node = Node("rmf_charging_schedule") + use_sim_time = node.get_parameter('use_sim_time').value + + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser( + prog='rmf_charging_schedule', + description='Manage a fixed 24-hour charger schedule rotation', + ) + parser.add_argument( + 'schedule', type=str, + help=( + 'A .yaml file representing the schedule. See README for the ' + 'expected format.' + ) + ) + parser.add_argument( + '-z', '--timezone', type=str, required=False, + help=( + 'Timezone that the 24-hour rotation will be based on. If not ' + 'provided, the system\'s local timezone will be used.' + ) + ) + parser.add_argument( + '-t', '--test-time', action='store_true', + help=( + 'Use this option to test the real time calculation by printing the ' + 'current HH:MM based on your settings. This may be used in ' + 'conjunction with the --timezone option and sim time. The node ' + 'will immediately quit after printing the time, so this will not ' + 'publish any assignment messages.' + ) + ) + + args = parser.parse_args(args_without_ros[1:]) + schedule_file = args.schedule + + if args.timezone is not None: + timezone = ZoneInfo(args.timezone) + else: + timezone = None + + if use_sim_time: + get_time = lambda: simulation_time(node) + else: + get_time = lambda: real_time(node, timezone) + + if args.test_time: + t = get_time() + print(f'{t.hour:02d}:{t.minute:02d}') + return + + with open(schedule_file, 'r') as f: + schedule_yaml = yaml.safe_load(f) + + unsorted_schedule = {} + for fleet, change in schedule_yaml.items(): + for time_text, assignments in change.items(): + time = ScheduleTimePoint.parse(time_text) + entry: list[Assignment] = unsorted_schedule.get(time, list()) + for robot, charger in assignments.items(): + entry.append(Assignment(fleet, robot, charger)) + unsorted_schedule[time] = entry + + schedule = {} + sorted_times = [] + for time in sorted(unsorted_schedule.keys()): + sorted_times.append(time) + schedule[time] = unsorted_schedule[time] + + num_fleets = len(schedule_yaml.keys()) + transient_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=2*num_fleets, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + publisher = node.create_publisher( + ChargingAssignments, 'charging_assignments', transient_qos + ) + + # fleet -> robot -> charger + assignments = {} + last_update_index = bisect.bisect_right(sorted_times, get_time()) + update_assignments( + None, last_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + + def update(): + nonlocal last_update_index + nonlocal sorted_times + nonlocal schedule + nonlocal assignments + nonlocal publisher + + next_update_index = bisect.bisect_right(sorted_times, get_time()) + if last_update_index < next_update_index: + update_assignments( + last_update_index, next_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + last_update_index = next_update_index + + elif next_update_index < last_update_index: + # The cycle must have restarted, e.g. passing midnight + update_assignments( + None, next_update_index, + sorted_times, schedule, assignments, publisher, node, + ) + last_update_index = next_update_index + + node.create_timer(10.0, update) + + rclpy.spin(node) + +if __name__ == '__main__': + main(sys.argv) diff --git a/rmf_charging_schedule/setup.cfg b/rmf_charging_schedule/setup.cfg new file mode 100644 index 000000000..fb5d24c84 --- /dev/null +++ b/rmf_charging_schedule/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_charging_schedule +[install] +install_scripts=$base/lib/rmf_charging_schedule diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py new file mode 100644 index 000000000..6ae87a05e --- /dev/null +++ b/rmf_charging_schedule/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup, find_packages + +package_name = 'rmf_charging_schedule' + +setup( + name=package_name, + version='2.3.2', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Grey', + maintainer_email='mxgrey@intrinsic.ai', + description='A node that manages a fixed schedule for robot charger usage', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'charging_schedule=rmf_charging_schedule.main:main', + ], + }, +) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index c38326902..152250741 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -255,6 +255,25 @@ target_include_directories(lift_supervisor # ----------------------------------------------------------------------------- +add_executable(mutex_group_supervisor + src/mutex_group_supervisor/main.cpp +) + +target_link_libraries(mutex_group_supervisor + PRIVATE + rmf_fleet_adapter + ${rclcpp_LIBARRIES} + ${rmf_fleet_msgs_LIBRARIES} +) + +target_include_directories(mutex_group_supervisor + PRIVATE + ${rclcpp_INCLUDE_DIRS} + ${rmf_fleet_msgs_INCLUDE_DIRS} +) + +# ----------------------------------------------------------------------------- + add_executable(experimental_lift_watchdog src/experimental_lift_watchdog/main.cpp ) @@ -502,6 +521,7 @@ install( mock_traffic_light full_control lift_supervisor + mutex_group_supervisor experimental_lift_watchdog door_supervisor robot_state_aggregator diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 9d0ac0591..a736ce9e5 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -68,6 +68,13 @@ const std::string InterruptRequestTopicName = "robot_interrupt_request"; const std::string TaskApiRequests = "task_api_requests"; const std::string TaskApiResponses = "task_api_responses"; +const std::string ChargingAssignmentsTopicName = "charging_assignments"; + +const std::string MutexGroupRequestTopicName = "mutex_group_request"; +const std::string MutexGroupStatesTopicName = "mutex_group_states"; + +const uint64_t Unclaimed = (uint64_t)(-1); + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index b5f9531c9..2be6c0698 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -74,7 +74,11 @@ class EasyFullControl : public std::enable_shared_from_this class CommandExecution; /// Signature for a function that handles navigation requests. The request - /// will specify an (x, y) location and yaw on a map. + /// will specify a destination for the robot to go to. + /// + /// \param[in] destination + /// Where the robot should move to. Includings (x, y) coordinates, a target + /// yaw, a map name, and may include a graph index when one is available. /// /// \param[in] execution /// The command execution progress updater. Use this to keep the fleet @@ -87,6 +91,19 @@ class EasyFullControl : public std::enable_shared_from_this /// Signature for a function to handle stop requests. using StopRequest = std::function; + /// Signature for a function that handles localization requests. The request + /// will specify an approximate location for the robot. + /// + /// \param[in] location_estimate + /// An estimate for where the robot is currently located. + /// + /// \param[in] execution + /// The command execution progress updater. Use this to keep the fleet + /// adapter updated on the progress of localizing. + using LocalizationRequest = std::function; + /// Add a robot to the fleet once it is available. /// /// \param[in] name @@ -333,6 +350,13 @@ class EasyFullControl::RobotCallbacks /// Get the action executor. ActionExecutor action_executor() const; + /// Give the robot a localization callback. Unlike the callbacks used by the + /// constructor, this callback is optional. + RobotCallbacks& with_localization(LocalizationRequest localization); + + /// Get the callback for localizing if available. + LocalizationRequest localize() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; @@ -414,6 +438,10 @@ class EasyFullControl::Destination /// from this field. std::optional graph_index() const; + /// The name of this destination, if it has one. Nameless destinations will + /// give an empty string. + std::string name() const; + /// If there is a speed limit that should be respected while approaching the /// destination, this will indicate it. std::optional speed_limit() const; @@ -422,6 +450,10 @@ class EasyFullControl::Destination /// will contain the name of the dock. std::optional dock() const; + /// Get whether the destination is inside of a lift, and if so get the + /// properties of the lift. + rmf_traffic::agv::Graph::LiftPropertiesPtr inside_lift() const; + class Implementation; private: Destination(); @@ -743,6 +775,38 @@ class EasyFullControl::FleetConfiguration /// Set the minimum lane length. void set_default_min_lane_length(double distance); + /// During a fire emergency, real-life lifts might be required to move to a + /// specific level and refuse to stop or go to any other level. This function + /// lets you provide this information to the fleet adapter so that it can + /// produce reasonable emergency pullover plans for robots that happen to be + /// inside of a lift when the fire alarm goes off. + /// + /// Internally, this will close all lanes that go into the specified lift and + /// close all lanes exiting this lift (except on the designated level) when a + /// fire emergency begins. Lifts that were not specified in a call to this + /// function will not behave any differently during a fire emergency. + /// + /// \param[in] lift_name + /// The name of the lift whose behavior is being specified + /// + /// \param[in] emergency_level_name + /// The level that lift will go to when a fire emergency is happening + void set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name); + + /// Get mutable access to the level that each specified lift will go to during + /// a fire emergency. + /// + /// \sa set_lift_emergency_level + std::unordered_map& change_lift_emergency_levels(); + + /// Get the level that each specified lift will go to during a fire emergency. + /// + /// \sa set_lift_emergency_level + const std::unordered_map& + lift_emergency_levels() const; + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 4bd5ad23d..290f5bd35 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -210,6 +210,26 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// Specify a set of lanes that should be open. void open_lanes(std::vector lane_indices); + /// During a fire emergency, real-life lifts might be required to move to a + /// specific level and refuse to stop or go to any other level. This function + /// lets you provide this information to the fleet adapter so that it can + /// produce reasonable emergency pullover plans for robots that happen to be + /// inside of a lift when the fire alarm goes off. + /// + /// Internally, this will close all lanes that go into the specified lift and + /// close all lanes exiting this lift (except on the designated level) when a + /// fire emergency begins. Lifts that were not specified in a call to this + /// function will not behave any differently during a fire emergency. + /// + /// \param[in] lift_name + /// The name of the lift whose behavior is being specified + /// + /// \param[in] emergency_level_name + /// The level that lift will go to when a fire emergency is happening + void set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name); + /// A class used to describe speed limit imposed on lanes. class SpeedLimitRequest { diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index d0aa2c22e..22da7b444 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -126,6 +126,10 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; + /// Get the current task ID of the robot, or an empty string if the robot + /// is not performing any task. + const std::string current_task_id() const; + /// Unique identifier for an activity that the robot is performing. Used by /// the EasyFullControl API. class ActivityIdentifier @@ -404,6 +408,9 @@ class RobotUpdateHandle /// By default this behavior is enabled. void enable_responsive_wait(bool value); + /// If the robot is holding onto a session with a lift, release that session. + void release_lift(); + class Implementation; /// This API is experimental and will not be supported in the future. Users @@ -471,6 +478,9 @@ class RobotUpdateHandle Watchdog watchdog, rmf_traffic::Duration wait_duration = std::chrono::seconds(10)); + /// Turn on/off a debug dump of how position updates are being processed + void debug_positions(bool on); + private: friend Implementation; Implementation* _pimpl; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp new file mode 100644 index 000000000..01a2d315b --- /dev/null +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/tasks/ParkRobotIndefinitely.hpp @@ -0,0 +1,62 @@ +/* + * 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. + * +*/ + +#ifndef RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP +#define RMF_FLEET_ADAPTER__TASKS__PARKROBOT_HPP + +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +/// Use this task factory to make finisher tasks (idle tasks) that will move the +/// robot to a parking spot. +class ParkRobotIndefinitely : public rmf_task::RequestFactory +{ +public: + /// Constructor + /// + /// \param[in] requester + /// The identifier of the entity that owns this RequestFactory, that will be + /// the designated requester of each new request. + /// + /// \param[in] time_now_cb + /// Callback function that returns the current time. + /// + /// \param[in] parking_waypoint + /// The graph index of the waypoint assigned to this AGV for parking. + /// If nullopt, the AGV will return to its charging_waypoint and remain idle + /// there. It will not wait for its battery to charge up before undertaking + /// new tasks. + ParkRobotIndefinitely( + const std::string& requester, + std::function time_now_cb, + std::optional parking_waypoint = std::nullopt); + + rmf_task::ConstRequestPtr make_request( + const rmf_task::State& state) const final; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__PARKROBOT_HPP diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index e979212fd..2888482da 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -1249,10 +1249,12 @@ std::shared_ptr make_fleet( if (finishing_request_string == "charge") { - finishing_request = + auto charge_factory = std::make_shared( std::string(node->get_name()), std::move(get_time)); + charge_factory->set_indefinite(true); + finishing_request = charge_factory; RCLCPP_INFO( node->get_logger(), "Fleet is configured to perform ChargeBattery as finishing request"); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index fc096dcd0..77c05fdcb 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -28,12 +28,14 @@ Node::Node() : rclcpp::Node("rmf_lift_supervisor") { const auto default_qos = rclcpp::SystemDefaultsQoS(); + const auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); _lift_request_pub = create_publisher( - FinalLiftRequestTopicName, default_qos); + FinalLiftRequestTopicName, transient_qos); _adapter_lift_request_sub = create_subscription( - AdapterLiftRequestTopicName, default_qos, + AdapterLiftRequestTopicName, transient_qos, [&](LiftRequest::UniquePtr msg) { _adapter_lift_request_update(std::move(msg)); @@ -53,6 +55,12 @@ Node::Node() //============================================================================== void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) { + + RCLCPP_INFO( + this->get_logger(), + "[%s] Received adapter lift request to [%s] with request type [%d]", + msg->session_id.c_str(), msg->destination_floor.c_str(), msg->request_type + ); auto& curr_request = _active_sessions.insert( std::make_pair(msg->lift_name, nullptr)).first->second; @@ -64,7 +72,13 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) curr_request = std::move(msg); else { + msg->request_time = this->now(); _lift_request_pub->publish(*msg); + RCLCPP_INFO( + this->get_logger(), + "[%s] Published end lift session from lift supervisor", + msg->session_id.c_str() + ); curr_request = nullptr; } } @@ -90,7 +104,25 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) { if ((lift_request->destination_floor != msg->current_floor) || (lift_request->door_state != msg->door_state)) - _lift_request_pub->publish(*lift_request); + lift_request->request_time = this->now(); + _lift_request_pub->publish(*lift_request); + RCLCPP_INFO( + this->get_logger(), + "[%s] Published lift request to [%s] from lift supervisor", + msg->session_id.c_str(), lift_request->destination_floor.c_str() + ); + } + else + { + // If there are no active sessions going on, we keep publishing session + // end requests to ensure that the lift is released + LiftRequest request; + request.lift_name = msg->lift_name; + request.destination_floor = msg->current_floor; + request.session_id = msg->session_id; + request.request_time = this->now(); + request.request_type = LiftRequest::REQUEST_END_SESSION; + _lift_request_pub->publish(request); } // For now, we do not need to publish this. diff --git a/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp new file mode 100644 index 000000000..4f07df44d --- /dev/null +++ b/rmf_fleet_adapter/src/mutex_group_supervisor/main.cpp @@ -0,0 +1,286 @@ +/* + * 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 + +#include +#include +#include + +#include +#include + +#include +#include +#include + +struct TimeStamps +{ + rclcpp::Time claim_time; + std::chrono::steady_clock::time_point heartbeat_time; +}; + +using ClaimMap = std::unordered_map; +using MutexGroupRequest = rmf_fleet_msgs::msg::MutexGroupRequest; +using MutexGroupStates = rmf_fleet_msgs::msg::MutexGroupStates; +using MutextGroupAssignment = rmf_fleet_msgs::msg::MutexGroupAssignment; + +const uint64_t Unclaimed = rmf_fleet_adapter::Unclaimed; + +class Node : public rclcpp::Node +{ +public: + Node() + : rclcpp::Node("mutex_group_supervisor") + { + const auto qos = rclcpp::SystemDefaultsQoS() + .reliable() + .transient_local() + .keep_last(100); + + latest_states = rmf_fleet_msgs::build() + .assignments({}); + + request_sub = create_subscription( + rmf_fleet_adapter::MutexGroupRequestTopicName, + qos, + [&](const MutexGroupRequest& request) + { + handle_request(request); + }); + + state_pub = create_publisher( + rmf_fleet_adapter::MutexGroupStatesTopicName, + qos); + + heartbeat_timer = create_wall_timer( + std::chrono::seconds(2), + [&]() { do_heartbeat(); }); + } + + void handle_request(const MutexGroupRequest& request) + { + auto& claims = mutex_groups[request.group]; + if (request.mode == request.MODE_RELEASE) + { + const auto g_it = mutex_groups.find(request.group); + if (g_it != mutex_groups.end()) + { + const auto c_it = g_it->second.find(request.claimant); + if (c_it != g_it->second.end()) + { + if (c_it->second.claim_time <= request.claim_time) + { + g_it->second.erase(c_it); + pick_next(request.group); + state_pub->publish(latest_states); + } + } + } + return; + } + + auto now = std::chrono::steady_clock::now(); + auto claim_time = rclcpp::Time(request.claim_time); + auto timestamps = TimeStamps { request.claim_time, now }; + claims.insert_or_assign(request.claimant, timestamps); + for (const auto& s : latest_states.assignments) + { + if (s.group == request.group && s.claimant != Unclaimed) + { + check_for_conflicts(); + return; + } + } + pick_next(request.group); + } + + void do_heartbeat() + { + const auto now = std::chrono::steady_clock::now(); + // TODO(MXG): Make this timeout configurable + const auto timeout = std::chrono::seconds(10); + for (auto& [group, claims] : mutex_groups) + { + std::vector remove_claims; + for (const auto& [claimant, timestamp] : claims) + { + if (timestamp.heartbeat_time + timeout < now) + { + remove_claims.push_back(claimant); + } + } + + uint64_t current_claimer = Unclaimed; + for (const auto& assignment : latest_states.assignments) + { + if (assignment.group == group) + { + current_claimer = assignment.claimant; + break; + } + } + + bool need_next_pick = false; + for (const auto& remove_claim : remove_claims) + { + if (current_claimer == remove_claim) + { + need_next_pick = true; + } + + claims.erase(remove_claim); + } + + if (need_next_pick) + { + pick_next(group); + } + } + + state_pub->publish(latest_states); + } + + struct ClaimList + { + std::unordered_set groups; + std::optional earliest_time; + + void insert(std::string group, rclcpp::Time time) + { + groups.insert(std::move(group)); + if (!earliest_time.has_value() || time < *earliest_time) + { + earliest_time = time; + } + } + + void normalize( + uint64_t claimant, + std::unordered_map& mutex_groups) const + { + if (!earliest_time.has_value()) + return; + + for (const auto& group : groups) + { + mutex_groups[group][claimant].claim_time = *earliest_time; + } + } + }; + + void check_for_conflicts() + { + std::unordered_map claims; + for (const auto& [group, group_claims] : mutex_groups) + { + for (const auto& [claimant, timestamps] : group_claims) + { + claims[claimant].insert(group, timestamps.claim_time); + } + } + + std::unordered_set normalized_groups; + for (auto i = claims.begin(); i != claims.end(); ++i) + { + auto j = i; + ++j; + for (; j != claims.end(); ++j) + { + const auto& claim_i = i->second; + const auto& claim_j = j->second; + if (claim_i.groups == claim_j.groups && claim_i.groups.size() > 1) + { + claim_i.normalize(i->first, mutex_groups); + claim_j.normalize(j->first, mutex_groups); + + std::stringstream ss; + for (const auto& group : claim_i.groups) + { + normalized_groups.insert(group); + ss << "[" << group << "]"; + } + + RCLCPP_INFO( + get_logger(), + "Resolving mutex conflict between claimants [%lu] and [%lu] which both " + "want the mutex combination %s", + i->first, + j->first, + ss.str().c_str()); + } + } + } + + for (const auto& group : normalized_groups) + pick_next(group); + } + + void pick_next(const std::string& group) + { + const auto& claimants = mutex_groups[group]; + std::optional> earliest; + for (const auto& [claimant, timestamp] : claimants) + { + const auto& t = timestamp.claim_time; + if (!earliest.has_value() || t < earliest->first) + { + earliest = std::make_pair(t, claimant); + } + } + + uint64_t claimant = Unclaimed; + builtin_interfaces::msg::Time claim_time; + if (earliest.has_value()) + { + claim_time = earliest->first; + claimant = earliest->second; + } + bool group_found = false; + for (auto& a : latest_states.assignments) + { + if (a.group == group) + { + a.claimant = claimant; + a.claim_time = claim_time; + group_found = true; + break; + } + } + if (!group_found) + { + latest_states.assignments.push_back( + rmf_fleet_msgs::build() + .group(group) + .claimant(claimant) + .claim_time(claim_time)); + } + } + + std::unordered_map mutex_groups; + rclcpp::Subscription::SharedPtr request_sub; + rclcpp::Publisher::SharedPtr state_pub; + rclcpp::TimerBase::SharedPtr heartbeat_timer; + MutexGroupStates latest_states; +}; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); +} diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp index c580924da..9c205e34c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp @@ -34,6 +34,8 @@ namespace rmf_fleet_adapter { +using PlanIdPtr = std::shared_ptr; + //============================================================================== class LegacyTask : public std::enable_shared_from_this { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index cbda9b141..67d9cc15e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -73,6 +73,10 @@ TaskManagerPtr TaskManager::make( std::optional> broadcast_client, std::weak_ptr fleet_handle) { + std::shared_ptr fleet = fleet_handle.lock(); + if (!fleet) + return nullptr; + auto mgr = TaskManagerPtr( new TaskManager( std::move(context), @@ -96,6 +100,7 @@ TaskManagerPtr TaskManager::make( auto task_id = "emergency_pullover." + self->_context->name() + "." + self->_context->group() + "-" + std::to_string(self->_count_emergency_pullover++); + self->_context->current_task_id(task_id); // TODO(MXG): Consider subscribing to the emergency pullover update self->_emergency_pullover = ActiveTask::start( @@ -115,18 +120,19 @@ TaskManagerPtr TaskManager::make( }); }; - mgr->_emergency_sub = mgr->_context->node()->emergency_notice() + mgr->_emergency_sub = agv::FleetUpdateHandle::Implementation::get(*fleet) + .emergency_obs .observe_on(rxcpp::identity_same_worker(mgr->_context->worker())) .subscribe( - [w = mgr->weak_from_this(), begin_pullover](const auto& msg) + [w = mgr->weak_from_this(), begin_pullover](const bool is_emergency) { if (auto mgr = w.lock()) { - if (mgr->_emergency_active == msg->data) + if (mgr->_emergency_active == is_emergency) return; - mgr->_emergency_active = msg->data; - if (msg->data) + mgr->_emergency_active = is_emergency; + if (is_emergency) { if (mgr->_waiting) { @@ -905,6 +911,20 @@ void TaskManager::enable_responsive_wait(bool value) } } +//============================================================================== +void TaskManager::set_idle_task(rmf_task::ConstRequestFactoryPtr task) +{ + if (_idle_task == task) + return; + + _idle_task = std::move(task); + std::lock_guard guard(_mutex); + if (!_active_task && _queue.empty() && _direct_queue.empty()) + { + _begin_waiting(); + } +} + //============================================================================== void TaskManager::set_queue( const std::vector& assignments) @@ -1268,7 +1288,7 @@ void TaskManager::_begin_next_task() if (_queue.empty() && _direct_queue.empty()) { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); return; @@ -1309,6 +1329,7 @@ void TaskManager::_begin_next_task() id.c_str(), _context->requester_id().c_str()); + _finished_waiting = false; _context->current_task_end_state(assignment.finish_state()); _context->current_task_id(id); _active_task = ActiveTask::start( @@ -1360,7 +1381,7 @@ void TaskManager::_begin_next_task() } else { - if (!_waiting) + if (!_waiting && !_finished_waiting) _begin_waiting(); } @@ -1457,6 +1478,23 @@ std::function TaskManager::_robot_interruption_callback() //============================================================================== void TaskManager::_begin_waiting() { + if (_idle_task) + { + const auto request = _idle_task->make_request(_context->make_get_state()()); + _waiting = ActiveTask::start( + _context->task_activator()->activate( + _context->make_get_state(), + _context->task_parameters(), + *request, + _update_cb(), + _checkpoint_cb(), + _phase_finished_cb(), + _make_resume_from_waiting()), + _context->now()); + _context->current_task_id(request->booking()->id()); + return; + } + if (!_responsive_wait_enabled) return; @@ -1500,6 +1538,7 @@ void TaskManager::_begin_waiting() _update_cb(), _make_resume_from_waiting()), _context->now()); + _context->current_task_id(task_id); } //============================================================================== @@ -1564,6 +1603,7 @@ std::function TaskManager::_make_resume_from_waiting() if (!self) return; + self->_finished_waiting = true; self->_waiting = ActiveTask(); self->_begin_next_task(); }); @@ -2151,6 +2191,7 @@ std::function TaskManager::_task_finished(std::string id) // Publish the final state of the task before destructing it self->_publish_task_state(); self->_active_task = ActiveTask(); + self->_context->current_task_id(std::nullopt); self->_context->worker().schedule( [w = self->weak_from_this()](const auto&) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 1d8759bcc..52de08f81 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -111,6 +111,8 @@ class TaskManager : public std::enable_shared_from_this void enable_responsive_wait(bool value); + void set_idle_task(rmf_task::ConstRequestFactoryPtr task); + /// Set the queue for this task manager with assignments generated from the /// task planner void set_queue(const std::vector& assignments); @@ -308,6 +310,9 @@ class TaskManager : public std::enable_shared_from_this std::weak_ptr _fleet_handle; rmf_task::ConstActivatorPtr _task_activator; ActiveTask _active_task; + /// The task that should be performed when the robot is idle. This takes + /// precedent over responsive waiting. + rmf_task::ConstRequestFactoryPtr _idle_task; bool _responsive_wait_enabled = false; bool _emergency_active = false; std::optional _emergency_pullover_interrupt_token; @@ -327,6 +332,7 @@ class TaskManager : public std::enable_shared_from_this /// will ensure that the agent continues to respond to traffic negotiations so /// it does not become a blocker for other traffic participants. ActiveTask _waiting; + bool _finished_waiting = false; uint16_t _count_waiting = 0; // TODO: Eliminate the need for a mutex by redesigning the use of the task diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index de6eb68e1..2bb577397 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -264,7 +264,7 @@ std::shared_ptr Adapter::add_easy_fleet( if (entry) entry->execute(finder); - const auto* exit = config.graph()->get_lane(i).entry().event(); + const auto* exit = config.graph()->get_lane(i).exit().event(); if (exit) exit->execute(finder); } @@ -367,6 +367,11 @@ std::shared_ptr Adapter::add_easy_fleet( *config.transformations_to_robot_coordinates()); } + for (const auto& [lift, level] : config.lift_emergency_levels()) + { + fleet_handle->set_lift_emergency_level(lift, level); + } + return EasyFullControl::Implementation::make( fleet_handle, config.skip_rotation_commands(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index a60f57043..bc9db015c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -217,6 +218,7 @@ class EasyFullControl::RobotCallbacks::Implementation NavigationRequest navigate; StopRequest stop; ActionExecutor action_executor; + LocalizationRequest localize; }; //============================================================================== @@ -227,7 +229,8 @@ EasyFullControl::RobotCallbacks::RobotCallbacks( : _pimpl(rmf_utils::make_impl(Implementation{ std::move(navigate), std::move(stop), - std::move(action_executor) + std::move(action_executor), + nullptr })) { // Do nothing @@ -252,334 +255,409 @@ auto EasyFullControl::RobotCallbacks::action_executor() const -> ActionExecutor } //============================================================================== -class EasyFullControl::CommandExecution::Implementation +auto EasyFullControl::RobotCallbacks::with_localization( + LocalizationRequest localization) -> RobotCallbacks& +{ + _pimpl->localize = std::move(localization); + return *this; +} + +//============================================================================== +auto EasyFullControl::RobotCallbacks::localize() const -> LocalizationRequest +{ + return _pimpl->localize; +} + +//============================================================================== +class EasyFullControl::CommandExecution::Implementation::Data { public: - struct Data + std::vector waypoints; + std::vector lanes; + std::optional target_location; + std::optional final_orientation; + rmf_traffic::Duration planned_wait_time; + std::optional schedule_override; + std::shared_ptr nav_params; + std::function arrival_estimator; + + void release_stubbornness() { - std::vector waypoints; - std::vector lanes; - std::optional final_orientation; - rmf_traffic::Duration planned_wait_time; - std::optional schedule_override; - std::shared_ptr nav_params; - std::function arrival_estimator; + if (schedule_override.has_value()) + { + schedule_override->release_stubbornness(); + } + } - void release_stubbornness() + void update_location( + const std::shared_ptr& context, + const std::string& map, + Eigen::Vector3d location) + { + if (schedule_override.has_value()) { - if (schedule_override.has_value()) - { - schedule_override->release_stubbornness(); - } + return schedule_override->overridden_update( + context, + map, + location); } - void update_location( - const std::shared_ptr& context, - const std::string& map, - Eigen::Vector3d location) + auto planner = context->planner(); + if (!planner) { - if (schedule_override.has_value()) - { - return schedule_override->overridden_update( - context, - map, - location); - } + RCLCPP_ERROR( + context->node()->get_logger(), + "Planner unavailable for robot [%s], cannot update its location", + context->requester_id().c_str()); + return; + } - auto planner = context->planner(); - if (!planner) + const auto& graph = planner->get_configuration().graph(); + const rmf_traffic::agv::LaneClosure* closures = + context->get_lane_closures(); + std::optional> on_waypoint; + auto p = Eigen::Vector2d(location[0], location[1]); + const double yaw = location[2]; + for (std::size_t wp : waypoints) + { + if (wp >= graph.num_waypoints()) { RCLCPP_ERROR( context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); + "Robot [%s] has a command with a waypoint [%lu] that is outside " + "the range of the graph [%lu]. We will not do a location update.", + context->requester_id().c_str(), + wp, + graph.num_waypoints()); + // Should we also issue a replan command? return; } - const auto& graph = planner->get_configuration().graph(); - const auto& closed_lanes = planner->get_configuration().lane_closures(); - std::optional> on_waypoint; - auto p = Eigen::Vector2d(location[0], location[1]); - const double yaw = location[2]; - for (std::size_t wp : waypoints) + const auto p_wp = graph.get_waypoint(wp).get_location(); + auto dist = (p - p_wp).norm(); + if (dist <= nav_params->max_merge_waypoint_distance) + { + if (!on_waypoint.has_value() || dist < on_waypoint->second) + { + on_waypoint = std::make_pair(wp, dist); + } + } + } + + rmf_traffic::agv::Plan::StartSet starts; + const auto now = rmf_traffic_ros2::convert(context->node()->now()); + if (on_waypoint.has_value()) + { + const auto wp = on_waypoint->first; + starts.push_back(rmf_traffic::agv::Plan::Start(now, wp, yaw, p)); + for (std::size_t lane_id : graph.lanes_from(wp)) { - if (wp >= graph.num_waypoints()) + if (lane_id >= graph.num_lanes()) { RCLCPP_ERROR( context->node()->get_logger(), - "Robot [%s] has a command with a waypoint [%lu] that is outside " - "the range of the graph [%lu]. We will not do a location update.", + "Nav graph for robot [%s] has an invalid lane ID [%lu] leaving " + "vertex [%lu], lane ID range is [%lu]. We will not do a location " + "update.", context->requester_id().c_str(), + lane_id, wp, - graph.num_waypoints()); + graph.num_lanes()); // Should we also issue a replan command? return; } - const auto p_wp = graph.get_waypoint(wp).get_location(); - auto dist = (p - p_wp).norm(); - if (dist <= nav_params->max_merge_waypoint_distance) + if (closures && closures->is_closed(lane_id)) { - if (!on_waypoint.has_value() || dist < on_waypoint->second) - { - on_waypoint = std::make_pair(wp, dist); - } + // Don't use a lane that's closed + continue; } - } - rmf_traffic::agv::Plan::StartSet starts; - const auto now = rmf_traffic_ros2::convert(context->node()->now()); - if (on_waypoint.has_value()) + auto wp_exit = graph.get_lane(lane_id).exit().waypoint_index(); + starts.push_back( + rmf_traffic::agv::Plan::Start(now, wp_exit, yaw, p, lane_id)); + } + } + else + { + std::optional> on_lane; + for (auto lane_id : lanes) { - const auto wp = on_waypoint->first; - starts.push_back(rmf_traffic::agv::Plan::Start(now, wp, yaw, p)); - for (std::size_t lane_id : graph.lanes_from(wp)) + if (lane_id >= graph.num_lanes()) { - if (lane_id >= graph.num_lanes()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Nav graph for robot [%s] has an invalid lane ID [%lu] leaving " - "vertex [%lu], lane ID range is [%lu]. We will not do a location " - "update.", - context->requester_id().c_str(), - lane_id, - wp, - graph.num_lanes()); - // Should we also issue a replan command? - return; - } + RCLCPP_ERROR( + context->node()->get_logger(), + "Robot [%s] has a command with a lane [%lu] that is outside the " + "range of the graph [%lu]. We will not do a location update.", + context->requester_id().c_str(), + lane_id, + graph.num_lanes()); + // Should we also issue a replan command? + return; + } - if (closed_lanes.is_closed(lane_id)) - { - // Don't use a lane that's closed - continue; - } + if (closures && closures->is_closed(lane_id)) + { + continue; + } - auto wp_exit = graph.get_lane(lane_id).exit().waypoint_index(); - starts.push_back( - rmf_traffic::agv::Plan::Start(now, wp_exit, yaw, p, lane_id)); + const auto& lane = graph.get_lane(lane_id); + const auto p0 = + graph.get_waypoint(lane.entry().waypoint_index()).get_location(); + const auto p1 = + graph.get_waypoint(lane.exit().waypoint_index()).get_location(); + const auto lane_length = (p1 - p0).norm(); + double dist_to_lane = 0.0; + if (lane_length < nav_params->min_lane_length) + { + dist_to_lane = std::min( + (p - p0).norm(), + (p - p1).norm()); } - } - else - { - std::optional> on_lane; - for (auto lane_id : lanes) + else { - if (lane_id >= graph.num_lanes()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Robot [%s] has a command with a lane [%lu] that is outside the " - "range of the graph [%lu]. We will not do a location update.", - context->requester_id().c_str(), - lane_id, - graph.num_lanes()); - // Should we also issue a replan command? - return; - } - - if (closed_lanes.is_closed(lane_id)) - { - continue; - } - - const auto& lane = graph.get_lane(lane_id); - const auto p0 = - graph.get_waypoint(lane.entry().waypoint_index()).get_location(); - const auto p1 = - graph.get_waypoint(lane.exit().waypoint_index()).get_location(); - const auto lane_length = (p1 - p0).norm(); const auto lane_u = (p1 - p0)/lane_length; const auto proj = (p - p0).dot(lane_u); if (proj < 0.0 || lane_length < proj) { continue; } - - const auto dist_to_lane = (p - p0 - proj * lane_u).norm(); - if (dist_to_lane <= nav_params->max_merge_lane_distance) - { - if (!on_lane.has_value() || dist_to_lane < on_lane->second) - { - on_lane = std::make_pair(lane_id, dist_to_lane); - } - } + dist_to_lane = (p - p0 - proj * lane_u).norm(); } - if (on_lane.has_value()) + if (dist_to_lane <= nav_params->max_merge_lane_distance) { - const auto lane_id = on_lane->first; - const auto& lane = graph.get_lane(lane_id); - const auto wp0 = lane.entry().waypoint_index(); - const auto wp1 = lane.exit().waypoint_index(); - starts.push_back( - rmf_traffic::agv::Plan::Start(now, wp1, yaw, p, lane_id)); - - if (const auto* reverse_lane = graph.lane_from(wp1, wp0)) + if (!on_lane.has_value() || dist_to_lane < on_lane->second) { - starts.push_back(rmf_traffic::agv::Plan::Start( - now, wp0, yaw, p, reverse_lane->index())); + on_lane = std::make_pair(lane_id, dist_to_lane); } } - else - { - starts = nav_params->compute_plan_starts(graph, map, location, now); - } } - if (!starts.empty()) + if (on_lane.has_value()) { - context->set_location(starts); + const auto lane_id = on_lane->first; + const auto& lane = graph.get_lane(lane_id); + const auto wp0 = lane.entry().waypoint_index(); + const auto wp1 = lane.exit().waypoint_index(); + starts.push_back( + rmf_traffic::agv::Plan::Start(now, wp1, yaw, p, lane_id)); + + if (const auto* reverse_lane = graph.lane_from(wp1, wp0)) + { + starts.push_back(rmf_traffic::agv::Plan::Start( + now, wp0, yaw, p, reverse_lane->index())); + } } - else + } + + if (starts.empty()) + { + starts = nav_params->compute_plan_starts(graph, map, location, now); + } + + if (!starts.empty()) + { + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, graph); + std::cout << ss.str() << std::endl; } - - if (!waypoints.empty()) + context->set_location(starts); + } + else + { + if (context->debug_positions) { - const auto p_final = - graph.get_waypoint(waypoints.back()).get_location(); - const auto distance = (p_final - p).norm(); - double rotation = 0.0; - if (final_orientation.has_value()) + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map << " <" << location.block<2, 1>(0, 0).transpose() + << "> orientation " << location[2] * 180.0 / M_PI << "\n"; + ss << waypoints.size() << " waypoints:"; + for (std::size_t wp : waypoints) { - rotation = - std::fabs(rmf_utils::wrap_to_pi(location[2] - *final_orientation)); - const auto reversible = - planner->get_configuration().vehicle_traits() - .get_differential()->is_reversible(); - if (reversible) - { - const double alternate_orientation = rmf_utils::wrap_to_pi( - *final_orientation + M_PI); - - const double alternate_rotation = std::fabs( - rmf_utils::wrap_to_pi(location[2] - alternate_orientation)); - rotation = std::min(rotation, alternate_rotation); - } + ss << "\n -- " << print_waypoint(wp, graph); + } + ss << lanes.size() << " lanes:"; + for (std::size_t lane : lanes) + { + ss << "\n -- " << print_lane(lane, graph); } - const auto& traits = planner->get_configuration().vehicle_traits(); - const auto v = std::max(traits.linear().get_nominal_velocity(), 0.001); - const auto w = - std::max(traits.rotational().get_nominal_velocity(), 0.001); - const auto t = distance / v + rotation / w; - arrival_estimator( - rmf_traffic::time::from_seconds(t) + planned_wait_time); + std::cout << ss.str() << std::endl; } + context->set_lost(Location { now, map, location }); } - }; - using DataPtr = std::shared_ptr; - - std::weak_ptr w_context; - std::shared_ptr data; - std::function begin; - std::function finisher; - ActivityIdentifierPtr identifier; - void finish() - { - if (auto context = w_context.lock()) + if (target_location.has_value()) { - context->worker().schedule( - [ - context = context, - data = this->data, - identifier = this->identifier, - finisher = this->finisher - ](const auto&) + const auto p_final = *target_location; + const auto distance = (p_final - p).norm(); + double rotation = 0.0; + if (final_orientation.has_value()) + { + rotation = + std::fabs(rmf_utils::wrap_to_pi(location[2] - *final_orientation)); + const auto reversible = + planner->get_configuration().vehicle_traits() + .get_differential()->is_reversible(); + if (reversible) { - if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) - { - // This activity has already finished - return; - } + const double alternate_orientation = rmf_utils::wrap_to_pi( + *final_orientation + M_PI); - // Prevent this activity from doing any further updates - ActivityIdentifier::Implementation::get( - *identifier).update_fn = nullptr; - if (data->schedule_override.has_value()) - { - data->release_stubbornness(); - RCLCPP_INFO( - context->node()->get_logger(), - "Requesting replan for [%s] after finishing a schedule override", - context->requester_id().c_str()); - context->request_replan(); - } - else - { - // Trigger the next step in the sequence - finisher(); - } - }); + const double alternate_rotation = std::fabs( + rmf_utils::wrap_to_pi(location[2] - alternate_orientation)); + rotation = std::min(rotation, alternate_rotation); + } + } + + const auto& traits = planner->get_configuration().vehicle_traits(); + const auto v = std::max(traits.linear().get_nominal_velocity(), 0.001); + const auto w = + std::max(traits.rotational().get_nominal_velocity(), 0.001); + const auto t = distance / v + rotation / w; + arrival_estimator( + rmf_traffic::time::from_seconds(t) + planned_wait_time); } } +}; - Stubbornness override_schedule( - std::string map, - std::vector path, - rmf_traffic::Duration hold) +void EasyFullControl::CommandExecution::Implementation::finish() +{ + if (auto context = w_context.lock()) { - auto stubborn = std::make_shared(); - if (const auto context = w_context.lock()) - { - context->worker().schedule( - [ - context, - stubborn, - data = this->data, - identifier = this->identifier, - map = std::move(map), - path = std::move(path), - hold - ](const auto&) + context->worker().schedule( + [ + context = context, + data = this->data, + identifier = this->identifier, + finisher = this->finisher + ](const auto&) + { + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) { - if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) - { - // Don't do anything because this command is finished - return; - } + // This activity has already finished + return; + } + // Prevent this activity from doing any further updates + ActivityIdentifier::Implementation::get( + *identifier).update_fn = nullptr; + if (data && data->schedule_override.has_value()) + { data->release_stubbornness(); - data->schedule_override = ScheduleOverride::make( - context, map, path, hold, stubborn); - }); - } - - return Stubbornness::Implementation::make(stubborn); + RCLCPP_INFO( + context->node()->get_logger(), + "Requesting replan for [%s] after finishing a schedule override", + context->requester_id().c_str()); + context->request_replan(); + } + else + { + // Trigger the next step in the sequence + finisher(); + } + }); } +} - static CommandExecution make( - const std::shared_ptr& context, - Data data_, - std::function begin) +auto EasyFullControl::CommandExecution::Implementation::override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold) -> Stubbornness +{ + auto stubborn = std::make_shared(); + if (const auto context = w_context.lock()) { - auto data = std::make_shared(data_); - auto update_fn = [w_context = context->weak_from_this(), data]( - const std::string& map, - Eigen::Vector3d location) + context->worker().schedule( + [ + context, + stubborn, + data = this->data, + identifier = this->identifier, + map = std::move(map), + path = std::move(path), + hold + ](const auto&) { - if (auto context = w_context.lock()) + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) { - data->update_location(context, map, location); + // Don't do anything because this command is finished + return; } - }; - auto identifier = ActivityIdentifier::Implementation::make(update_fn); - CommandExecution cmd; - cmd._pimpl = rmf_utils::make_impl( - Implementation{context, data, begin, nullptr, identifier}); - return cmd; + data->release_stubbornness(); + data->schedule_override = ScheduleOverride::make( + context, map, path, hold, stubborn); + }); } - static Implementation& get(CommandExecution& cmd) - { - return *cmd._pimpl; - } -}; + return Stubbornness::Implementation::make(stubborn); +} + +auto EasyFullControl::CommandExecution::Implementation::make( + const std::shared_ptr& context, + Data data_, + std::function begin) -> CommandExecution +{ + auto data = std::make_shared(data_); + auto update_fn = [w_context = context->weak_from_this(), data]( + const std::string& map, + Eigen::Vector3d location) + { + if (auto context = w_context.lock()) + { + data->update_location(context, map, location); + } + }; + auto identifier = ActivityIdentifier::Implementation::make(update_fn); + + CommandExecution cmd; + cmd._pimpl = rmf_utils::make_impl( + Implementation{context, data, begin, nullptr, identifier}); + return cmd; +} + +auto EasyFullControl::CommandExecution::Implementation::make_hold( + const std::shared_ptr& context, + rmf_traffic::Time expected_time, + rmf_traffic::PlanId plan_id, + std::function finisher) -> CommandExecution +{ + auto update_fn = [ + w_context = context->weak_from_this(), + expected_time, + plan_id + ]( + const std::string& map, + Eigen::Vector3d location) + { + const auto context = w_context.lock(); + if (!context) + return; + + const auto delay = context->now() - expected_time; + context->itinerary().cumulative_delay(plan_id, delay); + if (const auto nav_params = context->nav_params()) + { + if (context->debug_positions) + { + std::cout << "Searching for location from " << __FILE__ << "|" << + __LINE__ << std::endl; + } + nav_params->search_for_location(map, location, *context); + } + }; + auto identifier = ActivityIdentifier::Implementation::make(update_fn); + + CommandExecution cmd; + cmd._pimpl = rmf_utils::make_impl( + Implementation{context, nullptr, nullptr, finisher, identifier}); + return cmd; +} //============================================================================== void EasyFullControl::CommandExecution::finished() @@ -631,26 +709,6 @@ EasyFullControl::EasyFullControl() // Do nothing } -//============================================================================== -class EasyFullControl::Destination::Implementation -{ -public: - std::string map; - Eigen::Vector3d position; - std::optional graph_index; - std::optional speed_limit; - std::optional dock = std::nullopt; - - template - static Destination make(Args&&... args) - { - Destination output; - output._pimpl = rmf_utils::make_impl( - Implementation{std::forward(args)...}); - return output; - } -}; - //============================================================================== const std::string& EasyFullControl::Destination::map() const { @@ -681,6 +739,12 @@ std::optional EasyFullControl::Destination::graph_index() const return _pimpl->graph_index; } +//============================================================================== +std::string EasyFullControl::Destination::name() const +{ + return _pimpl->name; +} + //============================================================================== std::optional EasyFullControl::Destination::speed_limit() const { @@ -693,6 +757,13 @@ std::optional EasyFullControl::Destination::dock() const return _pimpl->dock; } +//============================================================================== +rmf_traffic::agv::Graph::LiftPropertiesPtr +EasyFullControl::Destination::inside_lift() const +{ + return _pimpl->lift; +} + //============================================================================== EasyFullControl::Destination::Destination() { @@ -730,6 +801,8 @@ struct ProgressTracker : std::enable_shared_from_this progress->next(); } }; + + const auto begin = current_activity_impl.begin; if (begin) { @@ -794,28 +867,22 @@ class EasyCommandHandle : public RobotCommandHandle, const std::string& map, Eigen::Vector3d position) const { - if (!nav_params->transforms_to_robot_coords) + auto robot_position = nav_params->to_robot_coordinates(map, position); + if (robot_position.has_value()) { - return position; + return *robot_position; } - const auto tf_it = nav_params->transforms_to_robot_coords->find(map); - if (tf_it == nav_params->transforms_to_robot_coords->end()) + if (const auto context = w_context.lock()) { - const auto context = w_context.lock(); - if (context) - { - RCLCPP_WARN( - context->node()->get_logger(), - "[EasyFullControl] Unable to find robot transform for map [%s] for " - "robot [%s]. We will not apply a transform.", - map.c_str(), - context->requester_id().c_str()); - } - return position; + RCLCPP_WARN( + context->node()->get_logger(), + "[EasyFullControl] Unable to find robot transform for map [%s] for " + "robot [%s]. We will not apply a transform.", + map.c_str(), + context->requester_id().c_str()); } - - return tf_it->second.apply(position); + return position; } std::weak_ptr w_context; @@ -863,7 +930,7 @@ void EasyCommandHandle::stop() //============================================================================== void EasyCommandHandle::follow_new_path( - const std::vector& waypoints, + const std::vector& cmd_waypoints, ArrivalEstimator next_arrival_estimator_, RequestCompleted path_finished_callback_) { @@ -879,7 +946,7 @@ void EasyCommandHandle::follow_new_path( context->requester_id().c_str(), context->itinerary().current_plan_id()); - if (waypoints.empty() || + if (cmd_waypoints.empty() || next_arrival_estimator_ == nullptr || path_finished_callback_ == nullptr) { @@ -901,6 +968,18 @@ void EasyCommandHandle::follow_new_path( return; } const auto& graph = planner->get_configuration().graph(); + std::vector waypoints = cmd_waypoints; + + if (waypoints.size() < 2) + { + // This command doesn't actually want us to go anywhere so we will consider + // it completed right away. But in case the robot is doing something else + // right now, we will command it to stop. + stop(); + path_finished_callback_(); + return; + } + std::optional opt_initial_map; for (const auto& wp : waypoints) { @@ -946,7 +1025,8 @@ void EasyCommandHandle::follow_new_path( { for (const auto& l : current_location) { - if (*wp.graph_index() == l.waypoint() && !l.lane().has_value()) + if (nav_params->in_same_stack(*wp.graph_index(), + l.waypoint()) && !l.lane().has_value()) { if (l.location().has_value()) { @@ -978,16 +1058,16 @@ void EasyCommandHandle::follow_new_path( { for (const auto& l : current_location) { - Eigen::Vector2d p_wp; + Eigen::Vector2d p_l; if (l.location().has_value()) { - p_wp = l.location()->block<2, 1>(0, 0); + p_l = *l.location(); } else { - p_wp = graph.get_waypoint(l.waypoint()).get_location(); + p_l = graph.get_waypoint(l.waypoint()).get_location(); } - const double dist = (*l.location() - p_wp).norm(); + const double dist = (wp.position().block<2, 1>(0, 0) - p_l).norm(); if (dist <= nav_params->max_merge_lane_distance) { found_connection = true; @@ -1094,44 +1174,70 @@ void EasyCommandHandle::follow_new_path( while (i2 < waypoints.size()) { const auto& wp2 = waypoints[i2]; - if (wp1.graph_index().has_value() && wp2.graph_index().has_value()) + + const bool midlane_wait = + !wp1.graph_index().has_value() && !wp2.graph_index().has_value(); + const bool same_stack = + wp1.graph_index().has_value() && wp2.graph_index().has_value() + && nav_params->in_same_stack(*wp1.graph_index(), *wp2.graph_index()); + + if (same_stack || midlane_wait) { - if (*wp1.graph_index() == *wp2.graph_index()) + target_index = i2; + target_position = wp2.position(); + if (std::abs(wp1.position()[2] - + wp2.position()[2])*180.0 / M_PI < 1e-2) { - target_index = i2; - target_position = wp2.position(); - if (std::abs(wp1.position()[2] - - wp2.position()[2])*180.0 / M_PI < 1e-2) - { - // The plan had a wait between these points. - planned_wait_time += wp2.time() - wp1.time(); - } - ++i2; - continue; + // The plan had a wait between these points. + planned_wait_time += wp2.time() - wp1.time(); } + ++i2; + continue; } break; } } + rmf_traffic::agv::Graph::LiftPropertiesPtr in_lift; + if (wp1.graph_index().has_value()) + { + in_lift = graph.get_waypoint(*wp1.graph_index()).in_lift(); + } + else + { + for (const auto& lift : graph.all_known_lifts()) + { + if (lift->is_in_lift(target_position.block<2, 1>(0, 0))) + { + in_lift = lift; + break; + } + } + } + const auto command_position = to_robot_coordinates(map, target_position); auto destination = EasyFullControl::Destination::Implementation::make( std::move(map), command_position, wp1.graph_index(), - speed_limit); + nav_params->get_vertex_name(graph, wp1.graph_index()), + speed_limit, + in_lift); + const auto target_p = waypoints.at(target_index).position(); queue.push_back( EasyFullControl::CommandExecution::Implementation::make( context, EasyFullControl::CommandExecution::Implementation::Data{ cmd_wps, cmd_lanes, + target_position.block<2, 1>(0, 0), target_position[2], planned_wait_time, std::nullopt, nav_params, - [next_arrival_estimator_, target_index](rmf_traffic::Duration dt) + [next_arrival_estimator_, target_index, + target_p](rmf_traffic::Duration dt) { next_arrival_estimator_(target_index, dt); } @@ -1260,13 +1366,17 @@ void EasyCommandHandle::dock( const double dist = (p1 - p0).norm(); const auto& traits = planner->get_configuration().vehicle_traits(); const double v = std::max(traits.linear().get_nominal_velocity(), 0.001); - const double dt = dist / v; - const rmf_traffic::Time expected_arrival = - context->now() + rmf_traffic::time::from_seconds(dt); + const auto dt = rmf_traffic::time::from_seconds(dist / v); + const auto& itin = context->itinerary(); + const auto now = context->now(); + const auto initial_delay = itin.cumulative_delay(itin.current_plan_id()) + .value_or(rmf_traffic::Duration(0)); + const rmf_traffic::Time expected_arrival = now + dt - initial_delay; auto data = EasyFullControl::CommandExecution::Implementation::Data{ {i0, i1}, {*found_lane}, + p1, std::nullopt, rmf_traffic::Duration(0), std::nullopt, @@ -1280,11 +1390,23 @@ void EasyCommandHandle::dock( return; } - const rmf_traffic::Time now = context->now(); - const auto updated_arrival = now + dt; - const auto delay = updated_arrival - expected_arrival; - context->itinerary().cumulative_delay( - plan_id, delay, std::chrono::seconds(1)); + context->worker().schedule([ + w = context->weak_from_this(), + expected_arrival, + plan_id, + dt + ](const auto&) + { + const auto context = w.lock(); + if (!context) + return; + + const rmf_traffic::Time now = context->now(); + const auto updated_arrival = now + dt; + const auto delay = updated_arrival - expected_arrival; + context->itinerary().cumulative_delay( + plan_id, delay, std::chrono::seconds(1)); + }); } }; @@ -1304,7 +1426,9 @@ void EasyCommandHandle::dock( wp1.get_map_name(), command_position, i1, + nav_params->get_vertex_name(graph, i1), lane.properties().speed_limit(), + wp1.in_lift(), dock_name_); auto cmd = EasyFullControl::CommandExecution::Implementation::make( @@ -1318,6 +1442,7 @@ void EasyCommandHandle::dock( { handle_nav_request(destination, execution); }); + this->current_progress = ProgressTracker::make( {std::move(cmd)}, std::move(docking_finished_callback_)); @@ -1431,33 +1556,17 @@ void EasyFullControl::EasyRobotUpdateHandle::update( ActivityIdentifier::Implementation::get(*current_activity).update_fn; if (update_fn) { - update_fn(state.map(), position); + update_fn( + state.map(), position); return; } } - auto planner = context->planner(); - if (!planner) + if (context->debug_positions) { - RCLCPP_ERROR( - context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); - return; - } - const auto& graph = planner->get_configuration().graph(); - const auto& nav_params = updater->nav_params; - const auto now = context->now(); - auto starts = - nav_params->compute_plan_starts(graph, state.map(), position, now); - if (!starts.empty()) - { - context->set_location(std::move(starts)); - } - else - { - context->set_lost(Location { now, state.map(), position }); + std::cout << "Searching for location from " << __FILE__ << "|" << __LINE__ << std::endl; } + updater->nav_params->search_for_location(state.map(), position, *context); }); } @@ -1559,6 +1668,7 @@ class EasyFullControl::FleetConfiguration::Implementation double default_max_merge_waypoint_distance; double default_max_merge_lane_distance; double default_min_lane_length; + std::unordered_map lift_emergency_levels; }; //============================================================================== @@ -1612,7 +1722,8 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( default_responsive_wait, std::move(default_max_merge_waypoint_distance), std::move(default_max_merge_lane_distance), - std::move(default_min_lane_length) + std::move(default_min_lane_length), + {} })) { // Do nothing @@ -1893,8 +2004,10 @@ EasyFullControl::FleetConfiguration::from_config_files( rmf_task::ConstRequestFactoryPtr finishing_request; if (finishing_request_string == "charge") { - finishing_request = + auto charge_factory = std::make_shared(); + charge_factory->set_indefinite(true); + finishing_request = charge_factory; std::cout << "Fleet is configured to perform ChargeBattery as finishing request" << std::endl; @@ -1902,7 +2015,8 @@ EasyFullControl::FleetConfiguration::from_config_files( else if (finishing_request_string == "park") { finishing_request = - std::make_shared(); + std::make_shared( + "idle", nullptr); std::cout << "Fleet is configured to perform ParkRobot as finishing request" << std::endl; @@ -2060,7 +2174,7 @@ EasyFullControl::FleetConfiguration::from_config_files( const auto robot_name = robot.first.as(); const YAML::Node& robot_config_yaml = robot.second; - if (!robot_config_yaml.IsMap()) + if (!robot_config_yaml.IsMap() && !robot_config_yaml.IsNull()) { std::cerr << "Entry for [" << robot_name << "] in [robots] dictionary is not " @@ -2068,65 +2182,90 @@ EasyFullControl::FleetConfiguration::from_config_files( return std::nullopt; } - std::string charger; - const YAML::Node& charger_yaml = robot_config_yaml["charger"]; - if (charger_yaml) - { - charger = charger_yaml.as(); - } - else + if (robot_config_yaml.IsMap()) { - std::cerr - << "No [charger] listed for [" << robot_name << "] in the config " - << "file [" << config_file << "]. A robot configuration cannot be " - << "made for the robot." << std::endl; - return std::nullopt; - } + std::vector chargers; + const YAML::Node& charger_yaml = robot_config_yaml["charger"]; + if (charger_yaml) + { + chargers.push_back(charger_yaml.as()); + } - const YAML::Node& responsive_wait_yaml = - robot_config_yaml["responsive_wait"]; - std::optional responsive_wait = std::nullopt; - if (responsive_wait_yaml) - { - responsive_wait = responsive_wait_yaml.as(); - } + const YAML::Node& responsive_wait_yaml = + robot_config_yaml["responsive_wait"]; + std::optional responsive_wait = std::nullopt; + if (responsive_wait_yaml) + { + responsive_wait = responsive_wait_yaml.as(); + } - const YAML::Node& max_merge_waypoint_distance_yaml = - robot_config_yaml["max_merge_waypoint_distance"]; - std::optional max_merge_waypoint_distance = std::nullopt; - if (max_merge_waypoint_distance_yaml) - { - max_merge_waypoint_distance = - max_merge_waypoint_distance_yaml.as(); - } + const YAML::Node& max_merge_waypoint_distance_yaml = + robot_config_yaml["max_merge_waypoint_distance"]; + std::optional max_merge_waypoint_distance = std::nullopt; + if (max_merge_waypoint_distance_yaml) + { + max_merge_waypoint_distance = + max_merge_waypoint_distance_yaml.as(); + } - const YAML::Node& max_merge_lane_distance_yaml = - robot_config_yaml["max_merge_lane_distance"]; - std::optional max_merge_lane_distance = std::nullopt; - if (max_merge_lane_distance_yaml) - { - max_merge_lane_distance = max_merge_lane_distance_yaml.as(); - } + const YAML::Node& max_merge_lane_distance_yaml = + robot_config_yaml["max_merge_lane_distance"]; + std::optional max_merge_lane_distance = std::nullopt; + if (max_merge_lane_distance_yaml) + { + max_merge_lane_distance = max_merge_lane_distance_yaml.as(); + } - const YAML::Node& min_lane_length_yaml = - robot_config_yaml["min_lane_length"]; - std::optional min_lane_length = std::nullopt; - if (min_lane_length_yaml) - { - min_lane_length = min_lane_length_yaml.as(); - } + const YAML::Node& min_lane_length_yaml = + robot_config_yaml["min_lane_length"]; + std::optional min_lane_length = std::nullopt; + if (min_lane_length_yaml) + { + min_lane_length = min_lane_length_yaml.as(); + } - auto config = RobotConfiguration({std::move(charger)}, + auto config = RobotConfiguration( + std::move(chargers), responsive_wait, max_merge_waypoint_distance, max_merge_lane_distance, min_lane_length); - known_robot_configurations.insert_or_assign(robot_name, config); + known_robot_configurations.insert_or_assign(robot_name, config); + } + else + { + auto config = RobotConfiguration({}); + known_robot_configurations.insert_or_assign(robot_name, config); + } } } } - return FleetConfiguration( + std::unordered_map lift_emergency_levels; + const YAML::Node& lift_emergency_levels_yaml = + rmf_fleet["lift_emergency_levels"]; + if (lift_emergency_levels_yaml) + { + if (!lift_emergency_levels_yaml.IsMap()) + { + std::cerr + << "[lift_emergency_levels] element is not a map in the config file [" + << config_file << "] so we cannot parse what level each lift will go " + << "to in an emergency." << std::endl; + return std::nullopt; + } + else + { + for (const auto& lift : lift_emergency_levels_yaml) + { + auto lift_name = lift.first.as(); + auto level_name = lift.second.as(); + lift_emergency_levels[std::move(lift_name)] = std::move(level_name); + } + } + } + + auto config = FleetConfiguration( fleet_name, std::move(tf_dict), std::move(known_robot_configurations), @@ -2150,6 +2289,8 @@ EasyFullControl::FleetConfiguration::from_config_files( default_max_merge_waypoint_distance, default_max_merge_lane_distance, default_min_lane_length); + config.change_lift_emergency_levels() = lift_emergency_levels; + return config; } //============================================================================== @@ -2495,6 +2636,29 @@ void EasyFullControl::FleetConfiguration::set_default_min_lane_length( _pimpl->default_min_lane_length = distance; } +//============================================================================== +void EasyFullControl::FleetConfiguration::set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name) +{ + _pimpl->lift_emergency_levels[std::move(lift_name)] = + std::move(emergency_level_name); +} + +//============================================================================== +std::unordered_map& +EasyFullControl::FleetConfiguration::change_lift_emergency_levels() +{ + return _pimpl->lift_emergency_levels; +} + +//============================================================================== +const std::unordered_map& +EasyFullControl::FleetConfiguration::lift_emergency_levels() const +{ + return _pimpl->lift_emergency_levels; +} + //============================================================================== using EasyCommandHandlePtr = std::shared_ptr; @@ -2525,6 +2689,28 @@ auto EasyFullControl::add_robot( auto easy_updater = EasyRobotUpdateHandle::Implementation::make( robot_nav_params, worker); + LocalizationRequest localization = nullptr; + if (callbacks.localize()) + { + localization = [ + inner = callbacks.localize(), + nav_params = robot_nav_params + ](Destination estimate, CommandExecution execution) + { + auto robot_position = nav_params->to_robot_coordinates( + estimate.map(), + estimate.position()); + if (robot_position.has_value()) + { + auto transformed_estimate = estimate; + Destination::Implementation::get(transformed_estimate) + .position = *robot_position; + + inner(transformed_estimate, execution); + } + }; + } + const auto& fleet_impl = FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle); const auto& planner = *fleet_impl.planner; @@ -2549,36 +2735,39 @@ auto EasyFullControl::add_robot( return nullptr; } - if (configuration.compatible_chargers().size() != 1) + if (configuration.compatible_chargers().size() > 1) { - RCLCPP_ERROR( + RCLCPP_WARN( node->get_logger(), - "Robot [%s] is configured to have %lu chargers, but we require it to " - "have exactly 1. It will not be added to the fleet.", + "Robot [%s] is configured to have %lu chargers, but we will only make " + "use of the first one. Making use of multiple chargers will be added in " + "a future version of RMF.", robot_name.c_str(), configuration.compatible_chargers().size()); - - _pimpl->cmd_handles.erase(robot_name); - return nullptr; } - const auto& charger_name = configuration.compatible_chargers().front(); - const auto* charger_wp = graph.find_waypoint(charger_name); - if (!charger_wp) + std::optional charger_index; + if (!configuration.compatible_chargers().empty()) { - RCLCPP_ERROR( - node->get_logger(), - "Cannot find a waypoint named [%s] in the navigation graph of fleet [%s] " - "needed for the charging point of robot [%s]. We will not add the robot " - "to the fleet.", - charger_name.c_str(), - fleet_name.c_str(), - robot_name.c_str()); + const auto& charger_name = configuration.compatible_chargers().front(); + const auto* charger_wp = graph.find_waypoint(charger_name); + if (!charger_wp) + { + RCLCPP_ERROR( + node->get_logger(), + "Cannot find a waypoint named [%s] in the navigation graph of fleet " + "[%s] needed for the charging point of robot [%s]. We will not add the " + "robot to the fleet.", + charger_name.c_str(), + fleet_name.c_str(), + robot_name.c_str()); + + _pimpl->cmd_handles.erase(robot_name); + return nullptr; + } - _pimpl->cmd_handles.erase(robot_name); - return nullptr; + charger_index = charger_wp->index(); } - const std::size_t charger_index = charger_wp->index(); rmf_traffic::Time now = std::chrono::steady_clock::time_point( std::chrono::nanoseconds(node->now().nanoseconds())); @@ -2614,6 +2803,7 @@ auto EasyFullControl::add_robot( robot_nav_params->min_lane_length = *configuration.min_lane_length(); } + robot_nav_params->find_stacked_vertices(graph); const Eigen::Vector3d position = *position_opt; auto starts = robot_nav_params->compute_plan_starts( graph, initial_state.map(), position, now); @@ -2679,6 +2869,7 @@ auto EasyFullControl::add_robot( fleet_name = fleet_name, charger_index, action_executor = callbacks.action_executor(), + localization = std::move(localization), nav_params = robot_nav_params, enable_responsive_wait ](const RobotUpdateHandlePtr& updater) @@ -2696,6 +2887,7 @@ auto EasyFullControl::add_robot( fleet_name, charger_index, action_executor, + localization, context, nav_params, enable_responsive_wait @@ -2706,7 +2898,11 @@ auto EasyFullControl::add_robot( EasyRobotUpdateHandle::Implementation::get(*easy_updater) .updater->handle = handle; handle->set_action_executor(action_executor); - handle->set_charger_waypoint(charger_index); + context->set_localization(localization); + if (charger_index.has_value()) + { + handle->set_charger_waypoint(*charger_index); + } handle->enable_responsive_wait(enable_responsive_wait); RCLCPP_INFO( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 61365bf03..2e148d22d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -169,18 +169,6 @@ void FleetUpdateHandle::Implementation::dock_summary_cb( return; } -//============================================================================== -std::string FleetUpdateHandle::Implementation::make_error_str( - uint64_t code, std::string category, std::string detail) const -{ - nlohmann::json error; - error["code"] = code; - error["category"] = std::move(category); - error["detail"] = std::move(detail); - - return error.dump(); -} - //============================================================================== std::shared_ptr FleetUpdateHandle::Implementation::convert( const std::string& task_id, @@ -315,6 +303,131 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( return new_request; } +//============================================================================== +class AllocateTasks : public std::enable_shared_from_this +{ +public: + + template + void operator()(const Subscriber& s) + { + std::vector errors; + auto assignments = run(errors); + s.on_next(Result{std::move(assignments), std::move(errors)}); + s.on_completed(); + } + + std::optional run(std::vector& errors) + { + std::string id = ""; + + if (new_request) + { + expect.pending_requests.push_back(new_request); + id = new_request->booking()->id(); + } + + RCLCPP_INFO( + node->get_logger(), + "Planning for [%ld] robot(s) and [%ld] request(s)", + expect.states.size(), + expect.pending_requests.size()); + + // Generate new task assignments + const auto result = task_planner.plan( + rmf_traffic_ros2::convert(node->now()), + expect.states, + expect.pending_requests); + + auto assignments_ptr = std::get_if< + rmf_task::TaskPlanner::Assignments>(&result); + + if (!assignments_ptr) + { + auto error = std::get_if< + rmf_task::TaskPlanner::TaskPlannerError>(&result); + + if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery) + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + + "] due to insufficient initial battery charge for all robots in this " + "fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + else if (*error == + rmf_task::TaskPlanner::TaskPlannerError::limited_capacity) + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "] due to insufficient battery capacity to accommodate one or more " + "requests by any of the robots in this fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + else + { + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "]"; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } + + return std::nullopt; + } + + const auto assignments = *assignments_ptr; + + if (assignments.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "[TaskPlanner] Failed to compute assignments for task_id [%s]", + id.c_str()); + + return std::nullopt; + } + + return assignments; + } + + struct Result + { + std::optional assignments; + std::vector errors; + }; + + AllocateTasks( + rmf_task::ConstRequestPtr new_request_, + Expectations expect_, + rmf_task::TaskPlanner task_planner_, + std::shared_ptr node_) + : new_request(std::move(new_request_)), + expect(std::move(expect_)), + task_planner(std::move(task_planner_)), + node(std::move(node_)) + { + // Do nothing + } + + rmf_task::ConstRequestPtr new_request; + Expectations expect; + rmf_task::TaskPlanner task_planner; + std::shared_ptr node; +}; + + //============================================================================== void FleetUpdateHandle::Implementation::bid_notice_cb( const BidNoticeMsg& bid_notice, @@ -358,7 +471,8 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const auto request_msg = nlohmann::json::parse(bid_notice.request); static const auto request_validator = - nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request); + nlohmann::json_schema::json_validator( + rmf_api_msgs::schemas::task_request); try { @@ -390,104 +504,124 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( }); } - // TODO(MXG): Make the task planning asynchronous. The worker should schedule - // a job to perform the planning which should then spawn a job to save the - // plan result and respond. I started to refactor allocate_tasks(~) to make it - // async, but I will save the remaining effort for later, when there is more - // time to spare. - auto allocation_result = allocate_tasks(new_request, &errors); - if (!allocation_result.has_value()) - return respond({std::nullopt, std::move(errors)}); + calculate_bid = std::make_shared( + new_request, + aggregate_expectations(), + *task_planner, + node); - const auto& assignments = allocation_result.value(); + auto receive_allocation = [w = weak_self, respond, task_id]( + AllocateTasks::Result result) + { + const auto self = w.lock(); + if (!self) + return; - const double cost = task_planner->compute_cost(assignments); + auto allocation_result = result.assignments; + if (!allocation_result.has_value()) + return respond({std::nullopt, std::move(result.errors)}); - // Display computed assignments for debugging - std::stringstream debug_stream; - debug_stream << "Cost: " << cost << std::endl; - for (std::size_t i = 0; i < assignments.size(); ++i) - { - debug_stream << "--Agent: " << i << std::endl; - for (const auto& a : assignments[i]) - { - const auto& s = a.finish_state(); - const double request_seconds = - a.request()->booking()->earliest_start_time().time_since_epoch().count() - /1e9; - const double start_seconds = - a.deployment_time().time_since_epoch().count()/1e9; - const rmf_traffic::Time finish_time = s.time().value(); - const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - debug_stream << " <" << a.request()->booking()->id() << ": " << - request_seconds - << ", " << start_seconds - << ", "<< finish_seconds << ", " << s.battery_soc().value() - << "%>" << std::endl; - } - } - debug_stream << " ----------------------" << std::endl; + const auto& assignments = allocation_result.value(); - RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str()); + const double cost = self->_pimpl->task_planner->compute_cost(assignments); - // Map robot index to name to populate robot_name in BidProposal - std::unordered_map robot_name_map; - std::size_t index = 0; - for (const auto& t : task_managers) - { - robot_name_map.insert({index, t.first->name()}); - ++index; - } + // Display computed assignments for debugging + std::stringstream debug_stream; + debug_stream << "Cost: " << cost << std::endl; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + debug_stream << "--Agent: " << i << std::endl; + for (const auto& a : assignments[i]) + { + const auto& s = a.finish_state(); + const double request_seconds = + a.request()->booking()->earliest_start_time().time_since_epoch(). + count() + /1e9; + const double start_seconds = + a.deployment_time().time_since_epoch().count()/1e9; + const rmf_traffic::Time finish_time = s.time().value(); + const double finish_seconds = finish_time.time_since_epoch().count()/ + 1e9; + debug_stream << " <" << a.request()->booking()->id() << ": " << + request_seconds + << ", " << start_seconds + << ", "<< finish_seconds << ", " << + s.battery_soc().value() + << "%>" << std::endl; + } + } + debug_stream << " ----------------------" << std::endl; - std::optional robot_name; - std::optional finish_time; - index = 0; - for (const auto& agent : assignments) - { - for (const auto& assignment : agent) - { - if (assignment.request()->booking()->id() == task_id) + RCLCPP_DEBUG( + self->_pimpl->node->get_logger(), + "%s", + debug_stream.str().c_str()); + + // Map robot index to name to populate robot_name in BidProposal + std::unordered_map robot_name_map; + std::size_t index = 0; + for (const auto& t : self->_pimpl->task_managers) { - finish_time = assignment.finish_state().time().value(); - if (robot_name_map.find(index) != robot_name_map.end()) - robot_name = robot_name_map[index]; - break; + robot_name_map.insert({index, t.first->name()}); + ++index; } - } - ++index; - } - if (!robot_name.has_value() || !finish_time.has_value()) - { - errors.push_back( - make_error_str( - 13, "Internal bug", - "Failed to find robot_name or finish_time after allocating task. " - "Please report this bug to the RMF developers.")); + std::optional robot_name; + std::optional finish_time; + index = 0; + for (const auto& agent : assignments) + { + for (const auto& assignment : agent) + { + if (assignment.request()->booking()->id() == task_id) + { + finish_time = assignment.finish_state().time().value(); + if (robot_name_map.find(index) != robot_name_map.end()) + robot_name = robot_name_map[index]; + break; + } + } + ++index; + } - return respond({std::nullopt, std::move(errors)}); - } + if (!robot_name.has_value() || !finish_time.has_value()) + { + result.errors.push_back( + make_error_str( + 13, "Internal bug", + "Failed to find robot_name or finish_time after allocating task. " + "Please report this bug to the RMF developers.")); - // Publish BidProposal - respond( - { - rmf_task_ros2::bidding::Response::Proposal{ - name, - *robot_name, - current_assignment_cost, - cost, - *finish_time - }, - std::move(errors) - }); + return respond({std::nullopt, std::move(result.errors)}); + } - RCLCPP_INFO( - node->get_logger(), - "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]", - task_id.c_str(), robot_name->c_str(), cost); + // Publish BidProposal + respond( + { + rmf_task_ros2::bidding::Response::Proposal{ + self->_pimpl->name, + *robot_name, + self->_pimpl->current_assignment_cost, + cost, + *finish_time + }, + std::move(result.errors) + }); + + RCLCPP_INFO( + self->_pimpl->node->get_logger(), + "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]", + task_id.c_str(), robot_name->c_str(), cost); + + // Store assignments in internal map + self->_pimpl->bid_notice_assignments.insert({task_id, assignments}); + }; - // Store assignments in internal map - bid_notice_assignments.insert({task_id, assignments}); + calculate_bid_subscription = rmf_rxcpp::make_job( + calculate_bid) + .observe_on(rxcpp::identity_same_worker(worker)) + .subscribe(receive_allocation); } //============================================================================== @@ -619,7 +753,10 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( // TODO: This replanning is blocking the main thread. Instead, the // replanning should run on a separate worker and then deliver the // result back to the main worker. - const auto replan_results = allocate_tasks(request, &dispatch_ack.errors); + const auto replan_results = AllocateTasks( + nullptr, aggregate_expectations(), *task_planner, node) + .run(dispatch_ack.errors); + if (!replan_results) { std::string error_str = @@ -639,22 +776,24 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( // rxcpp worker. Hence, no new tasks would have started during this // replanning. } - - std::size_t index = 0; - for (auto& t : task_managers) + else { - t.second->set_queue(assignments[index]); - ++index; - } + std::size_t index = 0; + for (auto& t : task_managers) + { + t.second->set_queue(assignments[index]); + ++index; + } - current_assignment_cost = task_planner->compute_cost(assignments); - dispatch_ack.success = true; - dispatch_ack_pub->publish(dispatch_ack); + current_assignment_cost = task_planner->compute_cost(assignments); + dispatch_ack.success = true; + dispatch_ack_pub->publish(dispatch_ack); - RCLCPP_INFO( - node->get_logger(), - "Assignments updated for robots in fleet [%s] to accommodate task_id [%s]", - name.c_str(), task_id.c_str()); + RCLCPP_INFO( + node->get_logger(), + "TaskAssignments updated for robots in fleet [%s] to accommodate task_id [%s]", + name.c_str(), task_id.c_str()); + } } else if (msg->type == DispatchCmdMsg::TYPE_REMOVE) { @@ -681,7 +820,10 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( { // Re-plan assignments while ignoring request for task to be cancelled std::vector errors; - const auto replan_results = allocate_tasks(nullptr, &errors); + const auto replan_results = AllocateTasks( + nullptr, aggregate_expectations(), *task_planner, node) + .run(errors); + if (!replan_results.has_value()) { std::stringstream ss; @@ -718,7 +860,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( RCLCPP_INFO( node->get_logger(), - "Task with task_id [%s] has successfully been cancelled. Assignments " + "Task with task_id [%s] has successfully been cancelled. TaskAssignments " "updated for robots in fleet [%s].", task_id.c_str(), name.c_str()); } @@ -741,7 +883,7 @@ void FleetUpdateHandle::Implementation::dispatch_command_cb( //============================================================================== auto FleetUpdateHandle::Implementation::is_valid_assignments( - Assignments& assignments) const -> bool + TaskAssignments& assignments) const -> bool { std::unordered_set executed_tasks; for (const auto& [context, mgr] : task_managers) @@ -1016,6 +1158,160 @@ void FleetUpdateHandle::Implementation::update_fleet_logs() const } } +//============================================================================== +void FleetUpdateHandle::Implementation::handle_emergency( + const bool is_emergency) +{ + if (is_emergency == emergency_active) + return; + + emergency_active = is_emergency; + if (is_emergency) + { + update_emergency_planner(); + } + + for (const auto& [context, _] : task_managers) + { + context->_set_emergency(is_emergency); + } + emergency_publisher.get_subscriber().on_next(is_emergency); +} + +//============================================================================== +namespace { +class EmergencyLaneCloser : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + void execute(const DoorOpen&) override {} + void execute(const DoorClose&) override {} + void execute(const LiftSessionEnd&) override {} + void execute(const LiftMove&) override {} + void execute(const Wait&) override {} + void execute(const Dock& dock) override {} + void execute(const LiftSessionBegin& info) override + { + lift = info.lift_name(); + enter = true; + } + void execute(const LiftDoorOpen& info) override + { + lift = info.lift_name(); + exit = true; + } + + std::string lift; + bool enter = false; + bool exit = false; +}; +} // anonymous namespace + +//============================================================================== +std::vector find_emergency_lift_closures( + const rmf_traffic::agv::Graph& graph, + const std::unordered_map& emergency_level_for_lift) +{ + std::vector closures; + for (std::size_t i = 0; i < graph.num_lanes(); ++i) + { + EmergencyLaneCloser executor; + const auto& lane = graph.get_lane(i); + if (const auto* event = lane.entry().event()) + event->execute(executor); + + if (const auto* event = lane.exit().event()) + event->execute(executor); + + const auto wp0 = lane.entry().waypoint_index(); + const auto wp1 = lane.exit().waypoint_index(); + if (executor.enter) + { + if (emergency_level_for_lift.count(executor.lift) > 0) + { + closures.push_back(i); + } + } + else if (executor.exit) + { + const auto l_it = emergency_level_for_lift.find(executor.lift); + if (l_it != emergency_level_for_lift.end()) + { + const auto& map = graph.get_waypoint( + lane.exit().waypoint_index()).get_map_name(); + if (l_it->second != map) + { + closures.push_back(i); + } + } + } + } + + return closures; +} + +//============================================================================== +void FleetUpdateHandle::Implementation::update_emergency_planner() +{ + const auto& config = (*planner)->get_configuration(); + const auto lift_closures = find_emergency_lift_closures( + config.graph(), + emergency_level_for_lift); + const auto& normal_closures = config.lane_closures(); + auto emergency_closures = normal_closures; + for (const std::size_t lane : lift_closures) + { + emergency_closures.close(lane); + } + + auto emergency_config = config; + emergency_config.lane_closures(std::move(emergency_closures)); + + *emergency_planner = std::make_shared( + emergency_config, rmf_traffic::agv::Planner::Options(nullptr)); +} + +//============================================================================== +void FleetUpdateHandle::Implementation::update_charging_assignments( + const ChargingAssignments& charging) +{ + if (charging.fleet_name != name) + return; + + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] received new charging assignments", + name.c_str()); + + for (const ChargingAssignment& assignment : charging.assignments) + { + bool found_robot = false; + for (const auto& [context, _] : task_managers) + { + if (context->name() != assignment.robot_name) + continue; + + const rmf_traffic::agv::Graph& graph = context->navigation_graph(); + const auto wp = graph.find_waypoint(assignment.waypoint_name); + if (!wp) + { + RCLCPP_ERROR( + node->get_logger(), + "Cannot change charging waypoint for [%s] to [%s] because it does " + "not exist in the graph", + context->requester_id().c_str(), + assignment.waypoint_name.c_str()); + } + const bool wait_for_charger = assignment.mode == assignment.MODE_WAIT; + context->_set_charging(wp->index(), wait_for_charger); + } + + if (!found_robot) + { + unregistered_charging_assignments[assignment.robot_name] = assignment; + } + } +} + //============================================================================== nlohmann::json_schema::json_validator FleetUpdateHandle::Implementation::make_validator( @@ -1197,106 +1493,6 @@ auto FleetUpdateHandle::Implementation::aggregate_expectations() const return expect; } -//============================================================================== -auto FleetUpdateHandle::Implementation::allocate_tasks( - rmf_task::ConstRequestPtr new_request, - std::vector* errors, - std::optional expectations) const -> std::optional -{ - // Collate robot states, constraints and combine new requestptr with - // requestptr of non-charging tasks in task manager queues - auto expect = expectations.has_value() ? expectations.value() : - aggregate_expectations(); - std::string id = ""; - - if (new_request) - { - expect.pending_requests.push_back(new_request); - id = new_request->booking()->id(); - } - - RCLCPP_INFO( - node->get_logger(), - "Planning for [%ld] robot(s) and [%ld] request(s)", - expect.states.size(), - expect.pending_requests.size()); - - // Generate new task assignments - const auto result = task_planner->plan( - rmf_traffic_ros2::convert(node->now()), - expect.states, - expect.pending_requests); - - auto assignments_ptr = std::get_if< - rmf_task::TaskPlanner::Assignments>(&result); - - if (!assignments_ptr) - { - auto error = std::get_if< - rmf_task::TaskPlanner::TaskPlannerError>(&result); - - if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery) - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id - + "] due to insufficient initial battery charge for all robots in this " - "fleet."; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - else if (*error == - rmf_task::TaskPlanner::TaskPlannerError::limited_capacity) - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id - + "] due to insufficient battery capacity to accommodate one or more " - "requests by any of the robots in this fleet."; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - else - { - std::string error_str = - "[TaskPlanner] Failed to compute assignments for task_id [" + id + "]"; - - RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); - if (errors) - { - errors->push_back( - make_error_str(9, "Not feasible", std::move(error_str))); - } - } - - return std::nullopt; - } - - const auto assignments = *assignments_ptr; - - if (assignments.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id [%s]", - id.c_str()); - - return std::nullopt; - } - - return assignments; -} - //============================================================================== const std::string& FleetUpdateHandle::fleet_name() const { @@ -1355,23 +1551,20 @@ void FleetUpdateHandle::add_robot( rmf_task::State state; state.load_basic(start[0], charger_wp.value(), 1.0); - auto context = std::make_shared( - RobotContext - { - std::move(command), - std::move(start), - std::move(participant), - fleet->_pimpl->mirror, - fleet->_pimpl->planner, - fleet->_pimpl->activation.task, - fleet->_pimpl->task_parameters, - fleet->_pimpl->node, - fleet->_pimpl->worker, - fleet->_pimpl->default_maximum_delay, - state, - fleet->_pimpl->task_planner - } - ); + auto context = RobotContext::make( + std::move(command), + std::move(start), + std::move(participant), + fleet->_pimpl->mirror, + fleet->_pimpl->planner, + fleet->_pimpl->emergency_planner, + fleet->_pimpl->activation.task, + fleet->_pimpl->task_parameters, + fleet->_pimpl->node, + fleet->_pimpl->worker, + fleet->_pimpl->default_maximum_delay, + state, + fleet->_pimpl->task_planner); // We schedule the following operations on the worker to make sure we do not // have a multiple read/write race condition on the FleetUpdateHandle. @@ -1389,6 +1582,10 @@ void FleetUpdateHandle::add_robot( if (!node) return; + if (fleet->_pimpl->emergency_active) + { + context->_set_emergency(true); + } // TODO(MXG): We need to perform this test because we do not currently // support the distributed negotiation in unit test environments. We @@ -1408,21 +1605,11 @@ void FleetUpdateHandle::add_robot( { if (const auto c = w.lock()) { + const auto& graph = c->navigation_graph(); std::stringstream ss; ss << "Failed negotiation for [" << c->requester_id() - << "] with these starts:"; - for (const auto& l : c->location()) - { - ss << "\n -- t:" << l.time().time_since_epoch().count() - << " | wp:" << l.waypoint() << " | ori:" - << l.orientation(); - if (l.location().has_value()) - { - const auto& p = *l.location(); - ss << " | pos:(" << p.x() << ", " << p.y() << ")"; - } - } - ss << "\n -- Fin --"; + << "] with these starts:" + << print_starts(c->location(), graph); std::cout << ss.str() << std::endl; auto& last_time = *last_interrupt_time; @@ -1434,11 +1621,14 @@ void FleetUpdateHandle::add_robot( } last_time = now; - RCLCPP_INFO( - c->node()->get_logger(), - "Requesting replan for [%s] because it failed to negotiate", - c->requester_id().c_str()); - c->request_replan(); + if (!c->is_stubborn()) + { + RCLCPP_INFO( + c->node()->get_logger(), + "Requesting replan for [%s] because it failed to negotiate", + c->requester_id().c_str()); + c->request_replan(); + } } }); context->_set_negotiation_license(std::move(negotiation_license)); @@ -1456,11 +1646,39 @@ void FleetUpdateHandle::add_robot( if (fleet->_pimpl->broadcast_client) broadcast_client = fleet->_pimpl->broadcast_client; - fleet->_pimpl->task_managers.insert({context, - TaskManager::make( - context, - broadcast_client, - std::weak_ptr(fleet))}); + const auto mgr = TaskManager::make( + context, + broadcast_client, + std::weak_ptr(fleet)); + + fleet->_pimpl->task_managers.insert({context, mgr}); + + const auto c_it = fleet->_pimpl + ->unregistered_charging_assignments.find(context->name()); + if (c_it != fleet->_pimpl->unregistered_charging_assignments.end()) + { + const auto& charging = c_it->second; + const auto& graph = context->navigation_graph(); + const auto* wp = graph.find_waypoint(charging.waypoint_name); + if (!wp) + { + RCLCPP_ERROR( + fleet->_pimpl->node->get_logger(), + "Cannot find a waypoing named [%s] for robot [%s], which was " + "requested as its charging point", + charging.waypoint_name.c_str(), + context->requester_id().c_str()); + } + else + { + context->_set_charging( + wp->index(), + charging.mode == charging.MODE_WAIT); + } + fleet->_pimpl->unregistered_charging_assignments.erase(c_it); + } + + mgr->set_idle_task(fleet->_pimpl->idle_task); // -- Calling the handle_cb should always happen last -- if (handle_cb) @@ -1578,16 +1796,12 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) if (!self) return; - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - bool any_changes = false; for (const auto& lane : lane_indices) { - if (current_lane_closures.is_open(lane)) + if (self->_pimpl->closed_lanes.insert(lane).second) { any_changes = true; - break; } } @@ -1602,14 +1816,17 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) for (const auto& lane : lane_indices) { new_lane_closures.close(lane); - // Bookkeeping - self->_pimpl->closed_lanes.insert(lane); } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + if (self->_pimpl->emergency_active) + { + self->_pimpl->update_emergency_planner(); + } + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); self->_pimpl->publish_lane_states(); @@ -1631,16 +1848,17 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) if (!self) return; - const auto& current_lane_closures = - (*self->_pimpl->planner)->get_configuration().lane_closures(); - + // Note that this approach to tracking whether to open a lane will make + // it impossible for an external user to open a lane which has been closed + // by the emergency_level_for_lift behavior. For now this is intentional, + // but in future implementations we may want to allow users to decide if + // that is desirable behavior. bool any_changes = false; for (const auto& lane : lane_indices) { - if (current_lane_closures.is_closed(lane)) + if (self->_pimpl->closed_lanes.erase(lane) > 0) { any_changes = true; - break; } } @@ -1655,19 +1873,31 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) for (const auto& lane : lane_indices) { new_lane_closures.open(lane); - // Bookkeeping - self->_pimpl->closed_lanes.erase(lane); } *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + if (self->_pimpl->emergency_active) + { + self->_pimpl->update_emergency_planner(); + } + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); self->_pimpl->publish_lane_states(); }); } +//============================================================================== +void FleetUpdateHandle::set_lift_emergency_level( + std::string lift_name, + std::string emergency_level_name) +{ + _pimpl->emergency_level_for_lift[std::move(lift_name)] = + std::move(emergency_level_name); +} + //============================================================================== class FleetUpdateHandle::SpeedLimitRequest::Implementation { @@ -2075,7 +2305,7 @@ bool FleetUpdateHandle::set_task_planner_params( double recharge_threshold, double recharge_soc, bool account_for_battery_drain, - rmf_task::ConstRequestFactoryPtr finishing_request) + rmf_task::ConstRequestFactoryPtr idle_task) { if (battery_system && motion_sink && @@ -2101,15 +2331,19 @@ bool FleetUpdateHandle::set_task_planner_params( const rmf_task::TaskPlanner::Options options{ false, nullptr, - finishing_request}; + // The finishing request is no longer handled by the planner, we handle + // it separately as a waiting behavior now. + nullptr}; _pimpl->worker.schedule( - [w = weak_from_this(), task_config, options](const auto&) + [w = weak_from_this(), task_config, options, idle_task](const auto&) { const auto self = w.lock(); if (!self) return; + self->_pimpl->idle_task = idle_task; + // Here we update the task planner in all the RobotContexts. // The TaskManagers rely on the parameters in the task planner for // automatic retreat. Hence, we also update them whenever the @@ -2118,7 +2352,10 @@ bool FleetUpdateHandle::set_task_planner_params( self->_pimpl->name, std::move(task_config), std::move(options)); for (const auto& t : self->_pimpl->task_managers) + { t.first->task_planner(self->_pimpl->task_planner); + t.second->set_idle_task(idle_task); + } }); return true; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 14f5d2ddc..322281326 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -36,6 +36,8 @@ std::shared_ptr Node::make( auto default_qos = rclcpp::SystemDefaultsQoS(); default_qos.keep_last(100); + auto transient_qos = rclcpp::SystemDefaultsQoS() + .reliable().keep_last(100).transient_local(); node->_door_state_obs = node->create_observable( @@ -55,7 +57,7 @@ std::shared_ptr Node::make( node->_lift_request_pub = node->create_publisher( - AdapterLiftRequestTopicName, default_qos); + AdapterLiftRequestTopicName, transient_qos); node->_task_summary_pub = node->create_publisher( @@ -106,6 +108,18 @@ std::shared_ptr Node::make( node->create_publisher( TaskApiResponses, transient_local_qos); + node->_mutex_group_request_pub = + node->create_publisher( + MutexGroupRequestTopicName, transient_local_qos); + + node->_mutex_group_request_obs = + node->create_observable( + MutexGroupRequestTopicName, transient_local_qos); + + node->_mutex_group_states_obs = + node->create_observable( + MutexGroupStatesTopicName, transient_local_qos); + return node; } @@ -229,5 +243,23 @@ auto Node::task_api_response() const -> const ApiResponsePub& return _task_api_response_pub; } +//============================================================================== +auto Node::mutex_group_request() const -> const MutexGroupRequestPub& +{ + return _mutex_group_request_pub; +} + +//============================================================================== +auto Node::mutex_group_request_obs() const -> const MutexGroupRequestObs& +{ + return _mutex_group_request_obs->observe(); +} + +//============================================================================== +auto Node::mutex_group_states() const -> const MutexGroupStatesObs& +{ + return _mutex_group_states_obs->observe(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 0ced9dad7..5331b43e2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -35,6 +35,8 @@ #include #include +#include +#include #include #include @@ -122,6 +124,17 @@ class Node : public rmf_rxcpp::Transport using ApiResponsePub = rclcpp::Publisher::SharedPtr; const ApiResponsePub& task_api_response() const; + using MutexGroupRequest = rmf_fleet_msgs::msg::MutexGroupRequest; + using MutexGroupRequestPub = rclcpp::Publisher::SharedPtr; + const MutexGroupRequestPub& mutex_group_request() const; + + using MutexGroupRequestObs = rxcpp::observable; + const MutexGroupRequestObs& mutex_group_request_obs() const; + + using MutexGroupStates = rmf_fleet_msgs::msg::MutexGroupStates; + using MutexGroupStatesObs = rxcpp::observable; + const MutexGroupStatesObs& mutex_group_states() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -179,6 +192,9 @@ class Node : public rmf_rxcpp::Transport FleetStatePub _fleet_state_pub; Bridge _task_api_request_obs; ApiResponsePub _task_api_response_pub; + MutexGroupRequestPub _mutex_group_request_pub; + Bridge _mutex_group_request_obs; + Bridge _mutex_group_states_obs; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index efc92a5d2..0abfd7165 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -22,10 +22,325 @@ #include #include +#include + +#include namespace rmf_fleet_adapter { namespace agv { +//============================================================================== +void NavParams::search_for_location( + const std::string& map, + Eigen::Vector3d position, + RobotContext& context) +{ + auto planner = context.planner(); + if (!planner) + { + RCLCPP_ERROR( + context.node()->get_logger(), + "Planner unavailable for robot [%s], cannot update its location", + context.requester_id().c_str()); + return; + } + const auto& graph = planner->get_configuration().graph(); + const auto now = context.now(); + auto starts = compute_plan_starts(graph, map, position, now); + if (!starts.empty()) + { + if (context.debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, graph); + std::cout << ss.str() << std::endl; + } + context.set_location(std::move(starts)); + } + else + { + if (context.debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map << " <" << position.block<2, 1>(0, 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; + } + context.set_lost(Location { now, map, position }); + } +} + +//============================================================================== +void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) +{ + for (std::size_t i = 0; i < graph.num_waypoints() - 1; ++i) + { + const auto& wp_i = graph.get_waypoint(i); + const Eigen::Vector2d p_i = wp_i.get_location(); + const std::string& map_i = wp_i.get_map_name(); + for (std::size_t j = i+1; j < graph.num_waypoints(); ++j) + { + const auto& wp_j = graph.get_waypoint(j); + const Eigen::Vector2d p_j = wp_j.get_location(); + const std::string& map_j = wp_j.get_map_name(); + if (map_i != map_j) + { + continue; + } + + const double dist = (p_i - p_j).norm(); + if (dist > max_merge_waypoint_distance) + { + continue; + } + + // stack these waypoints + auto stack_i = stacked_vertices[i]; + auto stack_j = stacked_vertices[j]; + if (!stack_i && !stack_j) + { + // create a new stack + stack_i = std::make_shared>(); + stack_j = stack_i; + } + else if (stack_i && stack_j) + { + if (stack_i != stack_j) + { + for (const std::size_t other : *stack_j) + { + stack_i->insert(other); + stacked_vertices[other] = stack_i; + } + + } + } + else if (!stack_i) + { + stack_i = stack_j; + } + + assert(stack_i); + stack_i->insert(i); + stack_i->insert(j); + stacked_vertices[i] = stack_i; + stacked_vertices[j] = stack_j; + } + } +} + +//============================================================================== +std::string NavParams::get_vertex_name( + const rmf_traffic::agv::Graph& graph, + const std::optional v_opt) const +{ + if (!v_opt.has_value()) + return ""; + + const std::size_t v = v_opt.value(); + if (const std::string* n = graph.get_waypoint(v).name()) + { + return *n; + } + else + { + const auto& stack_it = stacked_vertices.find(v); + if (stack_it != stacked_vertices.end() && stack_it->second) + { + for (const auto& v : *stack_it->second) + { + if (const auto* n = graph.get_waypoint(v).name()) + { + return *n; + } + } + } + } + + return ""; +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::process_locations( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + return _lift_boundary_filter(graph, _descend_stacks(graph, locations)); +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::_descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + std::vector remove; + for (std::size_t i = 0; i < locations.size(); ++i) + { + rmf_traffic::agv::Plan::Start& location = locations[i]; + std::optional waypoint_opt; + if (location.lane().has_value()) + { + const rmf_traffic::agv::Graph::Lane& lane = + graph.get_lane(*location.lane()); + waypoint_opt = lane.entry().waypoint_index(); + } + else + { + waypoint_opt = location.waypoint(); + } + + if (!waypoint_opt.has_value()) + continue; + + const std::size_t original_waypoint = waypoint_opt.value(); + std::size_t waypoint = original_waypoint; + const auto s_it = stacked_vertices.find(waypoint); + VertexStack stack; + if (s_it != stacked_vertices.end()) + stack = s_it->second; + if (!stack) + continue; + + std::unordered_set visited; + bool can_descend = true; + bool has_loop = false; + while (can_descend) + { + can_descend = false; + if (!visited.insert(waypoint).second) + { + // These stacked vertices have a loop so there's no way to find a bottom + // for it. We need to just exit here. + has_loop = true; + break; + } + + for (std::size_t v : *stack) + { + if (graph.lane_from(v, waypoint)) + { + waypoint = v; + can_descend = true; + break; + } + } + } + + if (has_loop) + { + continue; + } + + // Transfer the location estimate over to the waypoint that's at the bottom + // of the vertex stack. + if (waypoint != original_waypoint) + { + bool can_merge = true; + if (const auto r_merge = graph.get_waypoint(waypoint).merge_radius()) + { + if (const auto p_opt = location.location()) + { + const auto p = p_opt.value(); + const auto p_wp = graph.get_waypoint(waypoint).get_location(); + if ((p - p_wp).norm() > r_merge) + { + can_merge = false; + remove.push_back(i); + } + } + } + + if (can_merge) + { + location.lane(std::nullopt); + location.waypoint(waypoint); + } + } + } + + for (auto r_it = remove.rbegin(); r_it != remove.rend(); ++r_it) + { + locations.erase(locations.begin() + *r_it); + } + + return locations; +} + +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::_lift_boundary_filter( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const +{ + const auto r_it = std::remove_if( + locations.begin(), + locations.end(), + [&graph](const rmf_traffic::agv::Plan::Start& location) + { + if (location.lane().has_value()) + { + // If the intention is to move along a lane, then it is okay to keep + // this. If the lane is entering or exiting the lift, then it should + // have the necessary events. + // TODO(@mxgrey): Should we check and make sure that the event is + // actually present? + return false; + } + + if (!location.location().has_value()) + { + // If the robot is perfectly on some waypoint then there is no need to + // filter. + return false; + } + + const Eigen::Vector2d p = location.location().value(); + + const auto robot_inside_lift = [&]() + -> rmf_traffic::agv::Graph::LiftPropertiesPtr + { + for (const auto& lift : graph.all_known_lifts()) + { + // We assume lifts never overlap so we will return the first + // positive hit. + if (lift->is_in_lift(p)) + return lift; + } + + return nullptr; + }(); + + const auto& wp = graph.get_waypoint(location.waypoint()); + const auto lift = wp.in_lift(); + // If the robot's lift status and the waypoint's lift status don't match + // then we should filter this waypoint out. + return wp.in_lift() != robot_inside_lift; + }); + + locations.erase(r_it, locations.end()); + return locations; +} + +//============================================================================== +bool NavParams::in_same_stack( + std::size_t wp0, + std::size_t wp1) const +{ + if (wp0 == wp1) + { + return true; + } + + const auto s_it = stacked_vertices.find(wp0); + if (s_it == stacked_vertices.end()) + return false; + + const auto stack = s_it->second; + if (!stack) + return false; + + return stack->count(wp1); +} + //============================================================================== std::shared_ptr RobotContext::command() { @@ -41,25 +356,43 @@ std::shared_ptr RobotContext::make_updater() //============================================================================== Eigen::Vector3d RobotContext::position() const { - assert(!_location.empty()); - const auto& l = _location.front(); - if (l.location().has_value()) + if (!_location.empty()) { - const Eigen::Vector2d& p = *l.location(); + const auto& l = _location.front(); + if (l.location().has_value()) + { + const Eigen::Vector2d& p = *l.location(); + return {p[0], p[1], l.orientation()}; + } + + const Eigen::Vector2d& p = + navigation_graph().get_waypoint(l.waypoint()).get_location(); return {p[0], p[1], l.orientation()}; } + else if (_lost.has_value() && _lost->location.has_value()) + { + return _lost->location->position; + } - const Eigen::Vector2d& p = - navigation_graph().get_waypoint(l.waypoint()).get_location(); - return {p[0], p[1], l.orientation()}; + throw std::runtime_error( + "No location information is available for [" + requester_id() + "]"); } //============================================================================== const std::string& RobotContext::map() const { - assert(!_location.empty()); - return navigation_graph() - .get_waypoint(_location.front().waypoint()).get_map_name(); + if (!_location.empty()) + { + return navigation_graph() + .get_waypoint(_location.front().waypoint()).get_map_name(); + } + else if (_lost.has_value() && _lost->location.has_value()) + { + return _lost->location->map; + } + + throw std::runtime_error( + "No location information is available for [" + requester_id() + "]"); } //============================================================================== @@ -83,10 +416,21 @@ const rmf_traffic::agv::Plan::StartSet& RobotContext::location() const //============================================================================== void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) { + for (auto& location : location_) + { + location.orientation(rmf_utils::wrap_to_pi(location.orientation())); + } + _location = std::move(location_); filter_closed_lanes(); + if (_location.empty()) { + if (debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST" << + std::endl; + } set_lost(std::nullopt); return; } @@ -142,30 +486,17 @@ void RobotContext::set_lost(std::optional location) //============================================================================== void RobotContext::filter_closed_lanes() { - if (const auto planner = *_planner) + const rmf_traffic::agv::LaneClosure* closures = get_lane_closures(); + if (closures) { - const auto& closures = planner->get_configuration().lane_closures(); for (std::size_t i = 0; i < _location.size(); ) { if (_location[i].lane().has_value()) { - if (closures.is_closed(*_location[i].lane())) + if (closures->is_closed(*_location[i].lane())) { - if (_location.size() > 1) - { - _location.erase(_location.begin() + i); - continue; - } - else - { - RCLCPP_WARN( - node()->get_logger(), - "Robot [%s] is being forced to use closed lane [%lu] because it " - "has not been provided any other feasible lanes to use.", - requester_id().c_str(), - *_location[i].lane()); - return; - } + _location.erase(_location.begin() + i); + continue; } } ++i; @@ -173,6 +504,27 @@ void RobotContext::filter_closed_lanes() } } +//============================================================================== +const rmf_traffic::agv::LaneClosure* RobotContext::get_lane_closures() const +{ + if (_emergency) + { + if (const auto planner = *_emergency_planner) + { + return &planner->get_configuration().lane_closures(); + } + } + else + { + if (const auto planner = *_planner) + { + return &planner->get_configuration().lane_closures(); + } + } + + return nullptr; +} + //============================================================================== rmf_traffic::schedule::Participant& RobotContext::itinerary() { @@ -222,6 +574,12 @@ const std::string& RobotContext::requester_id() const return _requester_id; } +//============================================================================== +rmf_traffic::ParticipantId RobotContext::participant_id() const +{ + return _itinerary.id(); +} + //============================================================================== const rmf_traffic::agv::Graph& RobotContext::navigation_graph() const { @@ -235,6 +593,13 @@ RobotContext::planner() const return *_planner; } +//============================================================================== +const std::shared_ptr& +RobotContext::emergency_planner() const +{ + return *_emergency_planner; +} + //============================================================================== std::shared_ptr RobotContext::nav_params() const { @@ -306,6 +671,13 @@ RobotContext::observe_replan_request() const return _replan_obs; } +//============================================================================== +const rxcpp::observable& +RobotContext::observe_charging_change() const +{ + return _charging_change_obs; +} + //============================================================================== void RobotContext::request_replan() { @@ -400,7 +772,7 @@ std::function RobotContext::make_get_state() rmf_task::State state; state.load_basic( self->_most_recent_valid_location.front(), - self->_charger_wp, + self->_charging_wp, self->_current_battery_soc); state.insert(GetContext{self->shared_from_this()}); @@ -420,10 +792,22 @@ const std::string* RobotContext::current_task_id() const //============================================================================== RobotContext& RobotContext::current_task_id(std::optional id) { + std::unique_lock lock(*_current_task_id_mutex); _current_task_id = std::move(id); + _initial_time_idle_outside_lift = std::nullopt; return *this; } +//============================================================================== +std::string RobotContext::copy_current_task_id() const +{ + std::unique_lock lock(*_current_task_id_mutex); + if (_current_task_id.has_value()) + return _current_task_id.value(); + + return {}; +} + //============================================================================== double RobotContext::current_battery_soc() const { @@ -450,9 +834,15 @@ RobotContext& RobotContext::current_battery_soc(const double battery_soc) } //============================================================================== -std::size_t RobotContext::dedicated_charger_wp() const +std::size_t RobotContext::dedicated_charging_wp() const { - return _charger_wp; + return _charging_wp; +} + +//============================================================================== +bool RobotContext::waiting_for_charger() const +{ + return _waiting_for_charger; } //============================================================================== @@ -600,13 +990,118 @@ const Reporting& RobotContext::reporting() const return _reporting; } +//============================================================================== +bool RobotContext::localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const +{ + if (_localize) + { + _localize(std::move(estimate), std::move(execution)); + return true; + } + + return false; +} + +//============================================================================== +void RobotContext::set_localization( + EasyFullControl::LocalizationRequest localization) +{ + _localize = std::move(localization); +} + +//============================================================================== +const LiftDestination* RobotContext::current_lift_destination() const +{ + return _lift_destination.get(); +} + +//============================================================================== +std::shared_ptr RobotContext::set_lift_destination( + std::string lift_name, + std::string destination_floor, + bool requested_from_inside) +{ + _lift_arrived = false; + _lift_destination = std::make_shared( + LiftDestination{ + std::move(lift_name), + std::move(destination_floor), + requested_from_inside + }); + _initial_time_idle_outside_lift = std::nullopt; + + _publish_lift_destination(); + return _lift_destination; +} + +//============================================================================== +void RobotContext::release_lift() +{ + if (_lift_destination) + { + RCLCPP_INFO( + _node->get_logger(), + "Releasing lift [%s] for [%s]", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + } + _lift_destination = nullptr; + _initial_time_idle_outside_lift = std::nullopt; + _lift_stubbornness = nullptr; + _lift_arrived = false; +} + +//============================================================================== +const std::optional& RobotContext::holding_door() const +{ + return _holding_door; +} + +//============================================================================== +const std::unordered_map& +RobotContext::locked_mutex_groups() const +{ + return _locked_mutex_groups; +} + +//============================================================================== +const rxcpp::observable& RobotContext::request_mutex_groups( + std::unordered_set groups, + rmf_traffic::Time claim_time) +{ + const auto t = rmf_traffic_ros2::convert(claim_time); + for (const auto& group : groups) + { + const auto [it, inserted] = _requesting_mutex_groups.insert({group, t}); + if (!inserted) + { + if (t.nanosec < it->second.nanosec) + it->second = t; + } + } + + _publish_mutex_group_requests(); + return _mutex_group_lock_obs; +} + +//============================================================================== +void RobotContext::retain_mutex_groups( + const std::unordered_set& retain) +{ + _retain_mutex_groups(retain, _requesting_mutex_groups); + _retain_mutex_groups(retain, _locked_mutex_groups); +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, std::vector _initial_location, rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, - std::shared_ptr> planner, + SharedPlanner planner, + SharedPlanner emergency_planner, rmf_task::ConstActivatorPtr activator, rmf_task::ConstParametersPtr parameters, std::shared_ptr node, @@ -619,6 +1114,7 @@ RobotContext::RobotContext( _itinerary(std::move(itinerary)), _schedule(std::move(schedule)), _planner(std::move(planner)), + _emergency_planner(std::move(emergency_planner)), _task_activator(std::move(activator)), _task_parameters(std::move(parameters)), _stubbornness(std::make_shared(0)), @@ -627,7 +1123,7 @@ RobotContext::RobotContext( _maximum_delay(maximum_delay), _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), - _charger_wp(state.dedicated_charging_waypoint().value()), + _charging_wp(state.dedicated_charging_waypoint().value()), _current_task_end_state(state), _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), @@ -639,8 +1135,9 @@ RobotContext::RobotContext( _replan_obs = _replan_publisher.get_observable(); _graph_change_obs = _graph_change_publisher.get_observable(); - + _charging_change_obs = _charging_change_publisher.get_observable(); _battery_soc_obs = _battery_soc_publisher.get_observable(); + _mutex_group_lock_obs = _mutex_group_lock_subject.get_observable(); _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE; _override_status = std::nullopt; @@ -648,6 +1145,96 @@ RobotContext::RobotContext( _action_executor = nullptr; } +//============================================================================== +void RobotContext::schedule_itinerary( + std::shared_ptr plan_id, + rmf_traffic::schedule::Itinerary new_itinerary) +{ + bool scheduled = false; + std::size_t attempts = 0; + while (!scheduled) + { + if (++attempts > 5) + { + std::stringstream ss_sizes; + for (const auto& r : new_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + node()->get_logger(), + "Repeatedly failled attempts to update schedule with an itinerary " + "containing [%lu] routes with sizes %s during LockMutexGroup " + "action for robot [%s]. Last attempted value was [%lu]. We will " + "continue without updating the traffic schedule. This could lead to " + "traffic management problems. Please report this bug to the " + "maintainers of RMF.", + new_itinerary.size(), + ss_sizes.str().c_str(), + requester_id().c_str(), + *plan_id); + break; + } + + scheduled = itinerary().set(*plan_id, new_itinerary); + + if (!scheduled) + { + for (const auto& r : new_itinerary) + { + if (r.trajectory().size() < 2) + { + std::stringstream ss_sizes; + for (const auto& r : new_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + node()->get_logger(), + "Attempting to schedule an itinerary for robot [%s] containing a " + "route that has fewer than 2 waypoints. Routes sizes are %s. " + "We will continue without updating the traffic schedule. This " + "could lead to traffic management problems. Please report this " + "bug to the maintainers of RMF.", + requester_id().c_str(), + ss_sizes.str().c_str()); + return; + } + } + + *plan_id = itinerary().assign_plan_id(); + if (attempts > 1) + { + RCLCPP_ERROR( + node()->get_logger(), + "Invalid plan_id [%lu] when current plan_id is [%lu] for robot [%s] " + "while performing a LockMutexGroup. Please report this bug to an RMF " + "developer.", + *plan_id, + itinerary().current_plan_id(), + requester_id().c_str()); + } + } + } +} + +//============================================================================== +void RobotContext::schedule_hold( + std::shared_ptr plan_id, + rmf_traffic::Time time, + rmf_traffic::Duration wait, + Eigen::Vector3d position, + const std::string& map) +{ + rmf_traffic::Trajectory hold; + const auto zero = Eigen::Vector3d::Zero(); + hold.insert(time, position, zero); + hold.insert(time + wait, position, zero); + schedule_itinerary(plan_id, {rmf_traffic::Route(map, std::move(hold))}); +} + //============================================================================== void RobotContext::_set_task_manager(std::shared_ptr mgr) { @@ -660,5 +1247,351 @@ void RobotContext::_set_negotiation_license(std::shared_ptr license) _negotiation_license = std::move(license); } +//============================================================================== +void RobotContext::_set_emergency(bool value) +{ + _emergency = value; + if (_emergency) + { + filter_closed_lanes(); + } +} + +//============================================================================== +void RobotContext::_set_charging(std::size_t wp, bool waiting_for_charger) +{ + _charging_wp = wp; + _waiting_for_charger = waiting_for_charger; + _charging_change_publisher.get_subscriber().on_next(Empty{}); +} + +//============================================================================== +void RobotContext::_hold_door(std::string door_name) +{ + _holding_door = std::move(door_name); +} + +//============================================================================== +void RobotContext::_release_door(const std::string& door_name) +{ + if (_holding_door.has_value() && *_holding_door == door_name) + _holding_door = std::nullopt; +} + +//============================================================================== +void RobotContext::_check_lift_state( + const rmf_lift_msgs::msg::LiftState& state) +{ + if (_lift_destination.use_count() < 2) + { + if (_lift_destination && !_lift_destination->requested_from_inside) + { + // The lift destination reference count dropping to one while the lift + // destination request is on the outside means the task that prompted the + // lift usage was cancelled while the robot was outside of the lift. + // Therefore we should release the usage of the lift. + release_lift(); + } + else if (_lift_destination && !_current_task_id.has_value()) + { + const Eigen::Vector2d p = position().block<2, 1>(0, 0); + const auto& graph = navigation_graph(); + const auto found_lift = + graph.find_known_lift(_lift_destination->lift_name); + + bool inside_lift = false; + if (found_lift) + { + inside_lift = found_lift->is_in_lift(p); + } + + if (inside_lift) + { + _initial_time_idle_outside_lift = std::nullopt; + } + else + { + const auto now = std::chrono::steady_clock::now(); + if (_initial_time_idle_outside_lift.has_value()) + { + const auto lapse = now - *_initial_time_idle_outside_lift; + if (lapse > std::chrono::seconds(2)) + { + RCLCPP_INFO( + _node->get_logger(), + "Releasing lift [%s] for robot [%s] because it has remained idle " + "outside of the lift.", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + } + release_lift(); + } + else + { + _initial_time_idle_outside_lift = now; + } + } + } + } + + if (state.session_id == requester_id()) + { + if (!_lift_destination || _lift_destination->lift_name != state.lift_name) + { + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = state.lift_name; + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; + msg.session_id = requester_id(); + _node->lift_request()->publish(msg); + } + + if (_lift_destination && _lift_destination->lift_name == state.lift_name) + { + if (!_lift_stubbornness) + { + // Be a stubborn negotiator while using the lift + _lift_stubbornness = be_stubborn(); + } + + _lift_arrived = + _lift_destination->destination_floor == state.current_floor; + } + } + else if (_lift_destination && _lift_destination->lift_name == state.lift_name) + { + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting to begin a session with lift [%s] but the lift is " + "currently held by [%s]", + _requester_id.c_str(), + _lift_destination->lift_name.c_str(), + state.session_id.c_str()); + } + + _publish_lift_destination(); +} + +//============================================================================== +void RobotContext::_publish_lift_destination() +{ + if (!_lift_destination) + { + return; + } + + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = _lift_destination->lift_name; + msg.destination_floor = _lift_destination->destination_floor; + msg.session_id = requester_id(); + msg.request_time = _node->now(); + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_AGV_MODE; + msg.door_state = rmf_lift_msgs::msg::LiftRequest::DOOR_OPEN; + + _node->lift_request()->publish(msg); +} + +//============================================================================== +void RobotContext::_check_door_supervisor( + const rmf_door_msgs::msg::SupervisorHeartbeat& state) +{ + const auto now = std::chrono::steady_clock::now(); + const auto dt = std::chrono::seconds(10); + if (_last_active_task_time + dt < now) + { + // Do not hold a door if a robot is idle for more than 10 seconds + _holding_door = std::nullopt; + } + + for (const auto& door : state.all_sessions) + { + for (const auto& session : door.sessions) + { + if (session.requester_id == _requester_id) + { + if (!_holding_door.has_value() || *_holding_door != door.door_name) + { + // We should not be holding this door open + _node->door_request()->publish( + rmf_door_msgs::build() + .request_time(_node->now()) + .requester_id(_requester_id) + .door_name(door.door_name) + .requested_mode( + rmf_door_msgs::build() + .value(rmf_door_msgs::msg::DoorMode::MODE_CLOSED))); + } + } + } + } +} + +//============================================================================== +void RobotContext::_check_mutex_groups( + const rmf_fleet_msgs::msg::MutexGroupStates& states) +{ + // Make sure to release any mutex groups that this robot is not trying to + // lock right now. + for (const auto& assignment : states.assignments) + { + if (assignment.claimant != participant_id()) + { + if (_requesting_mutex_groups.count(assignment.group) > 0) + { + if (assignment.claimant == (uint64_t)(-1)) + { + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting for the mutex superviser to receive its request " + "for mutex group [%s]", + _requester_id.c_str(), + assignment.group.c_str()); + } + else + { + std::string holder; + if (const auto p = _schedule->get_participant(assignment.claimant)) + { + holder = p->owner() + "/" + p->name(); + } + else + { + holder = "unknown participant #" + + std::to_string(assignment.claimant); + } + + RCLCPP_INFO( + _node->get_logger(), + "[%s] is waiting to lock mutex group [%s] but that mutex is " + "currently held by [%s]", + _requester_id.c_str(), + assignment.group.c_str(), + holder.c_str()); + } + } + + continue; + } + + if (_requesting_mutex_groups.count(assignment.group) > 0) + { + _requesting_mutex_groups.erase(assignment.group); + _locked_mutex_groups[assignment.group] = assignment.claim_time; + _mutex_group_lock_subject.get_subscriber().on_next(assignment.group); + } + else if (_locked_mutex_groups.count(assignment.group) == 0) + { + // This assignment does not match either the requested nor any currently + // locked mutex groups. This is an error so let's release it. + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(assignment.group) + .claimant(participant_id()) + .claim_time(assignment.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); + } + } +} + +void RobotContext::_retain_mutex_groups( + const std::unordered_set& retain, + std::unordered_map& groups) +{ + std::vector release; + for (const auto& [name, time] : groups) + { + if (retain.count(name) == 0) + { + release.push_back(MutexGroupData{name, time}); + } + } + + for (const auto& data : release) + { + _release_mutex_group(data); + _locked_mutex_groups.erase(data.name); + } +} + +//============================================================================== +void RobotContext::_release_mutex_group(const MutexGroupData& data) const +{ + if (data.name.empty()) + { + return; + } + + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimant(participant_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_RELEASE)); +} + +//============================================================================== +void RobotContext::_publish_mutex_group_requests() +{ + const auto now = std::chrono::steady_clock::now(); + if (_current_task_id.has_value()) + { + _last_active_task_time = now; + } + else + { + if (_last_active_task_time + std::chrono::seconds(10) < now) + { + // The robot has been idle for 10 seconds. It should not be keeping a + // mutex locked; a task probably ended wrongly. + if (!_requesting_mutex_groups.empty() + || !_locked_mutex_groups.empty()) + { + auto warning = [&](const std::string& name) + { + RCLCPP_ERROR( + _node->get_logger(), + "Forcibly releasing mutex group [%s] requested by robot [%s] " + "because the robot has been idle for an excessive amount of time.", + name.c_str(), + requester_id().c_str()); + }; + + for (const auto& [name, time] : _requesting_mutex_groups) + { + warning(name); + _release_mutex_group(MutexGroupData{name, time}); + } + _requesting_mutex_groups.clear(); + + for (const auto& [name, time] : _locked_mutex_groups) + { + warning(name); + _release_mutex_group(MutexGroupData{name, time}); + } + _locked_mutex_groups.clear(); + } + } + } + + auto publish = [&](const MutexGroupData& data) + { + _node->mutex_group_request()->publish( + rmf_fleet_msgs::build() + .group(data.name) + .claimant(participant_id()) + .claim_time(data.claim_time) + .mode(rmf_fleet_msgs::msg::MutexGroupRequest::MODE_LOCK)); + }; + + for (const auto& [name, time] : _requesting_mutex_groups) + { + publish(MutexGroupData{name, time}); + } + + for (const auto& [name, time] : _locked_mutex_groups) + { + publish(MutexGroupData{name, time}); + } +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index bafd331de..9c82be580 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,12 +37,15 @@ #include #include +#include #include #include "Node.hpp" #include "../Reporting.hpp" +#include + namespace rmf_fleet_adapter { // Forward declaration @@ -49,7 +53,191 @@ class TaskManager; namespace agv { +class RobotContext; using TransformDictionary = std::unordered_map; +using SharedPlanner = std::shared_ptr< + std::shared_ptr>; +using Destination = EasyFullControl::Destination; +using VertexStack = std::shared_ptr>; +using TimeMsg = builtin_interfaces::msg::Time; + +//============================================================================== +class EventPrinter : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + EventPrinter() + { + // Do nothing + } + + void execute(const DoorOpen& e) override + { + text = "DoorOpen " + e.name(); + } + + void execute(const DoorClose& e) override + { + text = "DoorClose " + e.name(); + } + + void execute(const LiftSessionBegin& e) override + { + text = "LiftSessionBegin " + e.lift_name(); + } + + void execute(const LiftDoorOpen& e) override + { + text = "LiftDoorOpen " + e.lift_name(); + } + + void execute(const LiftSessionEnd& e) override + { + text = "LiftSessionEnd " + e.lift_name(); + } + + void execute(const LiftMove& e) override + { + text = "LiftMove " + e.lift_name(); + } + + void execute(const Wait&) override + { + text = "Wait"; + } + + void execute(const Dock& dock) override + { + text = "Dock " + dock.dock_name(); + } + + std::string text; +}; + +//============================================================================== +inline std::string print_waypoint( + const std::size_t i_wp, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + const rmf_traffic::agv::Graph::Waypoint& wp = graph.get_waypoint(i_wp); + + ss << wp.get_map_name() << " <" << wp.get_location().transpose() << "> [" + << wp.name_or_index() << "]"; + return ss.str(); +} + +//============================================================================== +inline std::string print_plan_waypoint( + const rmf_traffic::agv::Plan::Waypoint& wp, + const rmf_traffic::agv::Graph& graph, + const rmf_traffic::Time t0 = rmf_traffic::Time(rmf_traffic::Duration(0))) +{ + std::stringstream ss; + ss << "t=" << rmf_traffic::time::to_seconds(wp.time() - t0); + if (wp.graph_index().has_value()) + ss << " #" << *wp.graph_index(); + ss << " <" << wp.position().transpose() + << "> yaw=" << wp.position()[2] * 180.0 / M_PI; + if (wp.event()) + { + EventPrinter event; + wp.event()->execute(event); + ss << " event [" << event.text << "]"; + } + + if (wp.graph_index().has_value()) + { + const auto& m = graph.get_waypoint(*wp.graph_index()).in_mutex_group(); + if (!m.empty()) + { + ss << " initial mutex [" << m << "]"; + } + } + + ss << " approach lanes:"; + for (const auto& l : wp.approach_lanes()) + { + ss << " " << l; + const auto& lane = graph.get_lane(l); + const auto& m = lane.properties().in_mutex_group(); + if (!m.empty()) + { + ss << " [" << m << "]"; + } + } + + return ss.str(); +} + +//============================================================================== +inline std::string print_lane_node( + const rmf_traffic::agv::Graph::Lane::Node& node, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + EventPrinter event; + if (node.event()) + node.event()->execute(event); + + ss << "{ " << print_waypoint(node.waypoint_index(), graph); + if (!event.text.empty()) + ss << " event " << event.text; + ss << " }"; + + return ss.str(); +} + +//============================================================================== +inline std::string print_lane( + const std::size_t i_lane, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + const auto& lane = graph.get_lane(i_lane); + ss << "lane " << i_lane << ": " << print_lane_node(lane.entry(), graph) + << " -> " << print_lane_node(lane.exit(), graph); + return ss.str(); +} + +//============================================================================== +inline std::string print_starts( + const rmf_traffic::agv::Plan::StartSet& starts, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + for (const rmf_traffic::agv::Plan::Start& l : starts) + { + ss << "\n -- "; + if (l.lane().has_value()) + { + ss << print_lane(*l.lane(), graph); + + const auto& lane = graph.get_lane(*l.lane()); + if (l.waypoint() != lane.exit().waypoint_index()) + { + ss << " !! MISMATCH BETWEEN KEY WAYPOINT AND LANE EXIT: key " + << l.waypoint() << " vs exit " << lane.exit().waypoint_index(); + } + } + else + { + ss << print_waypoint(l.waypoint(), graph); + } + + if (l.location().has_value()) + { + ss << " | location <" << l.location()->transpose() << ">"; + } + else + { + ss << " | on waypoint"; + } + + ss << " | orientation " << l.orientation() * 180.0 / M_PI; + } + + return ss.str(); +} //============================================================================== struct NavParams @@ -81,6 +269,27 @@ struct NavParams return tf_it->second.apply_inverse(position); } + std::optional to_robot_coordinates( + const std::string& map, + Eigen::Vector3d position) + { + if (!transforms_to_robot_coords) + return position; + + const auto tf_it = transforms_to_robot_coords->find(map); + if (tf_it == transforms_to_robot_coords->end()) + { + return std::nullopt; + } + + return tf_it->second.apply(position); + } + + void search_for_location( + const std::string& map, + Eigen::Vector3d position, + RobotContext& context); + rmf_traffic::agv::Plan::StartSet compute_plan_starts( const rmf_traffic::agv::Graph& graph, const std::string& map_name, @@ -99,11 +308,35 @@ struct NavParams min_lane_length); if (!starts.empty()) - return starts; + return process_locations(graph, starts); } return {}; } + + std::unordered_map stacked_vertices; + + void find_stacked_vertices(const rmf_traffic::agv::Graph& graph); + + std::string get_vertex_name( + const rmf_traffic::agv::Graph& graph, + std::optional v) const; + + rmf_traffic::agv::Plan::StartSet process_locations( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + rmf_traffic::agv::Plan::StartSet _descend_stacks( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + // If one of the locations is associated with a lift vertex, filter it out if + // the actual of the robot is outside the dimensions of the lift. + rmf_traffic::agv::Plan::StartSet _lift_boundary_filter( + const rmf_traffic::agv::Graph& graph, + rmf_traffic::agv::Plan::StartSet locations) const; + + bool in_same_stack(std::size_t wp0, std::size_t wp1) const; }; //============================================================================== @@ -114,6 +347,7 @@ struct Location Eigen::Vector3d position; }; +//============================================================================== /// Store position information when the robot is lost, i.e. it has diverged too /// far from its navigation graph. struct Lost @@ -126,6 +360,36 @@ struct Lost std::unique_ptr ticket; }; +//============================================================================== +struct LiftDestination +{ + std::string lift_name; + std::string destination_floor; + bool requested_from_inside; + + inline bool matches( + const std::string& desired_lift_name, + const std::string& desired_floor) const + { + return desired_lift_name == lift_name && desired_floor == destination_floor; + } +}; + +//============================================================================== +struct MutexGroupData +{ + std::string name; + TimeMsg claim_time; +}; + +//============================================================================== +struct MutexGroupSwitch +{ + std::string from; + std::string to; + std::function accept; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -172,6 +436,9 @@ class RobotContext /// plan starts are using closed lanes. void filter_closed_lanes(); + /// Get the current lane closures for the robot + const rmf_traffic::agv::LaneClosure* get_lane_closures() const; + /// Get a mutable reference to the schedule of this robot rmf_traffic::schedule::Participant& itinerary(); @@ -198,12 +465,20 @@ class RobotContext /// Get the requester ID to use for this robot when sending requests const std::string& requester_id() const; + /// Get the traffic participant ID for this robot + rmf_traffic::ParticipantId participant_id() const; + /// Get the navigation graph used by this robot const rmf_traffic::agv::Graph& navigation_graph() const; - /// Get a mutable reference to the planner for this robot + /// Get an immutable reference to the planner for this robot const std::shared_ptr& planner() const; + /// Get an immutable reference to the planner to use for this robot during + /// emergencies (fire alarm). + const std::shared_ptr& emergency_planner() + const; + /// Get the navigation params for this robot, if it has any. This will only be /// available for EasyFullControl robots. std::shared_ptr nav_params() const; @@ -228,8 +503,12 @@ class RobotContext bool is_stubborn() const; struct Empty {}; + /// Use this to get notified when const rxcpp::observable& observe_replan_request() const; + /// Use this to get notified when the charging policy changes for this robot. + const rxcpp::observable& observe_charging_change() const; + /// Request this robot to replan void request_replan(); @@ -286,6 +565,10 @@ class RobotContext /// being performed. RobotContext& current_task_id(std::optional id); + /// Get a string copy of the current task ID of the robot, or an empty string + /// if the robot is not performing any task + std::string copy_current_task_id() const; + /// Get the current battery state of charge double current_battery_soc() const; @@ -293,7 +576,16 @@ class RobotContext /// publishes the battery soc via _battery_soc_publisher. RobotContext& current_battery_soc(const double battery_soc); - std::size_t dedicated_charger_wp() const; + /// The waypoint dedicated to this robot that it should go to if it needs to + /// charge. This may be a waypoint that has a charger or it may be a parking + /// spot where the robot should wait until a charger becomes available. Use + /// waiting_for_charger() to determine which. + std::size_t dedicated_charging_wp() const; + + /// When the robot reaches its dedicated_charging_wp, is it there to wait for + /// a charger to become available (true) or to actually perform the charging + /// (false)? + bool waiting_for_charger() const; // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; @@ -352,6 +644,51 @@ class RobotContext const Reporting& reporting() const; + /// Tell the robot to localize near here + bool localize( + EasyFullControl::Destination estimate, + EasyFullControl::CommandExecution execution) const; + + /// Set the callback for localizing the robot + void set_localization(EasyFullControl::LocalizationRequest localization); + + /// Get the current lift destination request for this robot + const LiftDestination* current_lift_destination() const; + + /// Ask for a certain lift to go to a certain destination and open the doors + std::shared_ptr set_lift_destination( + std::string lift_name, + std::string destination_floor, + bool requested_from_inside); + + /// Indicate that the lift is no longer needed + void release_lift(); + + /// Check if a door is being held + const std::optional& holding_door() const; + + /// What mutex group is currently being locked. + const std::unordered_map& locked_mutex_groups() const; + + /// Set the mutex group that this robot needs to lock. + const rxcpp::observable& request_mutex_groups( + std::unordered_set groups, + rmf_traffic::Time claim_time); + + /// Retain only the mutex groups listed in the set. Release all others. + void retain_mutex_groups(const std::unordered_set& groups); + + void schedule_itinerary( + std::shared_ptr plan_id, + rmf_traffic::schedule::Itinerary itinerary); + + void schedule_hold( + std::shared_ptr plan_id, + rmf_traffic::Time time, + rmf_traffic::Duration wait, + Eigen::Vector3d position, + const std::string& map); + /// Set the task manager for this robot. This should only be called in the /// TaskManager::make function. void _set_task_manager(std::shared_ptr mgr); @@ -360,12 +697,84 @@ class RobotContext /// the FleetUpdateHandle::add_robot function. void _set_negotiation_license(std::shared_ptr license); + /// Use this to trigger emergencies on/off. This should only be called in the + /// FleetUpdateHandle::handle_emergency function. + void _set_emergency(bool value); + + /// Use this to change the charging settings for the robot and trigger a + /// charger change notification. + void _set_charging(std::size_t wp, bool waiting_for_charger); + + /// Request a door to stay open. This should only be used by DoorOpen. + void _hold_door(std::string door_name); + + /// Release a door. This should only be used by DoorClose + void _release_door(const std::string& door_name); + + template + static std::shared_ptr make(Args&&... args) + { + auto context = std::shared_ptr( + new RobotContext(std::forward(args)...)); + + context->_lift_subscription = context->_node->lift_state() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_lift_state(*msg); + }); + + context->_door_subscription = context->_node->door_supervisor() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_door_supervisor(*msg); + }); + + context->_mutex_group_sanity_check = context->_node->mutex_group_states() + .observe_on(rxcpp::identity_same_worker(context->_worker)) + .subscribe([w = context->weak_from_this()](const auto& msg) + { + const auto self = w.lock(); + if (!self) + return; + + self->_check_mutex_groups(*msg); + }); + + context->_mutex_group_heartbeat = context->_node->try_create_wall_timer( + std::chrono::seconds(2), + [w = context->weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_publish_mutex_group_requests(); + }); + + return context; + } + + bool debug_positions = false; + +private: + RobotContext( std::shared_ptr command_handle, std::vector _initial_location, rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, - std::shared_ptr> planner, + SharedPlanner planner, + SharedPlanner emergency_planner, rmf_task::ConstActivatorPtr activator, rmf_task::ConstParametersPtr parameters, std::shared_ptr node, @@ -374,14 +783,13 @@ class RobotContext rmf_task::State state, std::shared_ptr task_planner); -private: - std::weak_ptr _command_handle; std::vector _location; std::vector _most_recent_valid_location; rmf_traffic::schedule::Participant _itinerary; std::shared_ptr _schedule; - std::shared_ptr> _planner; + SharedPlanner _planner; + SharedPlanner _emergency_planner; std::shared_ptr _nav_params; rmf_task::ConstActivatorPtr _task_activator; rmf_task::ConstParametersPtr _task_parameters; @@ -393,6 +801,9 @@ class RobotContext rxcpp::subjects::subject _replan_publisher; rxcpp::observable _replan_obs; + rxcpp::subjects::subject _charging_change_publisher; + rxcpp::observable _charging_change_obs; + rxcpp::subjects::subject _graph_change_publisher; rxcpp::observable _graph_change_obs; @@ -405,17 +816,24 @@ class RobotContext /// Always call the current_battery_soc() setter to set a new value double _current_battery_soc = 1.0; - std::size_t _charger_wp; + std::size_t _charging_wp; + /// When the robot reaches its _charging_wp, is there to wait for a charger + /// (true) or to actually charge (false)? + bool _waiting_for_charger; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; rmf_task::State _current_task_end_state; std::optional _current_task_id; + std::unique_ptr _current_task_id_mutex = + std::make_unique(); std::shared_ptr _task_planner; std::weak_ptr _task_manager; RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); bool _commissioned = true; + bool _emergency = false; + EasyFullControl::LocalizationRequest _localize; // Mode value for RobotMode message uint32_t _current_mode; @@ -425,6 +843,34 @@ class RobotContext Reporting _reporting; /// Keep track of a lost robot std::optional _lost; + + void _check_lift_state(const rmf_lift_msgs::msg::LiftState& state); + void _publish_lift_destination(); + std::shared_ptr _lift_destination; + rmf_rxcpp::subscription_guard _lift_subscription; + std::optional + _initial_time_idle_outside_lift; + std::shared_ptr _lift_stubbornness; + bool _lift_arrived = false; + + void _check_door_supervisor( + const rmf_door_msgs::msg::SupervisorHeartbeat& hb); + std::optional _holding_door; + rmf_rxcpp::subscription_guard _door_subscription; + + void _check_mutex_groups(const rmf_fleet_msgs::msg::MutexGroupStates& states); + void _retain_mutex_groups( + const std::unordered_set& retain, + std::unordered_map& _groups); + void _release_mutex_group(const MutexGroupData& data) const; + void _publish_mutex_group_requests(); + std::unordered_map _requesting_mutex_groups; + std::unordered_map _locked_mutex_groups; + rxcpp::subjects::subject _mutex_group_lock_subject; + rxcpp::observable _mutex_group_lock_obs; + rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; + rmf_rxcpp::subscription_guard _mutex_group_sanity_check; + std::chrono::steady_clock::time_point _last_active_task_time; }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 1a617ebe9..46fbb44f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -77,11 +77,19 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, waypoint, orientation](const auto&) { - context->set_location({ + rmf_traffic::agv::Plan::StartSet starts = { rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, orientation) - }); + }; + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } + context->set_location(starts); }); } } @@ -117,6 +125,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(starts)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(std::move(starts)); }); } @@ -132,11 +147,19 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, position, waypoint](const auto&) { - context->set_location({ + rmf_traffic::agv::Plan::StartSet starts = { rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, position[2], Eigen::Vector2d(position.block<2, 1>(0, 0))) - }); + }; + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } + context->set_location(std::move(starts)); }); } } @@ -167,8 +190,16 @@ void RobotUpdateHandle::update_position( position[0], position[1], position[2], map_name.c_str()); context->worker().schedule( - [context, now, map_name, position](const auto&) + [context, now, map_name, position]( + const auto&) { + if (context->debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map_name << " <" << position.block<2, 1>(0, + 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; + } context->set_lost(Location { now, map_name, position }); }); return; @@ -177,6 +208,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(starts)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(std::move(starts)); }); } @@ -191,6 +229,13 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(position)](const auto&) { + if (context->debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, context->navigation_graph()); + std::cout << ss.str() << std::endl; + } context->set_location(starts); }); } @@ -202,14 +247,20 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( { if (const auto context = _pimpl->get_context()) { - auto end_state = context->current_task_end_state(); - end_state.dedicated_charging_waypoint(charger_wp); - context->current_task_end_state(end_state); - RCLCPP_INFO( - context->node()->get_logger(), - "Charger waypoint for robot [%s] set to index [%ld]", - context->name().c_str(), - charger_wp); + context->worker().schedule([charger_wp, w = context->weak_from_this()]( + const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + self->_set_charging(charger_wp, true); + RCLCPP_INFO( + self->node()->get_logger(), + "Charger waypoint for robot [%s] set to index [%ld]", + self->requester_id().c_str(), + charger_wp); + }); } return *this; @@ -331,6 +382,15 @@ RobotUpdateHandle::maximum_delay() const return rmf_utils::nullopt; } +//============================================================================== +const std::string RobotUpdateHandle::current_task_id() const +{ + if (const auto context = _pimpl->get_context()) + return context->copy_current_task_id(); + + return {}; +} + //============================================================================== void RobotUpdateHandle::set_action_executor( RobotUpdateHandle::ActionExecutor action_executor) @@ -638,6 +698,28 @@ void RobotUpdateHandle::enable_responsive_wait(bool value) }); } +//============================================================================== +void RobotUpdateHandle::release_lift() +{ + const auto context = _pimpl->get_context(); + if (!context) + return; + + context->worker().schedule( + [context](const auto&) + { + if (const auto* lift = context->current_lift_destination()) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Releasing lift [%s] for [%s] because of a user request", + lift->lift_name.c_str(), + context->requester_id().c_str()); + } + context->release_lift(); + }); +} + //============================================================================== RobotUpdateHandle::RobotUpdateHandle() { @@ -813,6 +895,17 @@ void RobotUpdateHandle::Unstable::set_lift_entry_watchdog( } } +//============================================================================== +void RobotUpdateHandle::Unstable::debug_positions(bool on) +{ + if (const auto context = _pimpl->get_context()) + { + // No need to worry about race conditions or data races here because this is + // a mostly inconsequential bool + context->debug_positions = on; + } +} + //============================================================================== void RobotUpdateHandle::ActionExecution::update_remaining_time( rmf_traffic::Duration remaining_time_estimate) @@ -1090,26 +1183,12 @@ void ScheduleOverride::overridden_update( } } - auto planner = context->planner(); - if (!planner) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context->requester_id().c_str()); - return; - } - - const auto& graph = planner->get_configuration().graph(); - auto starts = nav_params->compute_plan_starts(graph, map, location, now); - if (!starts.empty()) - { - context->set_location(std::move(starts)); - } - else + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::cout << "Search for location from " << __FILE__ << "|" << __LINE__ << + std::endl; } + nav_params->search_for_location(map, location, *context); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp index 80f77221d..ad8b2ea90 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp @@ -99,7 +99,7 @@ Eigen::Vector3d Transformation::apply( Eigen::Vector3d Transformation::apply_inverse( const Eigen::Vector3d& position) const { - Eigen::Vector2d p = _pimpl->transform * position.block<2, 1>(0, 0); + Eigen::Vector2d p = _pimpl->transform_inv * position.block<2, 1>(0, 0); double angle = rmf_utils::wrap_to_pi(position[2] - _pimpl->rotation); return Eigen::Vector3d(p[0], p[1], angle); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp index 6d3c9c896..f56291c12 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -22,6 +22,8 @@ namespace rmf_fleet_adapter { namespace agv { +class RobotContext; + class EasyCommandHandle; using EasyCommandHandlePtr = std::shared_ptr; @@ -67,5 +69,68 @@ class EasyFullControl::Implementation } }; +//============================================================================== +class EasyFullControl::Destination::Implementation +{ +public: + std::string map; + Eigen::Vector3d position; + std::optional graph_index; + std::string name; + std::optional speed_limit; + rmf_traffic::agv::Graph::LiftPropertiesPtr lift; + std::optional dock = std::nullopt; + + template + static Destination make(Args&&... args) + { + Destination output; + output._pimpl = rmf_utils::make_impl( + Implementation{std::forward(args)...}); + return output; + } + + static Implementation& get(Destination& self) + { + return *self._pimpl; + } +}; + +class EasyFullControl::CommandExecution::Implementation +{ +public: + struct Data; + using DataPtr = std::shared_ptr; + + std::weak_ptr w_context; + std::shared_ptr data; + std::function begin; + std::function finisher; + ActivityIdentifierPtr identifier; + + void finish(); + + Stubbornness override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold); + + static CommandExecution make( + const std::shared_ptr& context, + Data data_, + std::function begin); + + static CommandExecution make_hold( + const std::shared_ptr& context, + rmf_traffic::Time expected_time, + rmf_traffic::PlanId plan_id, + std::function finisher); + + static Implementation& get(CommandExecution& cmd) + { + return *cmd._pimpl; + } +}; + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 17bc2c2b1..22369f3ff 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -232,6 +233,18 @@ class ParticipantFactoryRos2 : public ParticipantFactory rmf_traffic_ros2::schedule::WriterPtr _writer; }; +//============================================================================== +struct Expectations +{ + std::vector states; + std::vector pending_requests; +}; + +//============================================================================== +// Map task id to pair of +using TaskAssignments = rmf_task::TaskPlanner::Assignments; +class AllocateTasks; + //============================================================================== class FleetUpdateHandle::Implementation { @@ -239,7 +252,7 @@ class FleetUpdateHandle::Implementation std::weak_ptr weak_self; std::string name; - std::shared_ptr> planner; + SharedPlanner planner; std::shared_ptr node; rxcpp::schedulers::worker worker; std::shared_ptr writer; @@ -259,6 +272,7 @@ class FleetUpdateHandle::Implementation rmf_task::BinaryPriorityScheme::make_cost_calculator(); std::shared_ptr task_parameters = nullptr; std::shared_ptr task_planner = nullptr; + rmf_task::ConstRequestFactoryPtr idle_task = nullptr; rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); @@ -277,8 +291,23 @@ class FleetUpdateHandle::Implementation rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr; rclcpp::TimerBase::SharedPtr memory_trim_timer = nullptr; - // Map task id to pair of - using Assignments = rmf_task::TaskPlanner::Assignments; + rxcpp::subscription emergency_sub; + rxcpp::subjects::subject emergency_publisher; + rxcpp::observable emergency_obs; + bool emergency_active = false; + // When an emergency (fire alarm) is active, this map says which level each + // lift will "home" to (if any). + std::unordered_map emergency_level_for_lift; + SharedPlanner emergency_planner; + + rclcpp::Subscription::SharedPtr + charging_assignments_sub = nullptr; + using ChargingAssignments = rmf_fleet_msgs::msg::ChargingAssignments; + using ChargingAssignment = rmf_fleet_msgs::msg::ChargingAssignment; + // Keep track of charging assignments for robots that have not been registered + // yet. + std::unordered_map + unregistered_charging_assignments; using DockParamMap = std::unordered_map< @@ -299,7 +328,7 @@ class FleetUpdateHandle::Implementation double current_assignment_cost = 0.0; // Map to store task id with assignments for BidNotice - std::unordered_map bid_notice_assignments = {}; + std::unordered_map bid_notice_assignments = {}; using BidNoticeMsg = rmf_task_msgs::msg::BidNotice; @@ -325,6 +354,9 @@ class FleetUpdateHandle::Implementation std::unordered_map speed_limited_lanes = {}; std::unordered_set closed_lanes = {}; + std::shared_ptr calculate_bid; + rmf_rxcpp::subscription_guard calculate_bid_subscription; + template static std::shared_ptr make(Args&&... args) { @@ -334,6 +366,21 @@ class FleetUpdateHandle::Implementation handle->_pimpl->add_standard_tasks(); + handle->_pimpl->emergency_obs = + handle->_pimpl->emergency_publisher.get_observable(); + handle->_pimpl->emergency_sub = handle->_pimpl->node->emergency_notice() + .observe_on(rxcpp::identity_same_worker(handle->_pimpl->worker)) + .subscribe( + [w = handle->weak_from_this()](const auto& msg) + { + if (const auto self = w.lock()) + { + self->_pimpl->handle_emergency(msg->data); + } + }); + handle->_pimpl->emergency_planner = + std::make_shared>(nullptr); + // TODO(MXG): This is a very crude implementation. We create a dummy set of // task planner parameters to stand in until the user sets the task planner // parameters. We'll distribute this shared_ptr to the robot contexts and @@ -533,6 +580,17 @@ class FleetUpdateHandle::Implementation } }; + handle->_pimpl->charging_assignments_sub = + handle->_pimpl->node->create_subscription< + rmf_fleet_msgs::msg::ChargingAssignments>( + ChargingAssignmentsTopicName, + reliable_transient_qos, + [w = handle->weak_from_this()](const ChargingAssignments& assignments) + { + if (const auto self = w.lock()) + self->_pimpl->update_charging_assignments(assignments); + }); + handle->_pimpl->deserialization.event->add( "perform_action", validator, deserializer); @@ -562,25 +620,11 @@ class FleetUpdateHandle::Implementation std::optional get_nearest_charger( const rmf_traffic::agv::Planner::Start& start); - struct Expectations - { - std::vector states; - std::vector pending_requests; - }; - Expectations aggregate_expectations() const; - /// Generate task assignments for a collection of task requests comprising of - /// task requests currently in TaskManager queues while optionally including a - /// new request and while optionally ignoring a specific request. - std::optional allocate_tasks( - rmf_task::ConstRequestPtr new_request = nullptr, - std::vector* errors = nullptr, - std::optional expectations = std::nullopt) const; - /// Helper function to check if assignments are valid. An assignment set is /// invalid if one of the assignments has already begun execution. - bool is_valid_assignments(Assignments& assignments) const; + bool is_valid_assignments(TaskAssignments& assignments) const; void publish_fleet_state_topic() const; @@ -590,21 +634,34 @@ class FleetUpdateHandle::Implementation void update_fleet_state() const; void update_fleet_logs() const; + void handle_emergency(bool is_emergency); + void update_emergency_planner(); + + void update_charging_assignments(const ChargingAssignments& assignments); nlohmann::json_schema::json_validator make_validator( const nlohmann::json& schema) const; void add_standard_tasks(); - std::string make_error_str( - uint64_t code, std::string category, std::string detail) const; - std::shared_ptr convert( const std::string& task_id, const nlohmann::json& request_msg, std::vector& errors) const; }; +//============================================================================== +inline std::string make_error_str( + uint64_t code, std::string category, std::string detail) +{ + nlohmann::json error; + error["code"] = code; + error["category"] = std::move(category); + error["detail"] = std::move(detail); + + return error.dump(); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index e8c8b24f4..b95dc85dc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -32,6 +32,7 @@ namespace rmf_fleet_adapter { namespace agv { +//============================================================================== class TriggerOnce { public: @@ -192,22 +193,12 @@ class RobotUpdateHandle::ActionExecution::Implementation if (nav_params) { - const auto& planner = context->planner(); - if (!planner) - return; - - const auto now = context->now(); - const auto& graph = planner->get_configuration().graph(); - auto starts = nav_params->compute_plan_starts( - graph, map, location, now); - if (!starts.empty()) - { - context->set_location(starts); - } - else + if (context->debug_positions) { - context->set_lost(Location { now, map, location }); + std::cout << "Searching for location from " << __FILE__ << "|" << + __LINE__ << std::endl; } + nav_params->search_for_location(map, location, *context); } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp index 9ab822af2..eca4e50b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp @@ -19,9 +19,13 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { +using LiftPropertiesPtr = rmf_traffic::agv::Graph::LiftPropertiesPtr; + //============================================================================== rmf_traffic::agv::Graph parse_graph( const std::string& graph_file, @@ -33,6 +37,76 @@ rmf_traffic::agv::Graph parse_graph( throw std::runtime_error("Failed to load graph file [" + graph_file + "]"); } + rmf_traffic::agv::Graph graph; + bool has_lifts = false; + const YAML::Node lifts_yaml = graph_config["lifts"]; + if (!lifts_yaml) + { + std::cout << "Your navigation graph does not provide lift information. " + << + "This may cause problems with behaviors around lifts. Please consider " + << + "regenerating your navigration graph with the latest version of " + << "rmf_building_map_tools (from the rmf_traffic_editor repo)." + << std::endl; + } + else + { + has_lifts = true; + for (const auto& lift : lifts_yaml) + { + const std::string& name = lift.first.as(); + const YAML::Node& properties_yaml = lift.second; + + const YAML::Node& position_yaml = properties_yaml["position"]; + const Eigen::Vector2d location( + position_yaml[0].as(), + position_yaml[1].as()); + const double orientation = position_yaml[2].as(); + + const YAML::Node& dims_yaml = properties_yaml["dims"]; + const Eigen::Vector2d dimensions( + dims_yaml[0].as(), + dims_yaml[1].as()); + + graph.set_known_lift(rmf_traffic::agv::Graph::LiftProperties( + name, location, orientation, dimensions)); + } + } + + const YAML::Node doors_yaml = graph_config["doors"]; + if (!doors_yaml) + { + std::cout << "Your navigation graph does not provide door information. " + << + "This may cause problems with behaviors around doors. Please consider " + << + "regenerating your navigration graph with the latest version of " + << "rmf_building_map_tools (from the rmf_traffic_editor repo)." + << std::endl; + } + else + { + for (const auto& door : doors_yaml) + { + const std::string& name = door.first.as(); + const YAML::Node properties_yaml = door.second; + const YAML::Node& endpoints_yaml = properties_yaml["endpoints"]; + std::string map = properties_yaml["map"].as(); + + const YAML::Node& p0_yaml = endpoints_yaml[0]; + const auto p0 = Eigen::Vector2d( + p0_yaml[0].as(), p0_yaml[1].as()); + + const YAML::Node& p1_yaml = endpoints_yaml[1]; + const auto p1 = Eigen::Vector2d( + p1_yaml[0].as(), p1_yaml[1].as()); + + graph.set_known_door( + rmf_traffic::agv::Graph::DoorProperties(name, p0, p1, std::move(map))); + } + } + const YAML::Node levels = graph_config["levels"]; if (!levels) { @@ -56,9 +130,9 @@ rmf_traffic::agv::Graph parse_graph( using Lane = rmf_traffic::agv::Graph::Lane; using Event = Lane::Event; - rmf_traffic::agv::Graph graph; std::unordered_map> wps_of_lift; std::unordered_map lift_of_wp; + std::unordered_map stacked_vertex; std::size_t vnum = 0; // To increment lane endpoint ids for (const auto& level : levels) @@ -125,6 +199,12 @@ rmf_traffic::agv::Graph parse_graph( wp.set_charger(true); } + const YAML::Node& mutex_yaml = options["mutex"]; + if (mutex_yaml) + { + wp.set_in_mutex_group(mutex_yaml.as()); + } + const YAML::Node& lift_option = options["lift"]; if (lift_option) { @@ -133,14 +213,31 @@ rmf_traffic::agv::Graph parse_graph( { wps_of_lift[lift_name].push_back(wp.index()); lift_of_wp[wp.index()] = lift_name; + if (has_lifts) + { + const auto lift = graph.find_known_lift(lift_name); + if (!lift) + { + throw std::runtime_error( + "Lift properties for [" + lift_name + "] were not provided " + "even though it is used by a vertex. This suggests that your " + "nav graph was not generated correctly."); + } + wp.set_in_lift(lift); + } } } + + const YAML::Node& merge_radius_option = options["merge_radius"]; + if (merge_radius_option) + { + wp.set_merge_radius(merge_radius_option.as()); + } } const YAML::Node& lanes = level.second["lanes"]; for (const auto& lane : lanes) { - ConstraintPtr constraint = nullptr; const YAML::Node& options = lane[2]; @@ -273,24 +370,36 @@ rmf_traffic::agv::Graph parse_graph( if (const YAML::Node docking_option = options["dock_name"]) { const std::string dock_name = docking_option.as(); - const rmf_traffic::Duration duration = std::chrono::seconds(5); - if (entry_event) + if (!dock_name.empty()) { - // Add a waypoint and a lane leading to it for the dock maneuver - // to be done after the entry event - const auto entry_wp = graph.get_waypoint(begin); - auto& dock_wp = graph.add_waypoint(map_name, entry_wp.get_location()); - - graph.add_lane( - {begin, entry_event}, - {dock_wp.index(), rmf_utils::clone_ptr()}); - - // First lane from start -> dock, second lane from dock -> end - begin = dock_wp.index(); - - vnum_temp++; + const rmf_traffic::Duration duration = std::chrono::seconds(5); + if (entry_event) + { + // Add a waypoint and a lane leading to it for the dock maneuver + // to be done after the entry event + const auto entry_wp = graph.get_waypoint(begin); + auto& dock_wp = + graph.add_waypoint(map_name, entry_wp.get_location()); + dock_wp.set_in_mutex_group(entry_wp.in_mutex_group()); + dock_wp.set_merge_radius(0.0); + + graph.add_lane( + {begin, entry_event}, + {dock_wp.index(), rmf_utils::clone_ptr()}); + stacked_vertex.insert({begin, dock_wp.index()}); + + if (const auto lift = graph.get_waypoint(begin).in_lift()) + { + dock_wp.set_in_lift(lift); + } + + // First lane from start -> dock, second lane from dock -> end + begin = dock_wp.index(); + + vnum_temp++; + } + entry_event = Event::make(Lane::Dock(dock_name, duration)); } - entry_event = Event::make(Lane::Dock(dock_name, duration)); } auto& graph_lane = graph.add_lane( @@ -303,14 +412,21 @@ rmf_traffic::agv::Graph parse_graph( if (speed_limit > 0.0) graph_lane.properties().speed_limit(speed_limit); } + + if (const YAML::Node mutex_yaml = options["mutex"]) + { + graph_lane.properties() + .set_in_mutex_group(mutex_yaml.as()); + } } vnum += vnum_temp; } for (const auto& lift : wps_of_lift) { + double largest_dist = 0.0; const auto& wps = lift.second; - for (std::size_t i = 0; i < wps.size()-1; i++) + for (std::size_t i = 0; i < wps.size()-1; ++i) { rmf_utils::clone_ptr entry_event; rmf_utils::clone_ptr exit_event; @@ -327,6 +443,46 @@ rmf_traffic::agv::Graph parse_graph( graph.add_lane( {wps[i+1], entry_event}, {wps[i], exit_event}); + + const auto pi = graph.get_waypoint(wps[i]).get_location(); + for (std::size_t j = i+1; j < wps.size(); ++j) + { + const auto pj = graph.get_waypoint(wps[j]).get_location(); + const auto dist = (pj - pi).norm(); + if (dist > largest_dist) + largest_dist = dist; + } + } + + if (largest_dist > 0.1) + { + throw std::runtime_error( + "Bad vertical alignment for the waypoints in lift [" + lift.first + + "]. Largest variation is " + std::to_string(largest_dist)); + } + + Eigen::Vector2d lift_center = Eigen::Vector2d::Zero(); + double weight = 0.0; + for (const auto wp : wps) + { + lift_center += graph.get_waypoint(wp).get_location(); + weight += 1.0; + } + + if (weight > 0.0) + { + lift_center /= weight; + for (const auto wp : wps) + { + graph.get_waypoint(wp).set_location(lift_center); + const auto s_it = stacked_vertex.find(wp); + if (s_it != stacked_vertex.end()) + { + std::cout << "Also shifting stacked vertex " << s_it->first << ":" << + s_it->second << std::endl; + graph.get_waypoint(s_it->second).set_location(lift_center); + } + } } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index d0e19cc16..1be09febc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -345,7 +345,6 @@ void MockAdapter::dispatch_task( // NOTE: although the current adapter supports multiple fleets. The test // here assumses using a single fleet for each adapter - bool accepted = false; auto bid = rmf_task_msgs::build() .request(request.dump()) .task_id(task_id) @@ -353,27 +352,31 @@ void MockAdapter::dispatch_task( fimpl.bid_notice_cb( bid, - [&accepted](const rmf_task_ros2::bidding::Response& response) + [fimpl = &fimpl, + task_id](const rmf_task_ros2::bidding::Response& response) { - accepted = response.proposal.has_value(); + if (response.proposal.has_value()) + { + fimpl->worker.schedule([fimpl, task_id](const auto&) + { + rmf_task_msgs::msg::DispatchCommand req; + req.task_id = task_id; + req.fleet_name = fimpl->name; + req.type = req.TYPE_AWARD; + fimpl->dispatch_command_cb( + std::make_shared(req)); + std::cout << "Fleet [" << fimpl->name << + "] accepted the task request" + << std::endl; + }); + } + else + { + std::cout << "Fleet [" << fimpl->name << + "] rejected the task request" + << std::endl; + } }); - - if (accepted) - { - rmf_task_msgs::msg::DispatchCommand req; - req.task_id = task_id; - req.fleet_name = fimpl.name; - req.type = req.TYPE_AWARD; - fimpl.dispatch_command_cb( - std::make_shared(req)); - std::cout << "Fleet [" << fimpl.name << "] accepted the task request" - << std::endl; - } - else - { - std::cout << "Fleet [" << fimpl.name << "] rejected the task request" - << std::endl; - } } }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp index d64f29ef2..222df2d6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -243,7 +243,8 @@ void EmergencyPullover::Active::_find_plan() _state->update_log().info("Searching for an emergency pullover"); _find_pullover_service = std::make_shared( - _context->planner(), _context->location(), _context->schedule()->snapshot(), + _context->emergency_planner(), _context->location(), + _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile()); _pullover_subscription = @@ -335,9 +336,38 @@ void EmergencyPullover::Active::_execute_plan( if (_is_interrupted) return; + if (plan.get_itinerary().empty() || plan.get_waypoints().empty()) + { + _state->update_status(Status::Completed); + _state->update_log().info( + "The planner indicates that the robot is already in a pullover spot."); + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is already in a pullover spot", + _context->requester_id().c_str()); + + _finished(); + return; + } + + if (!plan.get_waypoints().back().graph_index().has_value()) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] has no graph index for its final waypoint. This is a serious " + "bug and should be reported to the RMF maintainers.", + _context->requester_id().c_str()); + _schedule_retry(); + return; + } + + auto goal = rmf_traffic::agv::Plan::Goal( + plan.get_waypoints().back().graph_index().value()); + _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), std::move(full_itinerary), _assign_id, - _state, _update, _finished, std::nullopt); + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), _assign_id, _state, _update, + _finished, std::nullopt); if (!_execution.has_value()) { @@ -371,7 +401,7 @@ Negotiator::NegotiatePtr EmergencyPullover::Active::_respond( const auto evaluator = Negotiator::make_evaluator(table_view); return services::Negotiate::emergency_pullover( - _context->itinerary().assign_plan_id(), _context->planner(), + _context->itinerary().assign_plan_id(), _context->emergency_planner(), _context->location(), table_view, responder, std::move(approval_cb), std::move(evaluator)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 75a5160e0..e8127f8ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -19,6 +19,7 @@ #include "LegacyPhaseShim.hpp" #include "WaitForTraffic.hpp" #include "WaitUntil.hpp" +#include "LockMutexGroup.hpp" #include "../phases/MoveRobot.hpp" #include "../phases/DoorOpen.hpp" @@ -26,6 +27,8 @@ #include "../phases/RequestLift.hpp" #include "../phases/DockRobot.hpp" +#include "../agv/internal_EasyFullControl.hpp" + #include namespace rmf_fleet_adapter { @@ -37,16 +40,29 @@ using StandbyPtr = rmf_task_sequence::Event::StandbyPtr; using UpdateFn = std::function; using MakeStandby = std::function; +std::string print_plan_waypoints( + const std::vector& waypoints, + const rmf_traffic::agv::Graph& graph) +{ + std::stringstream ss; + for (const auto& wp : waypoints) + ss << "\n -- " << agv::print_plan_waypoint(wp, graph, + waypoints.front().time()); + return ss.str(); +} + //============================================================================== struct LegacyPhaseWrapper { LegacyPhaseWrapper( std::shared_ptr phase_, rmf_traffic::Time time_, - rmf_traffic::Dependencies dependencies_) + rmf_traffic::Dependencies dependencies_, + std::optional mutex_group_dependency_) : phase(std::move(phase_)), time(time_), - dependencies(std::move(dependencies_)) + dependencies(std::move(dependencies_)), + mutex_group_dependency(std::move(mutex_group_dependency_)) { // Do nothing } @@ -54,6 +70,7 @@ struct LegacyPhaseWrapper std::shared_ptr phase; rmf_traffic::Time time; rmf_traffic::Dependencies dependencies; + std::optional mutex_group_dependency; }; using LegacyPhases = std::vector; @@ -65,10 +82,22 @@ using RequestLift = phases::RequestLift::PendingPhase; using EndLift = phases::EndLiftSession::Pending; using Move = phases::MoveRobot::PendingPhase; +//============================================================================== +MakeStandby make_wait_for_mutex( + const agv::RobotContextPtr& context, + const rmf_task_sequence::Event::AssignIDPtr& id, + const LockMutexGroup::Data& data) +{ + return [context, id, data](UpdateFn update) + { + return LockMutexGroup::Standby::make(context, id, data); + }; +} + //============================================================================== MakeStandby make_wait_for_traffic( const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_traffic::Dependencies& deps, const rmf_traffic::Time time, const rmf_task_sequence::Event::AssignIDPtr& id) @@ -80,6 +109,42 @@ MakeStandby make_wait_for_traffic( }; } +//============================================================================== +void truncate_arrival( + rmf_traffic::schedule::Itinerary& previous_itinerary, + const rmf_traffic::agv::Plan::Waypoint& wp) +{ + std::size_t first_excluded_route = 0; + for (const auto& c : wp.arrival_checkpoints()) + { + first_excluded_route = std::max(first_excluded_route, c.route_id+1); + auto& r = previous_itinerary.at(c.route_id); + auto& t = r.trajectory(); + + t.erase(t.begin() + (int)c.checkpoint_id, t.end()); + } + + for (std::size_t i = 0; i < previous_itinerary.size(); ++i) + { + const auto& t = previous_itinerary.at(i).trajectory(); + if (t.size() < 2) + { + // If we've reduced this trajectory down to nothing, then erase + // it and all later routes. In the current version of RMF + // we assume that routes with higher indices will never have an + // earlier time value than the earliest of a lower index route. + // This is an assumption we should relax in the future, but it + // helps here for now. + first_excluded_route = + std::min(first_excluded_route, i); + } + } + + previous_itinerary.erase( + previous_itinerary.begin()+first_excluded_route, + previous_itinerary.end()); +} + //============================================================================== class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor { @@ -90,25 +155,57 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor EventPhaseFactory( agv::RobotContextPtr context, LegacyPhases& phases, - rmf_traffic::Time event_start_time, - const rmf_traffic::Dependencies& dependencies, + const rmf_traffic::agv::Plan::Waypoint& initial_waypoint_, + std::optional next_waypoint_, + const PlanIdPtr plan_id, + std::function&, + const rmf_traffic::agv::Plan::Waypoint&)> make_current_mutex_groups, + std::function>( + const rmf_traffic::agv::Plan::Waypoint&)> get_new_mutex_groups, + std::shared_ptr& previous_itinerary, + const rmf_traffic::schedule::Itinerary& full_itinerary, bool& continuous) - : _context(std::move(context)), + : initial_waypoint(initial_waypoint_), + next_waypoint(std::move(next_waypoint_)), + _context(std::move(context)), _phases(phases), - _event_start_time(event_start_time), - _dependencies(dependencies), + _event_start_time(initial_waypoint_.time()), + _plan_id(plan_id), + _make_current_mutex_groups(std::move(make_current_mutex_groups)), + _get_new_mutex_group(std::move(get_new_mutex_groups)), + _previous_itinerary(previous_itinerary), + _full_itinerary(full_itinerary), _continuous(continuous) { // Do nothing } + rmf_traffic::agv::Plan::Waypoint initial_waypoint; + std::optional next_waypoint; + void execute(const Dock& dock) final { + std::optional event_mutex_group; + if (next_waypoint.has_value() && next_waypoint->graph_index().has_value()) + { + const auto [mutex_group_change, new_mutex_groups] = + _get_new_mutex_group(*next_waypoint); + + if (mutex_group_change) + { + event_mutex_group = _make_current_mutex_groups( + new_mutex_groups, initial_waypoint); + } + } + assert(!_moving_lift); _phases.emplace_back( std::make_shared( - _context, dock.dock_name()), - _event_start_time, _dependencies); + _context, dock.dock_name(), + next_waypoint.value_or(initial_waypoint), + _plan_id), + _event_start_time, initial_waypoint.dependencies(), event_mutex_group); _continuous = false; } @@ -122,7 +219,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor open.name(), _context->requester_id(), _event_start_time + open.duration()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -137,12 +234,17 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.name(), _context->requester_id()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } void execute(const LiftSessionBegin& open) final { + truncate_arrival(*_previous_itinerary, initial_waypoint); + + _previous_itinerary = + std::make_shared(_full_itinerary); + assert(!_moving_lift); const auto node = _context->node(); _phases.emplace_back( @@ -150,9 +252,15 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, open.lift_name(), open.floor_name(), - _event_start_time, - phases::RequestLift::Located::Outside), - _event_start_time, _dependencies); + phases::RequestLift::Data{ + initial_waypoint.time(), + phases::RequestLift::Located::Outside, + _plan_id, + std::nullopt, + _previous_itinerary, + initial_waypoint + }), + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -169,7 +277,29 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor void execute(const LiftDoorOpen& open) final { + std::string name; + const auto& graph = _context->navigation_graph(); + const auto i1 = initial_waypoint.graph_index(); + if (const auto nav_params = _context->nav_params()) + { + name = nav_params->get_vertex_name(graph, i1); + } + else if (i1.has_value()) + { + if (const std::string* n = graph.get_waypoint(i1.value()).name()) + { + name = *n; + } + } + const auto node = _context->node(); + auto localize = agv::Destination::Implementation::make( + open.floor_name(), + initial_waypoint.position(), + initial_waypoint.graph_index(), + name, + std::nullopt, + nullptr); // TODO(MXG): The time calculation here should be considered more carefully. _phases.emplace_back( @@ -177,9 +307,13 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, open.lift_name(), open.floor_name(), - _event_start_time + open.duration() + _lifting_duration, - phases::RequestLift::Located::Inside), - _event_start_time, _dependencies); + phases::RequestLift::Data{ + _event_start_time + open.duration() + _lifting_duration, + phases::RequestLift::Located::Inside, + _plan_id, + localize + }), + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _moving_lift = false; _continuous = true; @@ -194,7 +328,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor _context, close.lift_name(), close.floor_name()), - _event_start_time, _dependencies); + _event_start_time, initial_waypoint.dependencies(), std::nullopt); _continuous = true; } @@ -213,13 +347,19 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor agv::RobotContextPtr _context; LegacyPhases& _phases; rmf_traffic::Time _event_start_time; - const rmf_traffic::Dependencies& _dependencies; + PlanIdPtr _plan_id; + std::function&, + const rmf_traffic::agv::Plan::Waypoint&)> _make_current_mutex_groups; + std::function>( + const rmf_traffic::agv::Plan::Waypoint&)> _get_new_mutex_group; + std::shared_ptr& _previous_itinerary; + const rmf_traffic::schedule::Itinerary& _full_itinerary; bool& _continuous; bool _moving_lift = false; rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); }; - //============================================================================== struct EventGroupInfo { @@ -232,7 +372,7 @@ std::optional search_for_door_group( LegacyPhases::const_iterator head, LegacyPhases::const_iterator end, const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_task::Event::AssignIDPtr& id) { const auto* door_open = dynamic_cast( @@ -268,12 +408,21 @@ std::optional search_for_door_group( ++tail; for (auto it = head; it != tail; ++it) { - door_group.push_back( - [legacy = it->phase, context, id](UpdateFn update) - { - return LegacyPhaseShim::Standby::make( - legacy, context->worker(), context->clock(), id, update); - }); + if (it->mutex_group_dependency.has_value()) + { + door_group.push_back(make_wait_for_mutex( + context, id, it->mutex_group_dependency.value())); + } + + if (it->phase) + { + door_group.push_back( + [legacy = it->phase, context, id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, update); + }); + } if (!it->dependencies.empty()) { @@ -323,7 +472,7 @@ std::optional search_for_lift_group( LegacyPhases::const_iterator head, LegacyPhases::const_iterator end, const agv::RobotContextPtr& context, - const rmf_traffic::PlanId plan_id, + const PlanIdPtr plan_id, const rmf_task::Event::AssignIDPtr& event_id, const rmf_task::events::SimpleEventStatePtr& state) { @@ -377,12 +526,21 @@ std::optional search_for_lift_group( ++tail; for (auto it = head; it != tail; ++it) { - lift_group.push_back( - [legacy = it->phase, context, event_id](UpdateFn update) - { - return LegacyPhaseShim::Standby::make( - legacy, context->worker(), context->clock(), event_id, update); - }); + if (it->mutex_group_dependency.has_value()) + { + lift_group.push_back(make_wait_for_mutex( + context, event_id, it->mutex_group_dependency.value())); + } + + if (it->phase) + { + lift_group.push_back( + [legacy = it->phase, context, event_id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), event_id, update); + }); + } if (!it->dependencies.empty()) { @@ -421,11 +579,84 @@ std::optional search_for_lift_group( } // anonymous namespace +//============================================================================== +class LiftFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + LiftFinder(std::string current_name_) + : current_name(std::move(current_name_)), + still_using(false) + { + // Do nothing + } + std::string current_name; + bool still_using = false; + + void execute(const Dock& dock) final {} + void execute(const Wait&) final {} + void execute(const DoorOpen&) final {} + void execute(const DoorClose&) final {} + void execute(const LiftSessionBegin& e) final + { + // If we're going to re-begin using a lift, then we don't need to keep this + // one locked. The new LiftSessionBegin event will re-lock the session. + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftMove& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftDoorOpen& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } + void execute(const LiftSessionEnd& e) final + { + if (e.lift_name() == current_name) + still_using = true; + } +}; + +//============================================================================== +class DoorFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + DoorFinder(std::string current_name_) + : current_name(std::move(current_name_)), + still_using(false) + { + // Do nothing + } + std::string current_name; + bool still_using; + + void execute(const Dock& dock) final {} + void execute(const Wait&) final {} + void execute(const DoorOpen& open) final + { + if (open.name() == current_name) + still_using = true; + } + void execute(const DoorClose& close) final + { + if (close.name() == current_name) + still_using = true; + } + void execute(const LiftSessionBegin& e) final {} + void execute(const LiftMove& e) final {} + void execute(const LiftDoorOpen& e) final {} + void execute(const LiftSessionEnd& e) final {} +}; + //============================================================================== std::optional ExecutePlan::make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + rmf_traffic::PlanId recommended_plan_id, rmf_traffic::agv::Plan plan, + rmf_traffic::agv::Plan::Goal goal, rmf_traffic::schedule::Itinerary full_itinerary, const rmf_task::Event::AssignIDPtr& event_id, rmf_task::events::SimpleEventStatePtr state, @@ -433,6 +664,165 @@ std::optional ExecutePlan::make( std::function finished, std::optional tail_period) { + if (plan.get_waypoints().empty()) + return std::nullopt; + + auto plan_id = std::make_shared(recommended_plan_id); + const auto& graph = context->navigation_graph(); + LegacyPhases legacy_phases; + + rmf_traffic::agv::Graph::LiftPropertiesPtr release_lift; + const auto envelope = context->profile()->footprint() + ->get_characteristic_length()/2.0; + if (const auto* current_lift = context->current_lift_destination()) + { + LiftFinder finder(current_lift->lift_name); + for (const auto& wp : plan.get_waypoints()) + { + if (wp.event()) + { + wp.event()->execute(finder); + if (finder.still_using) + break; + } + } + + if (!finder.still_using) + { + const auto found_lift = graph.find_known_lift(current_lift->lift_name); + if (found_lift) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will release lift [%s] after a replan because it is no " + "longer needed.", + context->requester_id().c_str(), + current_lift->lift_name.c_str()); + + release_lift = found_lift; + } + else + { + std::stringstream ss; + for (const auto& l : graph.all_known_lifts()) + { + ss << "[" << l->name() << "]"; + } + + RCLCPP_ERROR( + context->node()->get_logger(), + "Robot [%s] might be stuck with locking an unknown lift named [%s]. " + "Known lifts include %s", + context->requester_id().c_str(), + current_lift->lift_name.c_str(), + ss.str().c_str()); + } + } + } + + if (!plan.get_waypoints().front().graph_index().has_value()) + { + const Eigen::Vector2d p0 = + plan.get_waypoints().front().position().block<2, 1>(0, 0); + const auto t0 = plan.get_waypoints().front().time(); + + const auto first_graph_wp = [&]() -> std::optional + { + for (const auto& wp : plan.get_waypoints()) + { + if (wp.graph_index().has_value()) + return *wp.graph_index(); + } + + return std::nullopt; + }(); + + if (first_graph_wp.has_value()) + { + const Eigen::Vector2d p1 = + graph.get_waypoint(*first_graph_wp).get_location(); + + // Check if the line from the start of the plan to this waypoint crosses + // through a door, and add a DoorOpen phase if it does + for (const auto& door : graph.all_known_doors()) + { + + if (door->intersects(p0, p1, envelope)) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Ensuring door [%s] is open for [%s] after a replan", + door->name().c_str(), + context->requester_id().c_str()); + + legacy_phases.emplace_back( + std::make_shared( + context, door->name(), context->requester_id(), t0), + t0, rmf_traffic::Dependencies(), std::nullopt); + } + } + + const auto& map = graph.get_waypoint(*first_graph_wp).get_map_name(); + // Check if the robot is going into a lift and summon the lift + for (const auto& lift : graph.all_known_lifts()) + { + if (lift->is_in_lift(p1, envelope)) + { + auto side = phases::RequestLift::Located::Outside; + if (lift->is_in_lift(p0, envelope)) + { + side = phases::RequestLift::Located::Inside; + } + + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will summon lift [%s] to floor [%s] after a replan", + context->requester_id().c_str(), + lift->name().c_str(), + map.c_str()); + + legacy_phases.emplace_back( + std::make_shared( + context, lift->name(), map, + phases::RequestLift::Data{t0, side, plan_id}), + t0, rmf_traffic::Dependencies(), std::nullopt); + } + } + } + } + + std::optional release_door; + if (context->holding_door().has_value()) + { + const auto& current_door = *context->holding_door(); + DoorFinder finder(current_door); + for (const auto& wp : plan.get_waypoints()) + { + if (wp.event()) + { + wp.event()->execute(finder); + if (finder.still_using) + break; + } + } + + if (!finder.still_using) + { + RCLCPP_INFO( + context->node()->get_logger(), + "Robot [%s] will release door [%s] after a replan because it is no " + "longer needed.", + context->requester_id().c_str(), + current_door.c_str()); + release_door = current_door; + } + } + + auto initial_itinerary = + std::make_shared(full_itinerary); + auto previous_itinerary = initial_itinerary; + + std::optional finish_time_estimate; for (const auto& r : plan.get_itinerary()) { @@ -452,54 +842,275 @@ std::optional ExecutePlan::make( plan.get_waypoints(); std::vector move_through; + std::optional current_mutex_groups; + std::unordered_set remaining_mutex_groups; - LegacyPhases legacy_phases; + const auto make_current_mutex_groups = [&]( + const std::unordered_set& new_mutex_groups, + const rmf_traffic::agv::Plan::Waypoint& wp) + { + remaining_mutex_groups = new_mutex_groups; + const rmf_traffic::Time hold_time = wp.time(); + const Eigen::Vector3d hold_position = wp.position(); + std::string hold_map; + if (wp.graph_index().has_value()) + { + hold_map = + graph.get_waypoint(*wp.graph_index()).get_map_name(); + } + else + { + // Find the map name of the first waypoint that is on the graph + for (const auto& wp : waypoints) + { + if (wp.graph_index().has_value()) + { + hold_map = + graph.get_waypoint(*wp.graph_index()).get_map_name(); + break; + } + } + + if (hold_map.empty()) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Cannot find a map for a mutex group transition needed " + "by robot [%s]. There are [%lu] remaining waypoints. Please " + "report this situation to the maintainers of RMF.", + context->requester_id().c_str(), + waypoints.size()); + } + } + + truncate_arrival(*previous_itinerary, wp); + + auto expected_waypoints = waypoints; + expected_waypoints.insert(expected_waypoints.begin(), wp); + + auto next_itinerary = std::make_shared< + rmf_traffic::schedule::Itinerary>(full_itinerary); + auto data = LockMutexGroup::Data{ + new_mutex_groups, + hold_map, + hold_position, + hold_time, + plan_id, + next_itinerary, + expected_waypoints, + goal + }; + + previous_itinerary = data.resume_itinerary; + + return data; + }; + + const auto get_new_mutex_groups = [&]( + const rmf_traffic::agv::Plan::Waypoint& wp) + { + std::unordered_set new_mutex_groups; + if (wp.graph_index().has_value()) + { + const auto& group = + graph.get_waypoint(*wp.graph_index()).in_mutex_group(); + if (!group.empty()) + { + new_mutex_groups.insert(group); + } + } + + for (const auto l : wp.approach_lanes()) + { + const auto& lane = graph.get_lane(l); + const auto& group = lane.properties().in_mutex_group(); + if (!group.empty()) + { + new_mutex_groups.insert(group); + break; + } + } + + bool mutex_group_change = + (!new_mutex_groups.empty() && remaining_mutex_groups.empty()); + + if (!mutex_group_change && !remaining_mutex_groups.empty()) + { + for (const auto& new_group : new_mutex_groups) + { + if (remaining_mutex_groups.count(new_group) == 0) + { + mutex_group_change = true; + break; + } + } + } + + if (!mutex_group_change) + { + // We don't need to lock any new mutex groups, but we may be releasing + // some. We should reduce the remaining group set to whatever is in the + // new group set so that if we need to add some groups back later then + // we recognize it. + remaining_mutex_groups = new_mutex_groups; + } + + return std::make_pair(mutex_group_change, new_mutex_groups); + }; + + const auto t0 = waypoints.front().time(); while (!waypoints.empty()) { auto it = waypoints.begin(); bool event_occurred = false; for (; it != waypoints.end(); ++it) { + const auto [mutex_group_change, new_mutex_groups] = get_new_mutex_groups( + *it); + if (mutex_group_change) + { + if (move_through.size() > 1) + { + auto next_mutex_group = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + + legacy_phases.emplace_back( + std::make_shared( + context, move_through, plan_id, tail_period), + move_through.back().time(), move_through.back().dependencies(), + current_mutex_groups); + + auto last = move_through.back(); + move_through.clear(); + // Repeat the last waypoint so that follow_new_path has continuity. + move_through.push_back(last); + waypoints.erase(waypoints.begin(), it); + + current_mutex_groups = next_mutex_group; + + // We treat this the same as an event occurring to indicate that + // we should keep looping. + event_occurred = true; + break; + } + else + { + // We don't need to put in a break because nothing has been moved + // through yet. Just set the current_mutex_groups from this point. + if (move_through.empty()) + { + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, *it); + } + else + { + assert(move_through.size() == 1); + current_mutex_groups = make_current_mutex_groups( + new_mutex_groups, move_through.back()); + } + } + } + move_through.push_back(*it); + const bool release_lift_here = release_lift && + it->graph_index().has_value() && + !release_lift->is_in_lift(it->position().block<2, 1>(0, 0), envelope); - if (it->event()) + const bool release_door_here = release_door.has_value() + && it->graph_index().has_value(); + + if (it->event() || release_lift_here || release_door_here) { if (move_through.size() > 1) { legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies()); + it->time(), it->dependencies(), current_mutex_groups); + } + else if (current_mutex_groups.has_value()) + { + legacy_phases.emplace_back( + nullptr, it->time(), + rmf_traffic::Dependencies(), current_mutex_groups); + } + current_mutex_groups = std::nullopt; + + if (release_lift_here) + { + legacy_phases.emplace_back( + std::make_shared( + context, + release_lift->name(), + ""), + it->time(), rmf_traffic::Dependencies(), std::nullopt); + + release_lift = nullptr; + } + + if (release_door_here) + { + legacy_phases.emplace_back( + std::make_shared( + context, + *release_door, + context->requester_id()), + it->time(), rmf_traffic::Dependencies(), std::nullopt); + + release_door = std::nullopt; + } + + std::optional next_waypoint; + auto next_it = it + 1; + if (next_it != waypoints.end()) + { + next_waypoint = *next_it; } move_through.clear(); bool continuous = true; - EventPhaseFactory factory( - context, legacy_phases, it->time(), it->dependencies(), continuous); - it->event()->execute(factory); - while (factory.moving_lift()) + if (it->event()) { - const auto last_it = it; - ++it; - if (!it->event()) + EventPhaseFactory factory( + context, legacy_phases, *it, next_waypoint, plan_id, + make_current_mutex_groups, get_new_mutex_groups, + previous_itinerary, full_itinerary, + continuous); + + it->event()->execute(factory); + while (factory.moving_lift()) { - const double dist = - (it->position().block<2, 1>(0, 0) - - last_it->position().block<2, 1>(0, 0)).norm(); + const auto last_it = it; + ++it; + if (it == waypoints.end()) + { + // This should not happen... this would imply that the goal was + // inside of a lift + return std::nullopt; + } - if (dist < 0.5) + if (!it->event()) { - // We'll assume that this is just a misalignment in the maps + const double dist = + (it->position().block<2, 1>(0, 0) + - last_it->position().block<2, 1>(0, 0)).norm(); + + if (dist > 0.5) + { + // We'll assume that this is just a misalignment in the maps + state->update_log().warn( + "Plan involves a translation of [" + std::to_string( + dist) + + "m] while inside a lift. This may indicate an error in the " + "navigation graph. Please report this to the system integrator."); + } + continue; } - state->update_log().warn( - "Plan involves a translation of [" + std::to_string(dist) - + "m] while inside a lift. This may indicate an error in the " - "navigation graph. Please report this to the system integrator."); + factory.initial_waypoint = *it; + it->event()->execute(factory); } - - it->event()->execute(factory); } if (continuous) @@ -520,11 +1131,12 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - it->time(), it->dependencies()); + it->time(), it->dependencies(), current_mutex_groups); } else { - legacy_phases.emplace_back(nullptr, it->time(), it->dependencies()); + legacy_phases.emplace_back( + nullptr, it->time(), it->dependencies(), current_mutex_groups); } // Have the next sequence of waypoints begin with this one. @@ -548,7 +1160,8 @@ std::optional ExecutePlan::make( legacy_phases.emplace_back( std::make_shared( context, move_through, plan_id, tail_period), - finish_time_estimate.value(), rmf_traffic::Dependencies{}); + finish_time_estimate.value(), rmf_traffic::Dependencies{}, + current_mutex_groups); } if (!event_occurred) @@ -583,6 +1196,12 @@ std::optional ExecutePlan::make( } else { + if (head->mutex_group_dependency.has_value()) + { + standbys.push_back(make_wait_for_mutex( + context, event_id, head->mutex_group_dependency.value())); + } + if (head->phase) { standbys.push_back( @@ -618,49 +1237,63 @@ std::optional ExecutePlan::make( }); } - auto sequence = rmf_task_sequence::events::Bundle::standby( - rmf_task_sequence::events::Bundle::Type::Sequence, - standbys, state, std::move(update))->begin([]() {}, std::move(finished)); - std::size_t attempts = 0; - while (!context->itinerary().set(plan_id, std::move(full_itinerary))) - { - // Some mysterious behavior has been happening where plan_ids are invalid. - // We will attempt to catch that here and try to learn more about what - // could be causing that, while allowing progress to continue. - std::string task_id = ""; - if (context->current_task_id()) - task_id = *context->current_task_id(); - - RCLCPP_ERROR( - context->node()->get_logger(), - "Invalid plan_id [%lu] when current plan_id is [%lu] for [%s] in group " - "[%s] while performing task [%s]. Please notify an RMF developer.", - plan_id, - context->itinerary().current_plan_id(), - context->name().c_str(), - context->group().c_str(), - task_id.c_str()); - state->update_log().error( - "Invalid plan_id [" + std::to_string(plan_id) + "] when current plan_id " - "is [" + std::to_string(context->itinerary().current_plan_id()) + "] " - "Please notify an RMF developer."); - - plan_id = context->itinerary().assign_plan_id(); - - if (++attempts > 5) + if (!initial_itinerary->empty()) + { + while (!context->itinerary().set(*plan_id, *initial_itinerary)) { + // Some mysterious behavior has been happening where plan_ids are invalid. + // We will attempt to catch that here and try to learn more about what + // could be causing that, while allowing progress to continue. + std::string task_id = ""; + if (context->current_task_id()) + task_id = *context->current_task_id(); + RCLCPP_ERROR( context->node()->get_logger(), - "Requesting replan for [%s] in group [%s] because plan is repeatedly " - "being rejected while performing task [%s]", + "Failed to set plan_id [%lu] when current plan_id is [%lu] for [%s] in " + "group [%s] while executing plan for task [%s]. Please report this bug " + "to the RMF maintainers.", + *plan_id, + context->itinerary().current_plan_id(), context->name().c_str(), context->group().c_str(), task_id.c_str()); - return std::nullopt; + state->update_log().error( + "Invalid plan_id [" + std::to_string(*plan_id) + + "] when current plan_id is [" + + std::to_string(context->itinerary().current_plan_id()) + + "] Please notify an RMF developer."); + + *plan_id = context->itinerary().assign_plan_id(); + + if (++attempts > 5) + { + std::stringstream ss_sizes; + for (const auto& r : *initial_itinerary) + { + ss_sizes << "[" << r.map() << ":" << r.trajectory().size() << "]"; + } + + RCLCPP_ERROR( + context->node()->get_logger(), + "Requesting replan for [%s] in group [%s] because plan is repeatedly " + "being rejected while performing task [%s]. Itinerary has [%lu] " + "with sizes of %s.", + context->name().c_str(), + context->group().c_str(), + task_id.c_str(), + initial_itinerary->size(), + ss_sizes.str().c_str()); + return std::nullopt; + } } } + auto sequence = rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + standbys, state, std::move(update))->begin([]() {}, std::move(finished)); + return ExecutePlan{ std::move(plan), plan_id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp index 615ccffe6..703349d35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -19,6 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP #include "../agv/RobotContext.hpp" +#include "../LegacyTask.hpp" #include #include @@ -33,6 +34,7 @@ struct ExecutePlan agv::RobotContextPtr context, rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, + rmf_traffic::agv::Plan::Goal goal, rmf_traffic::schedule::Itinerary full_itinerary, const rmf_task_sequence::Event::AssignIDPtr& event_id, rmf_task::events::SimpleEventStatePtr state, @@ -41,7 +43,7 @@ struct ExecutePlan std::optional tail_period); rmf_traffic::agv::Plan plan; - rmf_traffic::PlanId plan_id; + PlanIdPtr plan_id; rmf_traffic::Time finish_time_estimate; rmf_task_sequence::Event::ActivePtr sequence; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index c3b465641..671e62b99 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -266,8 +266,19 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const const auto now = _context->now(); const auto& itin = _context->itinerary(); - if (const auto delay = itin.cumulative_delay(_execution->plan_id)) - return finish - now + *delay; + if (_execution->plan_id) + { + if (const auto delay = itin.cumulative_delay(*_execution->plan_id)) + return finish - now + *delay; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Missing plan_id for go_to_place of robot [%s]. Please report this " + "critical bug to the maintainers of RMF.", + _context->requester_id().c_str()); + } } const auto& estimate = @@ -384,6 +395,17 @@ void GoToPlace::Active::_find_plan() _state->update_log().info( "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); + const auto& graph = _context->navigation_graph(); + std::stringstream ss; + ss << "Planning for [" << _context->requester_id() + << "] to [" << goal_name << "] from one of these locations:" + << agv::print_starts(_context->location(), graph); + + RCLCPP_INFO( + _context->node()->get_logger(), + "%s", + ss.str().c_str()); + // TODO(MXG): Make the planning time limit configurable _find_path_service = std::make_shared( _context->planner(), _context->location(), _goal, @@ -497,6 +519,16 @@ void GoToPlace::Active::_execute_plan( _state->update_status(Status::Completed); _state->update_log().info( "The planner indicates that the robot is already at its goal."); + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is already at its goal [%lu]", + _context->requester_id().c_str(), + _goal.waypoint()); + + const auto& graph = _context->navigation_graph(); + _context->retain_mutex_groups( + {graph.get_waypoint(_goal.waypoint()).in_mutex_group()}); + _finished(); return; } @@ -508,7 +540,7 @@ void GoToPlace::Active::_execute_plan( _context->requester_id().c_str()); _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), std::move(full_itinerary), + _context, plan_id, std::move(plan), _goal, std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); if (!_execution.has_value()) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp new file mode 100644 index 000000000..8c1dba8f1 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.cpp @@ -0,0 +1,401 @@ +/* + * 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 "LockMutexGroup.hpp" +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +std::string all_str(const std::unordered_set& all) +{ + std::stringstream ss; + for (const auto& item : all) + { + ss << "[" << item << "]"; + } + return ss.str(); +} + +//============================================================================== +std::string LockMutexGroup::Data::all_groups_str() const +{ + return all_str(mutex_groups); +} + +//============================================================================== +auto LockMutexGroup::Standby::make( + agv::RobotContextPtr context, + const AssignIDPtr& id, + Data data) +-> std::shared_ptr +{ + auto standby = std::shared_ptr(new Standby(std::move(data))); + standby->_context = std::move(context); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Lock mutex groups " + data.all_groups_str(), + "Waiting for the mutex groups to be locked", + rmf_task::Event::Status::Standby, {}, standby->_context->clock()); + return standby; +} + +//============================================================================== +auto LockMutexGroup::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LockMutexGroup::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto LockMutexGroup::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + return Active::make(_context, _state, std::move(finished), _data); +} + +//============================================================================== +LockMutexGroup::Standby::Standby(Data data) +: _data(data) +{ + // Do nothing +} + +//============================================================================== +auto LockMutexGroup::Active::make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Data data) -> std::shared_ptr +{ + auto active = std::shared_ptr(new Active(std::move(data))); + active->_context = std::move(context); + active->_state = std::move(state); + active->_finished = std::move(finished); + active->_initialize(); + + return active; +} + +//============================================================================== +auto LockMutexGroup::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LockMutexGroup::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto LockMutexGroup::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto LockMutexGroup::Active::interrupt( + std::function task_is_interrupted) -> Resume +{ + _context->worker().schedule([task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + return Resume::make([]() { /* do nothing */ }); +} + +//============================================================================== +void LockMutexGroup::Active::cancel() +{ + _state->update_status(Status::Canceled); + const auto finished = _finished; + _finished = nullptr; + _context->worker().schedule([finished](const auto&) + { + finished(); + }); +} + +//============================================================================== +void LockMutexGroup::Active::kill() +{ + cancel(); +} + +//============================================================================== +LockMutexGroup::Active::Active(Data data) +: _data(std::move(data)) +{ + // Do nothing +} + +//============================================================================== +void LockMutexGroup::Active::_initialize() +{ + _state->update_status(State::Status::Underway); + using MutexGroupStatesPtr = + std::shared_ptr; + + _remaining = _data.mutex_groups; + for (const auto& [locked, _] : _context->locked_mutex_groups()) + { + _remaining.erase(locked); + } + + if (_remaining.empty()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "All mutex groups were already locked for [%s]", + _context->requester_id().c_str()); + + _schedule(*_data.resume_itinerary); + // We don't need to do anything further, we already got the mutex group + // previously. + _context->worker().schedule( + [state = _state, finished = _finished](const auto&) + { + state->update_status(State::Status::Completed); + finished(); + }); + return; + } + + *_data.plan_id += 1; + _context->schedule_hold( + _data.plan_id, + _data.hold_time, + std::chrono::seconds(5), + _data.hold_position, + _data.hold_map); + _stubborn = _context->be_stubborn(); + + _state->update_log().info( + "Waiting to lock mutex group " + _data.all_groups_str()); + RCLCPP_INFO( + _context->node()->get_logger(), + "Waiting to lock mutex groups %s for robot [%s]", + _data.all_groups_str().c_str(), + _context->requester_id().c_str()); + + const auto cumulative_delay = _context->now() - _data.hold_time; + _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); + + _delay_timer = _context->node()->try_create_wall_timer( + std::chrono::seconds(1), + [weak = weak_from_this(), plan_id = *_data.plan_id]() + { + const auto self = weak.lock(); + if (!self) + return; + + self->_apply_cumulative_delay(); + }); + + _listener = _context->request_mutex_groups( + _data.mutex_groups, _data.hold_time) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe([w = weak_from_this()](const std::string& locked) + { + const auto self = w.lock(); + if (!self) + return; + + self->_remaining.erase(locked); + if (self->_remaining.empty()) + { + const auto finished = self->_finished; + self->_finished = nullptr; + if (!finished) + return; + + const auto now = self->_context->now(); + const auto delay = now - self->_data.hold_time; + if (delay > std::chrono::seconds(2)) + { + const auto start = [&]() + -> std::optional + { + for (const auto& wp : self->_data.waypoints) + { + if (wp.graph_index().has_value()) + { + return rmf_traffic::agv::Plan::Start( + self->_context->now(), + *wp.graph_index(), + wp.position()[2]); + } + } + return std::nullopt; + }(); + + if (start.has_value()) + { + self->_find_path_service = std::make_shared( + self->_context->planner(), + std::vector({*start}), + self->_data.goal, + self->_context->schedule()->snapshot(), + self->_context->itinerary().id(), + self->_context->profile(), + std::chrono::seconds(5)); + + self->_plan_subscription = + rmf_rxcpp::make_job( + self->_find_path_service) + .observe_on( + rxcpp::identity_same_worker(self->_context->worker())) + .subscribe( + [w = self->weak_from_this(), finished]( + const services::FindPath::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + if (self->_consider_plan_result(result)) + { + // We have a matching plan so proceed + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutexes %s for [%s] and plan is " + "unchanged after waiting", + self->_data.all_groups_str().c_str(), + self->_context->requester_id().c_str()); + + self->_schedule(*self->_data.resume_itinerary); + self->_apply_cumulative_delay(); + self->_state->update_status(Status::Completed); + finished(); + return; + } + + // The new plan was not a match, so we should trigger a + // proper replan. + self->_state->update_status(Status::Completed); + self->_context->request_replan(); + }); + + self->_find_path_timeout = + self->_context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = self->_find_path_service->weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + { + service->interrupt(); + } + }); + return; + } + } + + // We locked the mutex quickly enough that we should proceed. + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Finished locking mutexes %s for [%s]", + self->_data.all_groups_str().c_str(), + self->_context->requester_id().c_str()); + + self->_schedule(*self->_data.resume_itinerary); + self->_apply_cumulative_delay(); + self->_state->update_status(Status::Completed); + finished(); + return; + } + }); +} + +//============================================================================== +void LockMutexGroup::Active::_schedule( + rmf_traffic::schedule::Itinerary itinerary) const +{ + _context->schedule_itinerary(_data.plan_id, std::move(itinerary)); +} + +//============================================================================== +void LockMutexGroup::Active::_apply_cumulative_delay() +{ + const auto cumulative_delay = _context->now() - _data.hold_time; + _context->itinerary().cumulative_delay(*_data.plan_id, cumulative_delay); +} + +//============================================================================== +namespace { +std::vector filter_graph_indices( + const std::vector& waypoints) +{ + std::vector output; + output.reserve(waypoints.size()); + for (const auto& wp : waypoints) + { + if (wp.graph_index().has_value()) + { + if (*wp.graph_index() != output.back()) + { + output.push_back(*wp.graph_index()); + } + } + } + return output; +} +} // anonymous namespace + +//============================================================================== +bool LockMutexGroup::Active::_consider_plan_result( + services::FindPath::Result result) +{ + if (!result.success()) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Replanning for [%s] after locking mutexes %s because the path to the " + "goal has become blocked.", + _context->requester_id().c_str(), + _data.all_groups_str().c_str()); + return false; + } + + const auto original_sequence = filter_graph_indices(_data.waypoints); + const auto new_sequence = filter_graph_indices(result->get_waypoints()); + if (original_sequence != new_sequence) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Replanning for [%s] after locking mutexes %s because the external " + "traffic has substantially changed.", + _context->requester_id().c_str(), + _data.all_groups_str().c_str()); + return false; + } + + return true; +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp new file mode 100644 index 000000000..1d8c75b35 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LockMutexGroup.hpp @@ -0,0 +1,119 @@ +/* + * 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. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP + +#include "../agv/RobotContext.hpp" +#include "../services/FindPath.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +std::string all_str(const std::unordered_set& all); + +//============================================================================== +class LockMutexGroup : public rmf_task_sequence::Event +{ +public: + struct Data + { + std::unordered_set mutex_groups; + std::string hold_map; + Eigen::Vector3d hold_position; + rmf_traffic::Time hold_time; + std::shared_ptr plan_id; + std::shared_ptr resume_itinerary; + std::vector waypoints; + rmf_traffic::agv::Plan::Goal goal; + + std::string all_groups_str() const; + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + const AssignIDPtr& id, + Data data); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + Standby(Data data); + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + Data _data; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Data data); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + Active(Data data); + void _initialize(); + void _schedule(rmf_traffic::schedule::Itinerary itinerary) const; + void _apply_cumulative_delay(); + bool _consider_plan_result(services::FindPath::Result result); + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _finished; + rmf_rxcpp::subscription_guard _listener; + rclcpp::TimerBase::SharedPtr _delay_timer; + std::shared_ptr _stubborn; + Data _data; + std::unordered_set _remaining; + rmf_rxcpp::subscription_guard _plan_subscription; + std::shared_ptr _find_path_service; + rclcpp::TimerBase::SharedPtr _find_path_timeout; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__LOCKMUTEXGROUP_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp index cc3eb5371..89d1aecc8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp @@ -223,6 +223,7 @@ void PerformAction::Active::cancel() { _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); + auto self = shared_from_this(); _finished(); if (auto data = _execution_data.lock()) data->okay = false; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp new file mode 100644 index 000000000..8d112de27 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.cpp @@ -0,0 +1,122 @@ +/* + * 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 "WaitForCancel.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +auto WaitForCancel::Standby::make( + agv::RobotContextPtr context, + const AssignIDPtr& id) +-> std::shared_ptr +{ + auto standby = std::shared_ptr(new Standby); + standby->_context = std::move(context); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Wait for cancel", + "This task will remain active until it gets canceled", + rmf_task::Event::Status::Standby, {}, standby->_context->clock()); + return standby; +} + +//============================================================================== +auto WaitForCancel::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration WaitForCancel::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto WaitForCancel::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + return Active::make( + _context, + _state, + std::move(finished)); +} + +//============================================================================== +auto WaitForCancel::Active::make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished) -> std::shared_ptr +{ + auto active = std::shared_ptr(new Active); + active->_context = std::move(context); + active->_finished = std::move(finished); + active->_state = std::move(state); + active->_state->update_status(Status::Underway); + return active; +} + +//============================================================================== +auto WaitForCancel::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration WaitForCancel::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto WaitForCancel::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto WaitForCancel::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + _context->worker().schedule([task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + return Resume::make([]() { /* do nothing */}); +} + +//============================================================================== +void WaitForCancel::Active::cancel() +{ + _state->update_status(Status::Canceled); + _context->worker().schedule([finished = _finished](const auto&) + { + finished(); + }); +} + +//============================================================================== +void WaitForCancel::Active::kill() +{ + cancel(); +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp new file mode 100644 index 000000000..9c4aad415 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForCancel.hpp @@ -0,0 +1,87 @@ +/* + * 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. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP + +#include "../agv/RobotContext.hpp" + +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class WaitForCancel : public rmf_task_sequence::Event +{ +public: + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + const AssignIDPtr& id); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + Standby() = default; + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _finished; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORCANCEL_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp index 1be020b61..f0acae3e4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -23,7 +23,7 @@ namespace events { //============================================================================== auto WaitForTraffic::Standby::make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, rmf_traffic::Dependencies dependencies, rmf_traffic::Time expected_time, const AssignIDPtr& id, @@ -59,9 +59,28 @@ auto WaitForTraffic::Standby::begin( std::function, std::function finished) -> ActivePtr { + RCLCPP_INFO( + _context->node()->get_logger(), + "[%s] waiting for traffic", + _context->requester_id().c_str()); + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for WaitForTraffic action for robot [%s]. This " + "is a critical internal error, please report this bug to the RMF " + "maintainers.", + _context->requester_id().c_str()); + } + return Active::make( _context, - _plan_id, + plan_id, _dependencies, _expected_time, _state, @@ -79,6 +98,9 @@ auto WaitForTraffic::Active::make( std::function update, std::function finished) -> std::shared_ptr { + using MutexGroupRequestPtr = + std::shared_ptr; + auto active = std::make_shared(); active->_context = std::move(context); active->_plan_id = plan_id; @@ -87,6 +109,37 @@ auto WaitForTraffic::Active::make( active->_update = std::move(update); active->_finished = std::move(finished); + active->_mutex_group_listener = active->_context->node() + ->mutex_group_request_obs() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe([w = active->weak_from_this()](const MutexGroupRequestPtr& msg) + { + const auto self = w.lock(); + if (!self) + return; + + if (msg->claimant == self->_context->participant_id()) + { + // We can ignore our own mutex group requests + return; + } + + if (self->_context->locked_mutex_groups().count(msg->group) > 0) + { + // If another participant is waiting to lock a mutex that we have + // already locked, then we must delete any dependencies related to + // that participant. + auto r_it = std::remove_if( + self->_dependencies.begin(), + self->_dependencies.end(), + [&](const DependencySubscription& d) + { + return d.dependency().on_participant == msg->claimant; + }); + self->_dependencies.erase(r_it, self->_dependencies.end()); + } + }); + const auto consider_going = [w = active->weak_from_this()]() { if (const auto self = w.lock()) @@ -221,6 +274,10 @@ void WaitForTraffic::Active::_consider_going() _decision_made = std::chrono::steady_clock::now(); _state->update_status(Status::Completed); _state->update_log().info("All traffic dependencies satisfied"); + RCLCPP_INFO( + _context->node()->get_logger(), + "[%s] done waiting for traffic", + _context->requester_id().c_str()); return _finished(); } @@ -233,6 +290,10 @@ void WaitForTraffic::Active::_consider_going() _state->update_status(Status::Delayed); _state->update_log().info( "Replanning because a traffic dependency is excessively delayed"); + RCLCPP_INFO( + _context->node()->get_logger(), + "Replanning for [%s] because a traffic dependency is excessively delayed", + _context->requester_id().c_str()); return _replan(); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp index 56e30605b..d4651c2be 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.hpp @@ -19,6 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__EVENTS__WAITFORTRAFFIC_HPP #include "../agv/RobotContext.hpp" +#include "../LegacyTask.hpp" #include #include @@ -38,7 +39,7 @@ class WaitForTraffic : public rmf_task_sequence::Event static std::shared_ptr make( agv::RobotContextPtr context, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, rmf_traffic::Dependencies dependencies, rmf_traffic::Time expected_time, const AssignIDPtr& id, @@ -54,7 +55,7 @@ class WaitForTraffic : public rmf_task_sequence::Event private: agv::RobotContextPtr _context; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; rmf_traffic::Dependencies _dependencies; rmf_traffic::Time _expected_time; rmf_task::events::SimpleEventStatePtr _state; @@ -104,6 +105,7 @@ class WaitForTraffic : public rmf_task_sequence::Event std::function _finished; rclcpp::TimerBase::SharedPtr _timer; std::optional _decision_made; + rmf_rxcpp::subscription_guard _mutex_group_listener; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp index 6f97efb97..a5df28611 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp @@ -23,18 +23,19 @@ namespace phases { //============================================================================== DockRobot::ActivePhase::ActivePhase( agv::RobotContextPtr context, - std::string dock_name) + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + rmf_traffic::PlanId plan_id) : _context{std::move(context)}, _dock_name{std::move(dock_name)}, + _waypoint(std::move(waypoint)), + _plan_id(plan_id), _be_stubborn(_context->be_stubborn()) { std::ostringstream oss; oss << "Docking robot to " << _dock_name; _description = oss.str(); - _action = std::make_shared(this); - _obs = rmf_rxcpp::make_job(_action); - _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_DOCKING); } @@ -42,7 +43,7 @@ DockRobot::ActivePhase::ActivePhase( const rxcpp::observable& DockRobot::ActivePhase::observe() const { - return _obs; + return obs; } //============================================================================== @@ -73,9 +74,13 @@ const std::string& DockRobot::ActivePhase::description() const //============================================================================== DockRobot::PendingPhase::PendingPhase( agv::RobotContextPtr context, - std::string dock_name) + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + PlanIdPtr plan_id) : _context{std::move(context)}, - _dock_name{std::move(dock_name)} + _dock_name{std::move(dock_name)}, + _waypoint(std::move(waypoint)), + _plan_id(std::move(plan_id)) { std::ostringstream oss; oss << "Dock robot to " << _dock_name; @@ -85,7 +90,24 @@ DockRobot::PendingPhase::PendingPhase( //============================================================================== std::shared_ptr DockRobot::PendingPhase::begin() { - return std::make_shared(_context, _dock_name); + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for MoveRobot action for robot [%s]. This is a " + "critical internal error, please report this bug to the RMF maintainers.", + _context->requester_id().c_str()); + } + auto active = std::make_shared( + _context, _dock_name, _waypoint, plan_id); + active->action = std::make_shared(active->weak_from_this()); + active->obs = rmf_rxcpp::make_job(active->action); + return active; } //============================================================================== @@ -102,7 +124,7 @@ const std::string& DockRobot::PendingPhase::description() const } //============================================================================== -DockRobot::Action::Action(ActivePhase* phase) +DockRobot::Action::Action(std::weak_ptr phase) : _phase(phase) { // Do nothing diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp index f76a9d90b..8bd1113c5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -29,13 +29,16 @@ struct DockRobot { class Action; - class ActivePhase : public LegacyTask::ActivePhase + class ActivePhase : public LegacyTask::ActivePhase, + public std::enable_shared_from_this { public: ActivePhase( agv::RobotContextPtr context, - std::string dock_name); + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + rmf_traffic::PlanId plan_id); const rxcpp::observable& observe() const override; @@ -47,14 +50,16 @@ struct DockRobot const std::string& description() const override; + std::shared_ptr action; + rxcpp::observable obs; private: friend class Action; agv::RobotContextPtr _context; std::string _dock_name; std::string _description; - std::shared_ptr _action; - rxcpp::observable _obs; + rmf_traffic::agv::Plan::Waypoint _waypoint; + rmf_traffic::PlanId _plan_id; std::shared_ptr _be_stubborn; }; @@ -64,7 +69,9 @@ struct DockRobot PendingPhase( agv::RobotContextPtr context, - std::string dock_name); + std::string dock_name, + rmf_traffic::agv::Plan::Waypoint waypoint, + PlanIdPtr plan_id); std::shared_ptr begin() override; @@ -77,19 +84,21 @@ struct DockRobot agv::RobotContextPtr _context; std::string _dock_name; std::string _description; + rmf_traffic::agv::Plan::Waypoint _waypoint; + PlanIdPtr _plan_id; }; class Action { public: - Action(ActivePhase* phase); + Action(std::weak_ptr phase); template void operator()(const Subscriber& s); private: - ActivePhase* _phase; + std::weak_ptr _phase; }; }; @@ -97,23 +106,57 @@ struct DockRobot template void DockRobot::Action::operator()(const Subscriber& s) { - LegacyTask::StatusMsg status; - status.state = LegacyTask::StatusMsg::STATE_ACTIVE; - status.status = "Docking [" + _phase->_context->requester_id() + - "] into dock [" - + _phase->_dock_name + "]"; - - s.on_next(status); - _phase->_context->command()->dock( - _phase->_dock_name, - [s, dock_name = _phase->_dock_name, context = _phase->_context]() + const auto active = _phase.lock(); + if (!active) + return; + + active->_context->worker().schedule( + [s, w = active->weak_from_this()](const auto&) { + const auto active = w.lock(); + if (!active) + return; + LegacyTask::StatusMsg status; - status.status = "Finished docking [" + context->requester_id() - + "] into dock [" + dock_name + "]"; - status.state = LegacyTask::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; + status.status = "Docking [" + active->_context->requester_id() + + "] into dock [" + + active->_dock_name + "]"; + s.on_next(status); - s.on_completed(); + active->_context->command()->dock( + active->_dock_name, + [s, dock_name = active->_dock_name, context = active->_context, + wp = active->_waypoint, plan_id = active->_plan_id]() + { + context->worker().schedule( + [s, dock_name, context, wp, plan_id](const auto&) + { + LegacyTask::StatusMsg status; + status.status = "Finished docking [" + context->requester_id() + + "] into dock [" + dock_name + "]"; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; + for (const auto& c : wp.arrival_checkpoints()) + { + context->itinerary().reached(plan_id, c.route_id, + c.checkpoint_id); + } + + if (wp.graph_index().has_value()) + { + const auto& graph = context->navigation_graph(); + context->retain_mutex_groups( + {graph.get_waypoint(*wp.graph_index()).in_mutex_group()}); + } + + const auto now = context->now(); + const auto cumulative_delay = now - wp.time(); + context->itinerary().cumulative_delay( + plan_id, cumulative_delay, std::chrono::seconds(1)); + s.on_next(status); + s.on_completed(); + }); + }); }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp index 0ccb1b678..6a8552737 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp @@ -29,6 +29,12 @@ std::shared_ptr DoorClose::ActivePhase::make( std::string door_name, std::string request_id) { + RCLCPP_INFO( + context->node()->get_logger(), + "Releasing door [%s] for [%s]", + door_name.c_str(), + context->requester_id().c_str()); + context->_release_door(door_name); auto inst = std::shared_ptr(new ActivePhase( std::move(context), std::move(door_name), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp index 3d5f9d82b..899d7a12c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp @@ -54,7 +54,13 @@ DoorOpen::ActivePhase::ActivePhase( _request_id(std::move(request_id)), _expected_finish(std::move(expected_finish)) { + _context->_hold_door(_door_name); _description = "Opening [door:" + _door_name + "]"; + RCLCPP_INFO( + _context->node()->get_logger(), + "Opening door [%s] for [%s]", + _door_name.c_str(), + _context->requester_id().c_str()); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp index a4224e938..a1c85a2ea 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp @@ -45,7 +45,13 @@ EndLiftSession::Active::Active( _lift_name(std::move(lift_name)), _destination(std::move(destination)) { - _description = "Ending session with lift [" + lift_name + "]"; + _description = "Ending session with lift [" + _lift_name + "]"; + RCLCPP_INFO( + _context->node()->get_logger(), + "Ending lift [%s] session for [%s]", + _lift_name.c_str(), + _context->requester_id().c_str()); + _context->release_lift(); } //============================================================================== @@ -86,24 +92,6 @@ void EndLiftSession::Active::_init_obs() using rmf_lift_msgs::msg::LiftRequest; using rmf_lift_msgs::msg::LiftState; _obs = _context->node()->lift_state() - .lift(on_subscribe([weak = weak_from_this()]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_publish_session_end(); - me->_timer = me->_context->node()->try_create_wall_timer( - std::chrono::milliseconds(1000), - [weak]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_publish_session_end(); - }); - })) .map([weak = weak_from_this()](const LiftState::SharedPtr& state) { const auto me = weak.lock(); @@ -124,27 +112,7 @@ void EndLiftSession::Active::_init_obs() return msg; }) - .lift(grab_while_active()) - .finally([weak = weak_from_this()]() - { - const auto me = weak.lock(); - if (!me) - return; - - me->_timer.reset(); - }); -} - -//============================================================================== -void EndLiftSession::Active::_publish_session_end() -{ - rmf_lift_msgs::msg::LiftRequest msg; - msg.lift_name = _lift_name; - msg.destination_floor = _destination; - msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; - msg.session_id = _context->requester_id(); - - _context->node()->lift_request()->publish(msg); + .lift(grab_while_active()); } //============================================================================== @@ -156,7 +124,7 @@ EndLiftSession::Pending::Pending( _lift_name(std::move(lift_name)), _destination(std::move(destination)) { - _description = "End session with lift [" + lift_name + "]"; + _description = "End session with lift [" + _lift_name + "]"; } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp index fdd2e339b..dc15dc0de 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp @@ -59,10 +59,8 @@ struct EndLiftSession std::string _destination; std::string _description; rxcpp::observable _obs; - rclcpp::TimerBase::SharedPtr _timer; void _init_obs(); - void _publish_session_end(); }; class Pending : public LegacyTask::PendingPhase diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp index 2f9479aa1..92208d4fe 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -83,7 +83,7 @@ const std::string& MoveRobot::ActivePhase::description() const MoveRobot::PendingPhase::PendingPhase( agv::RobotContextPtr context, std::vector waypoints, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional tail_period) : _context{std::move(context)}, _waypoints{std::move(waypoints)}, @@ -93,15 +93,29 @@ MoveRobot::PendingPhase::PendingPhase( std::ostringstream oss; const auto dest = destination( _waypoints.back(), _context->planner()->get_configuration().graph()); - oss << "Move to " << dest; + oss << "Move to " << dest << " <" << _waypoints.back().position().transpose() + << "> through " << _waypoints.size() << " points"; _description = oss.str(); } //============================================================================== std::shared_ptr MoveRobot::PendingPhase::begin() { + rmf_traffic::PlanId plan_id = 0; + if (_plan_id) + { + plan_id = *_plan_id; + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No plan_id was provided for MoveRobot action for robot [%s]. This is a " + "critical internal error, please report this bug to the RMF maintainers.", + _context->requester_id().c_str()); + } return std::make_shared( - _context, _waypoints, _plan_id, _tail_period); + _context, _waypoints, plan_id, _tail_period); } //============================================================================== @@ -128,7 +142,16 @@ MoveRobot::Action::Action( _plan_id{plan_id}, _tail_period{tail_period} { - // no op + _first_graph_index = [&]() -> std::optional + { + for (const auto& wp : _waypoints) + { + if (wp.graph_index().has_value()) + return wp.graph_index(); + } + + return std::nullopt; + }(); } } // namespace phases diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp index 64f3fd5f5..575730fcd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -84,7 +84,7 @@ struct MoveRobot PendingPhase( agv::RobotContextPtr context, std::vector waypoints, - rmf_traffic::PlanId plan_id, + PlanIdPtr plan_id, std::optional tail_period); std::shared_ptr begin() override; @@ -97,7 +97,7 @@ struct MoveRobot agv::RobotContextPtr _context; std::vector _waypoints; - rmf_traffic::PlanId _plan_id; + PlanIdPtr _plan_id; std::optional _tail_period; std::string _description; }; @@ -123,6 +123,7 @@ struct MoveRobot std::optional _tail_period; std::optional _last_tail_bump; std::size_t _next_path_index = 0; + std::optional _first_graph_index; rclcpp::TimerBase::SharedPtr _update_timeout_timer; rclcpp::Time _last_update_rostime; @@ -138,164 +139,250 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (!command) return; - _last_update_rostime = _context->node()->now(); - _update_timeout_timer = _context->node()->try_create_wall_timer( - _update_timeout, [w = weak_from_this()]() + _context->worker().schedule([w = weak_from_this(), s](const auto&) { const auto self = w.lock(); if (!self) return; - const auto now = self->_context->node()->now(); - if (now < self->_last_update_rostime + self->_update_timeout) - { - // The simulation is paused or running slowly, so we should allow more - // patience before assuming that there's been a timeout. - return; - } - - self->_last_update_rostime = now; - - // The RobotCommandHandle seems to have frozen up. Perhaps a bug in the - // user's code has caused the RobotCommandHandle to drop the command. We - // will request a replan. - RCLCPP_WARN( - self->_context->node()->get_logger(), - "Requesting replan for [%s] because its command handle seems to be " - "unresponsive", - self->_context->requester_id().c_str()); - self->_context->request_replan(); - }); + self->_last_update_rostime = self->_context->node()->now(); + self->_update_timeout_timer = self->_context->node()->try_create_wall_timer( + self->_update_timeout, + [w = self->weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; - _context->command()->follow_new_path( - _waypoints, - [s, w_action = weak_from_this(), r = _context->requester_id()]( - std::size_t path_index, rmf_traffic::Duration estimate) - { - const auto action = w_action.lock(); - if (!action) - return; + const auto now = self->_context->node()->now(); + if (now < self->_last_update_rostime + self->_update_timeout) + { + // The simulation is paused or running slowly, so we should allow more + // patience before assuming that there's been a timeout. + return; + } - action->_last_update_rostime = action->_context->node()->now(); - action->_update_timeout_timer->reset(); + self->_last_update_rostime = now; + + // The RobotCommandHandle seems to have frozen up. Perhaps a bug in the + // user's code has caused the RobotCommandHandle to drop the command. We + // will request a replan. + RCLCPP_WARN( + self->_context->node()->get_logger(), + "Requesting replan for [%s] because its command handle seems to be " + "unresponsive", + self->_context->requester_id().c_str()); + self->_context->request_replan(); + }); - if (path_index == action->_waypoints.size()-1 - && estimate < std::chrono::seconds(1) - && action->_tail_period.has_value()) + const auto update = [ + s, + w_action = self->weak_from_this(), + r = self->_context->requester_id() + ]( + std::size_t path_index, rmf_traffic::Duration estimate) { - const auto now = action->_context->now(); - if (!action->_last_tail_bump.has_value() - || *action->_last_tail_bump + *action->_tail_period < now) + const auto action = w_action.lock(); + if (!action) + return; + + action->_last_update_rostime = action->_context->node()->now(); + action->_update_timeout_timer->reset(); + + if (path_index == action->_waypoints.size()-1 + && estimate < std::chrono::seconds(1) + && action->_tail_period.has_value()) { - action->_last_tail_bump = now; - action->_context->worker().schedule( - [ - context = action->_context, - bump = *action->_tail_period, - plan_id = action->_plan_id - ]( - const auto&) - { - if (const auto c = context->itinerary().cumulative_delay(plan_id)) + const auto now = action->_context->now(); + if (!action->_last_tail_bump.has_value() + || *action->_last_tail_bump + *action->_tail_period < now) + { + action->_last_tail_bump = now; + action->_context->worker().schedule( + [ + context = action->_context, + bump = *action->_tail_period, + plan_id = action->_plan_id + ]( + const auto&) { - context->itinerary().cumulative_delay(plan_id, *c + bump); - } - }); + if (const auto c = + context->itinerary().cumulative_delay(plan_id)) + { + context->itinerary().cumulative_delay(plan_id, *c + bump); + } + }); + } } - } - if (path_index != action->_next_path_index) - { - action->_next_path_index = path_index; - LegacyTask::StatusMsg msg; - msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; - - if (path_index < action->_waypoints.size()) + if (path_index != action->_next_path_index) { - msg.status = "Heading towards " - + destination( - action->_waypoints[path_index], - action->_context->planner()->get_configuration().graph()); + action->_next_path_index = path_index; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; + + if (path_index < action->_waypoints.size()) + { + msg.status = "Heading towards " + + destination( + action->_waypoints[path_index], + action->_context->planner()->get_configuration().graph()); + } + else + { + // TODO(MXG): This should really be a warning, but the legacy phase shim + // does not have a way for us to specify a warning. + msg.status = "[Bug] [MoveRobot] Current path index was specified as [" + + std::to_string(path_index) + "] but that exceeds the limit of [" + + std::to_string(action->_waypoints.size()-1) + "]"; + } + + s.on_next(msg); } - else + + if (action->_next_path_index > action->_waypoints.size()) { - // TODO(MXG): This should really be a warning, but the legacy phase shim - // does not have a way for us to specify a warning. - msg.status = "[Bug] [MoveRobot] Current path index was specified as [" - + std::to_string(path_index) + "] but that exceeds the limit of [" - + std::to_string(action->_waypoints.size()-1) + "]"; + return; } - s.on_next(msg); - } - - if (action->_next_path_index > action->_waypoints.size()) - { - return; - } - - if (action->_plan_id != action->_context->itinerary().current_plan_id()) - { - // If the current Plan ID of the itinerary does not match the Plan ID - // of this action, then we should not modify the delay here. - return; - } - - const auto& target_wp = action->_waypoints[path_index]; - using namespace std::chrono_literals; - const rmf_traffic::Time now = action->_context->now(); - const auto planned_time = target_wp.time(); - const auto newly_expected_arrival = now + estimate; - const auto new_cumulative_delay = newly_expected_arrival - planned_time; - action->_context->worker().schedule( - [ - context = action->_context, - plan_id = action->_plan_id, - now, - new_cumulative_delay - ](const auto&) + if (action->_plan_id != action->_context->itinerary().current_plan_id()) { - context->itinerary().cumulative_delay( - plan_id, new_cumulative_delay, 100ms); + // If the current Plan ID of the itinerary does not match the Plan ID + // of this action, then we should not modify the delay here. + return; + } - const auto& itin = context->itinerary().itinerary(); - for (std::size_t i = 0; i < itin.size(); ++i) + const auto& target_wp = action->_waypoints[path_index]; + using namespace std::chrono_literals; + const rmf_traffic::Time now = action->_context->now(); + const auto planned_time = target_wp.time(); + const auto newly_expected_arrival = now + estimate; + const auto new_cumulative_delay = newly_expected_arrival - planned_time; + + action->_context->worker().schedule( + [ + w = action->weak_from_this(), + now, + new_cumulative_delay + ](const auto&) { - const auto& traj = itin[i].trajectory(); - const auto t_it = traj.find(now); - if (t_it != traj.end() && t_it != traj.begin()) + const auto self = w.lock(); + if (!self) + return; + + const auto context = self->_context; + const auto plan_id = self->_plan_id; + context->itinerary().cumulative_delay( + plan_id, new_cumulative_delay, 100ms); + + // This itinerary has been adjusted according to the latest delay + // information, so our position along the trajectory is given by `now` + const auto& itin = context->itinerary().itinerary(); + for (std::size_t i = 0; i < itin.size(); ++i) { - if (t_it->time() == now) + const auto& traj = itin[i].trajectory(); + const auto t_it = traj.find(now); + if (t_it != traj.end() && t_it != traj.begin()) { - context->itinerary().reached(plan_id, i, t_it->index()); + std::size_t index = t_it->index() - 1; + if (t_it->time() == now) + { + index = t_it->index(); + } + + context->itinerary().reached(plan_id, i, index); } - else + } + + if (!context->locked_mutex_groups().empty()) + { + const auto adjusted_now = now - new_cumulative_delay; + const auto& graph = context->navigation_graph(); + std::unordered_set retain_mutexes; + for (const auto& wp : self->_waypoints) { - context->itinerary().reached(plan_id, i, t_it->index() - 1); + const auto s_100 = + (int)(rmf_traffic::time::to_seconds(adjusted_now - + wp.time()) * 100); + const auto s = (double)(s_100)/100.0; + if (wp.time() < adjusted_now) + { + continue; + } + + if (wp.graph_index().has_value()) + { + retain_mutexes.insert( + graph.get_waypoint(*wp.graph_index()).in_mutex_group()); + } + + for (const auto& l : wp.approach_lanes()) + { + retain_mutexes.insert( + graph.get_lane(l).properties().in_mutex_group()); + } } + + context->retain_mutex_groups(retain_mutexes); } - } - }); - }, - [s, w = weak_from_this()]() - { - if (const auto self = w.lock()) + }); + }; + + const auto finish = [ + s, + w = self->weak_from_this(), + name = self->_context->requester_id() + ]() { - if (!self->_waypoints.empty()) + if (const auto self = w.lock()) { - for (const auto& c : self->_waypoints.back().arrival_checkpoints()) + if (!self->_waypoints.empty()) { - self->_context->itinerary().reached( - self->_plan_id, c.route_id, c.checkpoint_id); + for (const auto& c : self->_waypoints.back().arrival_checkpoints()) + { + self->_context->itinerary().reached( + self->_plan_id, c.route_id, c.checkpoint_id); + } + + const auto last_index = self->_waypoints.back().graph_index(); + if (last_index.has_value()) + { + const auto& graph = self->_context->navigation_graph(); + self->_context->retain_mutex_groups( + {graph.get_waypoint(*last_index).in_mutex_group()}); + } + + const auto now = self->_context->now(); + const auto cumulative_delay = now - self->_waypoints.back().time(); + self->_context->itinerary().cumulative_delay( + self->_plan_id, cumulative_delay, std::chrono::seconds(1)); } + + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; + msg.status = "move robot success"; + s.on_next(msg); + s.on_completed(); } + }; - LegacyTask::StatusMsg msg; - msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; - msg.status = "move robot success"; - s.on_next(msg); - s.on_completed(); - } + self->_context->command()->follow_new_path( + self->_waypoints, + [worker = self->_context->worker(), update]( + std::size_t path_index, rmf_traffic::Duration estimate) + { + worker.schedule([path_index, estimate, update](const auto&) + { + update(path_index, estimate); + }); + }, + [worker = self->_context->worker(), finish]() + { + worker.schedule([finish](const auto&) + { + finish(); + }); + }); }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index bc76dc870..57272a2db 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -18,6 +18,8 @@ #include "RequestLift.hpp" #include "EndLiftSession.hpp" #include "RxOperators.hpp" +#include "../agv/internal_RobotUpdateHandle.hpp" +#include "../agv/internal_EasyFullControl.hpp" namespace rmf_fleet_adapter { namespace phases { @@ -27,17 +29,16 @@ std::shared_ptr RequestLift::ActivePhase::make( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - const Located located) + Data data) { auto inst = std::shared_ptr( new ActivePhase( std::move(context), std::move(lift_name), std::move(destination), - std::move(expected_finish), - located + std::move(data) )); + inst->_init_obs(); return inst; } @@ -65,7 +66,7 @@ void RequestLift::ActivePhase::emergency_alarm(bool /*on*/) //============================================================================== void RequestLift::ActivePhase::cancel() { - _cancelled.get_subscriber().on_next(true); + // GoToPlace and ExecutePlan don't call the cancel function anyway } //============================================================================== @@ -79,13 +80,11 @@ RequestLift::ActivePhase::ActivePhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift [" << lift_name << "] to [" << destination << "]"; @@ -98,7 +97,49 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; + if (_data.located == Located::Outside && _context->current_lift_destination()) + { + // Check if the current destination is the one we want and also has arrived. + // If so, we can skip the rest of this process and just make an observable + // that says it's completed right away. + if (_context->current_lift_destination()->matches(_lift_name, _destination)) + { + _obs = rxcpp::observable<>::create( + [w = weak_from_this()](rxcpp::subscriber s) + { + const auto self = w.lock(); + if (!self) + return; + + if (self->_data.resume_itinerary) + { + self->_context->schedule_itinerary( + self->_data.plan_id, *self->_data.resume_itinerary); + const auto delay = + self->_context->now() - self->_data.expected_finish; + self->_context->itinerary().cumulative_delay( + *self->_data.plan_id, delay); + } + + s.on_completed(); + }); + return; + } + } + + if (_data.hold_point.has_value()) + { + *_data.plan_id = _context->itinerary().assign_plan_id(); + _context->schedule_hold( + _data.plan_id, + _data.hold_point->time(), + std::chrono::seconds(10), + _data.hold_point->position(), + _destination); + } + _obs = _context->node()->lift_state() + .observe_on(rxcpp::identity_same_worker(_context->worker())) .lift( on_subscribe( [weak = weak_from_this()]() @@ -119,17 +160,17 @@ void RequestLift::ActivePhase::_init_obs() // TODO(MXG): We can stop publishing the door request once the // supervisor sees our request. me->_do_publish(); - - const auto current_expected_finish = - me->_expected_finish + me->_context->itinerary().delay(); - - const auto delay = me->_context->now() - current_expected_finish; + const auto delay = me->_context->now() - me->_data.expected_finish; if (delay > std::chrono::seconds(0)) { me->_context->worker().schedule( - [context = me->_context, delay](const auto&) + [ + context = me->_context, + plan_id = *me->_data.plan_id, + delay + ](const auto&) { - context->itinerary().delay(delay); + context->itinerary().cumulative_delay(plan_id, delay); }); } }); @@ -166,21 +207,62 @@ void RequestLift::ActivePhase::_init_obs() if (!me) return; - // FIXME: is this thread-safe? - if (!me->_cancelled.get_value() || me->_located == Located::Inside) + if (me->_data.localize_after.has_value()) { - s.on_completed(); - } - else if (me->_located == Located::Outside) - { - auto transport = me->_context->node(); - me->_lift_end_phase = EndLiftSession::Active::make( + auto finish = [s, worker = me->_context->worker(), weak]() + { + worker.schedule([s, weak](const auto&) + { + if (const auto me = weak.lock()) + { + if (!me->_finish()) + { + return; + } + } + + s.on_completed(); + }); + }; + + auto cmd = agv::EasyFullControl + ::CommandExecution::Implementation::make_hold( me->_context, - me->_lift_name, - me->_destination); + me->_data.expected_finish, + *me->_data.plan_id, + std::move(finish)); - me->_lift_end_phase->observe().subscribe(s); + agv::Destination::Implementation::get(*me->_data.localize_after) + .position = me->_context->position(); + + if (me->_context->localize(*me->_data.localize_after, + std::move(cmd))) + { + me->_rewait_timer = me->_context->node()->try_create_wall_timer( + std::chrono::seconds(300), + [weak, s] + { + const auto me = weak.lock(); + if (!me) + return; + + RCLCPP_ERROR( + me->_context->node()->get_logger(), + "Waiting for robot [%s] to localize timed out. Please " + "ensure that your localization function triggers " + "execution.finished() when the robot's localization " + "process is finished.", + me->_context->requester_id().c_str()); + + if (me->_finish()) + s.on_completed(); + }); + return; + } } + + if (me->_finish()) + s.on_completed(); })); } @@ -198,6 +280,11 @@ LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status( lift_state->door_state == LiftState::DOOR_OPEN && lift_state->session_id == _context->requester_id()) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Lift has arrived on floor [%s] and opened its doors for robot [%s]", + lift_state->current_floor.c_str(), + lift_state->session_id.c_str()); bool completed = false; const auto& watchdog = _context->get_lift_watchdog(); @@ -254,7 +341,7 @@ LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status( _watchdog_info.reset(); } } - else if (_located == Located::Outside && watchdog) + else if (_data.located == Located::Outside && watchdog) { _watchdog_info = std::make_shared(); watchdog( @@ -303,15 +390,57 @@ void RequestLift::ActivePhase::_do_publish() if (_rewaiting) return; - rmf_lift_msgs::msg::LiftRequest msg{}; - msg.lift_name = _lift_name; - msg.destination_floor = _destination; - msg.session_id = _context->requester_id(); - msg.request_time = _context->node()->now(); - msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_AGV_MODE; - msg.door_state = rmf_lift_msgs::msg::LiftRequest::DOOR_OPEN; + if (!_destination_handle) + { + _destination_handle = _context->set_lift_destination( + _lift_name, _destination, _data.located == Located::Inside); + } +} + +//============================================================================== +bool RequestLift::ActivePhase::_finish() +{ + // The return value of _finish tells us whether we should have the observable + // proceed to trigger on_completed(). If we have already finished before then + // _finished will be true, so we should return false to indicate that the + // observable should not proceed to trigger on_completed(). + if (_finished) + return false; + + _finished = true; + + if (_data.located == Located::Outside) + { + // The robot is going to start moving into the lift now, so we should lock + // the destination in. + _context->set_lift_destination(_lift_name, _destination, true); + + // We should replan to make sure there are no traffic issues that came up + // in the time that we were waiting for the lift. + if (_data.hold_point.has_value()) + { + if (_data.hold_point->graph_index().has_value()) + { + auto start = rmf_traffic::agv::Plan::Start( + _context->now(), + _data.hold_point->graph_index().value(), + _data.hold_point->position()[2]); + _context->set_location({std::move(start)}); + } + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Requesting replan for [%s] because it has finished waiting lift [%s] " + "to arrive at [%s]", + _context->requester_id().c_str(), + _lift_name.c_str(), + _destination.c_str()); + _context->request_replan(); + return false; + } - _context->node()->lift_request()->publish(msg); + return true; } //============================================================================== @@ -319,13 +448,11 @@ RequestLift::PendingPhase::PendingPhase( agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located) + Data data) : _context(std::move(context)), _lift_name(std::move(lift_name)), _destination(std::move(destination)), - _expected_finish(std::move(expected_finish)), - _located(located) + _data(std::move(data)) { std::ostringstream oss; oss << "Requesting lift \"" << lift_name << "\" to \"" << destination << "\""; @@ -340,8 +467,7 @@ std::shared_ptr RequestLift::PendingPhase::begin() _context, _lift_name, _destination, - _expected_finish, - _located); + _data); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp index 6197ee306..cc6b9233d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -34,6 +34,17 @@ struct RequestLift Outside }; + struct Data + { + rmf_traffic::Time expected_finish; + Located located; + PlanIdPtr plan_id; + std::optional localize_after = std::nullopt; + std::shared_ptr resume_itinerary = + nullptr; + std::optional hold_point = std::nullopt; + }; + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { @@ -43,8 +54,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); const rxcpp::observable& observe() const override; @@ -61,15 +71,16 @@ struct RequestLift agv::RobotContextPtr _context; std::string _lift_name; std::string _destination; - rmf_traffic::Time _expected_finish; + Data _data; rxcpp::subjects::behavior _cancelled = rxcpp::subjects::behavior(false); std::string _description; rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; std::shared_ptr _lift_end_phase; - Located _located; rmf_rxcpp::subscription_guard _reset_session_subscription; + std::shared_ptr _destination_handle; + bool _finished = false; struct WatchdogInfo { @@ -85,8 +96,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); void _init_obs(); @@ -94,6 +104,7 @@ struct RequestLift const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state); void _do_publish(); + bool _finish(); }; class PendingPhase : public LegacyTask::PendingPhase @@ -104,8 +115,7 @@ struct RequestLift agv::RobotContextPtr context, std::string lift_name, std::string destination, - rmf_traffic::Time expected_finish, - Located located); + Data data); std::shared_ptr begin() override; @@ -123,8 +133,8 @@ struct RequestLift std::string _lift_name; std::string _destination; rmf_traffic::Time _expected_finish; - Located _located; std::string _description; + Data _data; }; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index b19b4206d..cd48937c2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -32,8 +32,9 @@ rmf_traffic::Duration WaitForCharge::Active::estimate_remaining_time() const { const double capacity = _battery_system.capacity(); const double charging_current = _battery_system.charging_current(); + const double charge_to_soc = _charge_to_soc.value_or(1.0); const double time_estimate = - 3600.0 * capacity * (_charge_to_soc - _context->current_battery_soc()) / + 3600.0 * capacity * (charge_to_soc - _context->current_battery_soc()) / charging_current; return rmf_traffic::time::from_seconds(time_estimate); @@ -48,7 +49,7 @@ void WaitForCharge::Active::emergency_alarm(const bool) //============================================================================== void WaitForCharge::Active::cancel() { - // TODO + _status_publisher.get_subscriber().on_completed(); } //============================================================================== @@ -61,7 +62,7 @@ const std::string& WaitForCharge::Active::description() const WaitForCharge::Active::Active( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, rmf_traffic::Time start_time) : _context(std::move(context)), _battery_system(battery_system), @@ -75,7 +76,12 @@ WaitForCharge::Active::Active( _battery_system.charging_current())); _description = "Charging [" + _context->requester_id() + "] to [" - + std::to_string(100.0 * _charge_to_soc) + "]"; + + std::to_string(100.0 * _charge_to_soc.value_or(1.0)) + "]"; + + RCLCPP_INFO( + _context->node()->get_logger(), + "%s", + _description.c_str()); StatusMsg initial_msg; initial_msg.status = _description; @@ -105,7 +111,7 @@ std::shared_ptr WaitForCharge::Pending::begin() "Robot [%s] has begun waiting for its battery to charge to %.1f%%. " "Please ensure that the robot is charging.", _context->name().c_str(), - _charge_to_soc * 100.0); + _charge_to_soc.value_or(1.0) * 100.0); active->_battery_soc_subscription = _context->observe_battery_soc() .observe_on(rxcpp::identity_same_worker(_context->worker())) @@ -117,9 +123,12 @@ std::shared_ptr WaitForCharge::Pending::begin() if (!active) return; - if (active->_charge_to_soc <= battery_soc) + if (active->_charge_to_soc.has_value()) { - active->_status_publisher.get_subscriber().on_completed(); + if (*active->_charge_to_soc <= battery_soc) + { + active->_status_publisher.get_subscriber().on_completed(); + } } const auto& now = std::chrono::steady_clock::now(); @@ -139,14 +148,13 @@ std::shared_ptr WaitForCharge::Pending::begin() "is %.1f %%/hour. If the battery percentage has not been rising, " "please check that the robot is connected to its charger.", active->_context->name().c_str(), - active->_charge_to_soc * 100.0, + active->_charge_to_soc.value_or(1.0) * 100.0, battery_soc * 100, average_charging_rate, active->_expected_charging_rate); active->_last_update_time = now; } - }); return active; @@ -168,7 +176,7 @@ const std::string& WaitForCharge::Pending::description() const WaitForCharge::Pending::Pending( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, double time_estimate) : _context(std::move(context)), _battery_system(battery_system), @@ -176,20 +184,21 @@ WaitForCharge::Pending::Pending( _time_estimate(time_estimate) { _description = - "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]"; + "Charging robot to [" + std::to_string(100.0 * charge_to_soc.value_or(1.0)) + + "%]"; } //============================================================================== auto WaitForCharge::make( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc) -> std::unique_ptr + std::optional charge_to_soc) -> std::unique_ptr { - const double capacity = battery_system.capacity(); const double charging_current = battery_system.charging_current(); const double time_estimate = - 3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / + 3600.0 * capacity + * (charge_to_soc.value_or(1.0) - context->current_battery_soc()) / charging_current; return std::unique_ptr( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index b603fcd92..b61389366 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -62,12 +62,12 @@ class WaitForCharge Active( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, rmf_traffic::Time start_time); agv::RobotContextPtr _context; rmf_battery::agv::BatterySystem _battery_system; - double _charge_to_soc; + std::optional _charge_to_soc; std::string _description; rxcpp::observable _status_obs; rxcpp::subjects::subject _status_publisher; @@ -96,12 +96,12 @@ class WaitForCharge Pending( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc, + std::optional charge_to_soc, double time_estimate); agv::RobotContextPtr _context; rmf_battery::agv::BatterySystem _battery_system; - double _charge_to_soc; + std::optional _charge_to_soc; std::string _description; double _time_estimate; }; @@ -109,7 +109,7 @@ class WaitForCharge static std::unique_ptr make( agv::RobotContextPtr context, rmf_battery::agv::BatterySystem battery_system, - double charge_to_soc); + std::optional charge_to_soc); }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 06c5a02bb..863704d0b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -16,7 +16,7 @@ */ #include "../phases/WaitForCharge.hpp" - +#include "../events/WaitForCancel.hpp" #include "../events/GoToPlace.hpp" #include "../events/LegacyPhaseShim.hpp" @@ -28,15 +28,476 @@ #include +#include + +#include namespace rmf_fleet_adapter { namespace tasks { +//============================================================================== +rmf_traffic::Duration estimate_charge_time( + const double initial_soc, + const double recharged_soc, + const rmf_battery::agv::BatterySystem& battery_system) +{ + if (initial_soc < recharged_soc) + { + const double delta_soc = recharged_soc - initial_soc; + const double dt = (3600 * delta_soc * battery_system.capacity()) / + battery_system.charging_current(); + return rmf_traffic::time::from_seconds(dt); + } + + return rmf_traffic::Duration(0); +} + +//============================================================================== +// TODO(MXG): Consider making the ChargeBatteryEvent public so it +// can be incorporated into other task types +class ChargeBatteryEvent : public rmf_task_sequence::Event +{ +public: + + class Model : public rmf_task_sequence::Activity::Model + { + public: + static rmf_task_sequence::Activity::ConstModelPtr make( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) + { + const auto model = std::shared_ptr( + new Model(invariant_initial_state, parameters)); + model->_invariant_initial_state = invariant_initial_state; + + const auto wp_opt = invariant_initial_state.dedicated_charging_waypoint(); + if (wp_opt.has_value()) + { + const auto go_to_place_desc = + rmf_task_sequence::events::GoToPlace::Description::make(*wp_opt); + model->_go_to_place = go_to_place_desc->make_model( + invariant_initial_state, parameters); + } + + model->_parameters = parameters; + return model; + } + + std::optional estimate_finish( + rmf_task::State state, + rmf_traffic::Time earliest_arrival_time, + const rmf_task::Constraints& constraints, + const rmf_task::TravelEstimator& travel_estimator) const final + { + rmf_traffic::Time wait_until = earliest_arrival_time; + if (_go_to_place) + { + const auto estimate_go_to = _go_to_place->estimate_finish( + std::move(state), earliest_arrival_time, constraints, + travel_estimator); + if (!estimate_go_to.has_value()) + return std::nullopt; + + wait_until = estimate_go_to->wait_until(); + state = estimate_go_to->finish_state(); + } + + const auto recharged_soc = constraints.recharge_soc(); + rmf_traffic::Duration dt = rmf_traffic::Duration(0); + const auto initial_soc_opt = state.battery_soc(); + if (!initial_soc_opt.has_value()) + { + // Assume no charging is needed if the "robot" does not have a battery + return rmf_task::Estimate(state, wait_until); + } + + const double initial_soc = *initial_soc_opt; + if (initial_soc < recharged_soc) + { + state.battery_soc(recharged_soc); + } + wait_until += estimate_charge_time( + initial_soc, + recharged_soc, + _parameters.battery_system()); + + return rmf_task::Estimate(state, wait_until); + } + + rmf_traffic::Duration invariant_duration() const final + { + return rmf_traffic::Duration(0); + } + + rmf_task::State invariant_finish_state() const final + { + return _invariant_initial_state; + } + + private: + Model( + rmf_task::State invariant_initial_state, + rmf_task::Parameters parameters) + : _invariant_initial_state(std::move(invariant_initial_state)), + _parameters(std::move(parameters)) + { + // Do nothing + } + rmf_task::State _invariant_initial_state; + rmf_task_sequence::Activity::ConstModelPtr _go_to_place; + rmf_task::Parameters _parameters; + }; + + class Description : public rmf_task_sequence::Activity::Description + { + public: + Description( + std::optional specific_location_, + bool indefinite_, + bool park_) + : specific_location(specific_location_), + indefinite(indefinite_), + park(park_) + { + // Do nothing + } + + std::optional specific_location; + bool indefinite; + bool park; + + rmf_task_sequence::Activity::ConstModelPtr make_model( + rmf_task::State invariant_initial_state, + const rmf_task::Parameters& parameters) const final + { + return Model::make(std::move(invariant_initial_state), parameters); + } + + rmf_task::Header generate_header( + const rmf_task::State& initial_state, + const rmf_task::Parameters& parameters) const final + { + rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0); + double recharged_soc = 1.0; + if (const auto c = initial_state.get()) + { + if (const auto context = c->value) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + + const auto header_go_to = + rmf_task_sequence::events::GoToPlace::Description::make( + context->dedicated_charging_wp())->generate_header( + initial_state, parameters); + duration_estimate += header_go_to.original_duration_estimate(); + } + } + + double initial_soc = initial_state.battery_soc().value_or(0.0); + duration_estimate += estimate_charge_time( + initial_soc, recharged_soc, parameters.battery_system()); + + std::string name = park ? "Park" : "Charge Battery"; + + return rmf_task::Header(name, "", duration_estimate); + } + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + static std::shared_ptr make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + std::function update) + { + const auto state = get_state(); + const auto context = state.get()->value; + const auto header = description.generate_header(state, *parameters); + + auto standby = std::shared_ptr(new Standby(description)); + standby->_assign_id = id; + standby->_context = context; + standby->_time_estimate = header.original_duration_estimate(); + standby->_update = std::move(update); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + header.category(), + header.detail(), + rmf_task::Event::Status::Standby, + {}, + context->clock()); + + return standby; + } + + ConstStatePtr state() const final + { + return _state; + } + + rmf_traffic::Duration duration_estimate() const final + { + return _time_estimate; + } + + ActivePtr begin( + std::function, + std::function finished) final + { + if (!_active) + { + const std::string& task = _desc.park ? "parking" : "charging"; + + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new %s task for robot [%s]", + task.c_str(), + _context->requester_id().c_str()); + + _active = Active::make( + _desc, + _assign_id, + _context, + _state, + _update, + std::move(finished)); + } + + return _active; + } + + private: + Standby(Description desc) + : _desc(desc) + { + // Do nothing + } + Description _desc; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_traffic::Duration _time_estimate; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + }; + + class Active : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + static std::shared_ptr make( + Description desc, + AssignIDPtr assign_id, + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) + { + auto active = std::shared_ptr(new Active(desc)); + active->_assign_id = std::move(assign_id); + active->_context = std::move(context); + active->_state = std::move(state); + active->_update = std::move(update); + active->_finished = std::move(finished); + + active->_charging_update_subscription = active + ->_context + ->observe_charging_change() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe( + [w = active->weak_from_this()](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + self->_consider_restart(); + }); + + active->_consider_restart(); + return active; + } + + rmf_task::Event::ConstStatePtr state() const final + { + return _state; + } + + rmf_traffic::Duration remaining_time_estimate() const final + { + if (_sequence) + return _sequence->remaining_time_estimate(); + + return rmf_traffic::Duration(0); + } + + Backup backup() const final + { + return Backup::make(0, nlohmann::json()); + } + + Resume interrupt(std::function task_is_interrupted) final + { + if (_sequence) + _sequence->interrupt(std::move(task_is_interrupted)); + + _current_target_wp = std::nullopt; + _current_waiting_for_charger = std::nullopt; + return Resume::make( + [w = weak_from_this()]() + { + if (const auto self = w.lock()) + { + self->_consider_restart(); + } + }); + } + + void cancel() final + { + if (_sequence) + _sequence->cancel(); + } + + void kill() final + { + if (_sequence) + _sequence->kill(); + } + + private: + + Active(Description desc) + : _desc(desc) + { + // Do nothing + } + void _consider_restart() + { + std::size_t target_wp = _context->dedicated_charging_wp(); + if (!_desc.specific_location.has_value()) + { + bool location_changed = true; + if (_current_target_wp.has_value()) + { + if (target_wp == *_current_target_wp) + { + location_changed = false; + } + } + + bool waiting_changed = true; + if (_current_waiting_for_charger.has_value()) + { + if (_context->waiting_for_charger() == *_current_waiting_for_charger) + { + waiting_changed = false; + } + } + + + if (!location_changed && !waiting_changed) + { + // No need to do anything, the charging location has not changed + // nor has the mode changed. + return; + } + } + else + { + target_wp = _desc.specific_location.value(); + if (_current_target_wp == target_wp) + return; + } + + _current_target_wp = target_wp; + _current_waiting_for_charger = _context->waiting_for_charger(); + + using UpdateFn = std::function; + using MakeStandby = std::function; + std::vector standbys; + + using GoToPlaceDesc = rmf_task_sequence::events::GoToPlace::Description; + standbys.push_back( + [ + target_wp, + assign_id = _assign_id, + context = _context + ](UpdateFn update) -> StandbyPtr + { + return events::GoToPlace::Standby::make( + assign_id, + context->make_get_state(), + context->task_parameters(), + *GoToPlaceDesc::make(target_wp), + update); + }); + + if (_desc.park) + { + standbys.push_back( + [ + assign_id = _assign_id, + context = _context + ](UpdateFn update) -> StandbyPtr + { + return events::WaitForCancel::Standby::make(context, assign_id); + }); + } + else + { + standbys.push_back( + [ + assign_id = _assign_id, + context = _context, + indefinite = _desc.indefinite + ](UpdateFn update) -> StandbyPtr + { + std::optional recharged_soc; + if (!indefinite) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + } + + auto legacy = phases::WaitForCharge::make( + context, + context->task_parameters()->battery_system(), + recharged_soc); + + return events::LegacyPhaseShim::Standby::make( + std::move(legacy), context->worker(), context->clock(), assign_id, + std::move(update)); + }); + } + + _sequence = rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + standbys, _state, _update)->begin([]() {}, _finished); + } + + Description _desc; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_task::events::SimpleEventStatePtr _state; + std::function _update; + std::function _finished; + rmf_rxcpp::subscription_guard _charging_update_subscription; + std::optional _current_target_wp; + std::optional _current_waiting_for_charger; + rmf_task_sequence::Event::ActivePtr _sequence; + }; +}; + //============================================================================== struct GoToChargerDescription : public rmf_task_sequence::events::Placeholder::Description { GoToChargerDescription() - : rmf_task_sequence::events::Placeholder::Description("Go to charger", "") + : rmf_task_sequence::events::Placeholder::Description("Go to place", "") { // Do nothing } @@ -54,7 +515,7 @@ struct GoToChargerDescription return events::GoToPlace::Standby::make( id, get_state, parameters, - *GoToPlace::make(context->dedicated_charger_wp()), + *GoToPlace::make(context->dedicated_charging_wp()), std::move(update)); } @@ -92,27 +553,37 @@ struct GoToChargerDescription struct WaitForChargeDescription : public rmf_task_sequence::events::Placeholder::Description { - WaitForChargeDescription() + WaitForChargeDescription(bool indefinite_) : rmf_task_sequence::events::Placeholder::Description( - "Wait for charging", "") + "Wait for charging", ""), + indefinite(indefinite_) { // Do nothing } + bool indefinite; + static rmf_task_sequence::Event::StandbyPtr standby( const rmf_task_sequence::Event::AssignIDPtr& id, const std::function& get_state, const rmf_task::ConstParametersPtr& parameters, - const WaitForChargeDescription&, + const WaitForChargeDescription& desc, std::function update) { const auto state = get_state(); const auto context = state.get()->value; + std::optional recharged_soc; + if (!desc.indefinite) + { + recharged_soc = context->task_planner() + ->configuration().constraints().recharge_soc(); + } + auto legacy = phases::WaitForCharge::make( context, parameters->battery_system(), - context->task_planner()->configuration().constraints().recharge_soc()); + recharged_soc); return events::LegacyPhaseShim::Standby::make( std::move(legacy), context->worker(), context->clock(), id, @@ -149,20 +620,6 @@ struct WaitForChargeDescription } }; -//============================================================================== -// TODO(MXG): Consider making the ChargeBatteryEvent description public so it -// can be incorporated into other task types -struct ChargeBatteryEventDescription - : public rmf_task_sequence::events::Placeholder::Description -{ - ChargeBatteryEventDescription() - : rmf_task_sequence::events::Placeholder::Description( - "Charge Battery", "") - { - // Do nothing - } -}; - //============================================================================== void add_charge_battery( rmf_task::Activator& task_activator, @@ -172,46 +629,128 @@ void add_charge_battery( { using Bundle = rmf_task_sequence::events::Bundle; using Phase = rmf_task_sequence::phases::SimplePhase; - using ChargeBattery = rmf_task::requests::ChargeBattery; + using ChargeBatteryTask = rmf_task::requests::ChargeBattery; - auto private_initializer = - std::make_shared(); - - WaitForChargeDescription::add(*private_initializer); - GoToChargerDescription::add(*private_initializer); - - auto charge_battery_event_unfolder = - [](const ChargeBatteryEventDescription&) + event_initializer.add( + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const ChargeBatteryEvent::Description& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr { - return Bundle::Description({ - std::make_shared(), - std::make_shared() - }, Bundle::Sequence, "Charge Battery"); - }; - - Bundle::unfold( - std::move(charge_battery_event_unfolder), - event_initializer, private_initializer); + return ChargeBatteryEvent::Standby::make( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const ChargeBatteryEvent::Description& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return ChargeBatteryEvent::Standby::make( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); auto charge_battery_task_unfolder = - [](const rmf_task::requests::ChargeBattery::Description&) + [](const rmf_task::requests::ChargeBattery::Description& desc) { rmf_task_sequence::Task::Builder builder; builder .add_phase( Phase::Description::make( - std::make_shared(), + std::make_shared( + std::nullopt, desc.indefinite(), false), "Charge Battery", ""), {}); return *builder.build("Charge Battery", ""); }; - rmf_task_sequence::Task::unfold( + rmf_task_sequence::Task::unfold( std::move(charge_battery_task_unfolder), task_activator, phase_activator, std::move(clock)); } +//============================================================================== +namespace { +std::string generate_uuid(const std::size_t length = 3) +{ + std::stringstream ss; + for (std::size_t i = 0; i < length; ++i) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis(0, 255); + const auto random_char = dis(gen); + std::stringstream hexstream; + hexstream << std::hex << random_char; + auto hex = hexstream.str(); + ss << (hex.length() < 2 ? '0' + hex : hex); + } + return ss.str(); +} + +} // anonymous namespace + +//============================================================================== +class ParkRobotIndefinitely::Implementation +{ +public: + std::string requester; + std::function time_now_cb; + std::optional parking_waypoint; +}; + +//============================================================================== +ParkRobotIndefinitely::ParkRobotIndefinitely( + const std::string& requester, + std::function time_now_cb, + std::optional parking_waypoint) +: _pimpl(rmf_utils::make_impl(Implementation { + requester, + time_now_cb, + parking_waypoint + })) +{ + // Do nothing +} + +//============================================================================== +rmf_task::ConstRequestPtr ParkRobotIndefinitely::make_request( + const rmf_task::State& state) const +{ + std::string id = "ParkRobot-" + generate_uuid(); + auto phase_desc = std::make_shared( + _pimpl->parking_waypoint, true, true); + + auto desc = rmf_task_sequence::Task::Builder() + .add_phase(rmf_task_sequence::phases::SimplePhase::Description::make( + phase_desc), {}) + .build("Park", ""); + + auto now = _pimpl->time_now_cb ? + _pimpl->time_now_cb() : state.time().value_or( + rmf_traffic::Time(std::chrono::system_clock::now().time_since_epoch())); + rmf_task::Task::ConstBookingPtr booking = + std::make_shared( + std::move(id), + now, + nullptr, + _pimpl->requester, + now, + true); + + return std::make_shared( + std::move(booking), + std::move(desc)); +} + } // namespace task } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index 55657bbcf..b0215f55f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -33,14 +33,6 @@ namespace rmf_fleet_adapter { namespace tasks { -//============================================================================== -std::shared_ptr make_charge_battery( - const rmf_task::ConstRequestPtr request, - const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start start, - const rmf_traffic::Time deployment_time, - const rmf_task::State finish_state); - //============================================================================== void add_charge_battery( rmf_task::Activator& task_activator, diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index 6e2a76c1f..8e2d23798 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -60,6 +60,8 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") test->received_requests.emplace_back(*lift_request); test->received_requests_cv.notify_all(); }); + auto lift_state_pub = data->ros_node->create_publisher( + LiftStateTopicName, 10); const auto info = add_robot(); const auto& context = info.context; @@ -70,41 +72,13 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") context, lift_name, destination, - context->now() + std::chrono::seconds(5), - RequestLift::Located::Outside - ); + RequestLift::Data{ + context->now() + std::chrono::seconds(5), + RequestLift::Located::Outside, + std::make_shared(0) + }); auto active_phase = pending_phase->begin(); - WHEN("it is cancelled before its started") - { - THEN("it should not send lift requests") - { - bool received_open = false; - rxcpp::composite_subscription rx_sub; - auto subscription = - data->adapter->node()->create_subscription( - AdapterLiftRequestTopicName, 10, - [&rx_sub, &received_open](LiftRequest::UniquePtr lift_request) - { - if (lift_request->request_type != LiftRequest::REQUEST_END_SESSION) - received_open = true; - else - rx_sub.unsubscribe(); - }); - - auto obs = active_phase->observe(); - active_phase->cancel(); - - // TODO(MXG): Put an explicit timeout here so this line doesn't hang - // forever in the event of a failure. - obs.as_blocking().subscribe(rx_sub); - CHECK(!received_open); - - // Stop before destructing subscription to avoid a data race in rclcpp - data->node->stop(); - } - } - WHEN("it is started") { rmf_rxcpp::subscription_guard sub = active_phase->observe().subscribe( @@ -129,8 +103,16 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") CHECK(test->received_requests.front().destination_floor == destination); } - THEN("it should continuously send lift requests") + THEN("it should continuously send lift requests while lift states arrive") { + auto t = std::thread([&]() + { + for (std::size_t i = 0; i < 10; ++i) + { + lift_state_pub->publish(LiftState()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); std::unique_lock lk(test->m); test->received_requests_cv.wait(lk, [test]() { @@ -140,12 +122,12 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") { CHECK(lift_request.destination_floor == destination); } + + t.join(); } AND_WHEN("lift is on destination floor") { - auto lift_state_pub = data->ros_node->create_publisher( - LiftStateTopicName, 10); auto w_lift_state_pub = std::weak_ptr>(lift_state_pub); rclcpp::TimerBase::SharedPtr timer = data->node->try_create_wall_timer( @@ -198,29 +180,6 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") timer.reset(); } - - AND_WHEN("it is cancelled") - { - { - std::unique_lock lk(test->m); - test->received_requests_cv.wait(lk, [&]() - { - return !test->received_requests.empty(); - }); - active_phase->cancel(); - } - - THEN("it should send END_SESSION request") - { - std::unique_lock lk(test->m); - test->received_requests_cv.wait(lk, [&]() - { - if (test->received_requests.empty()) - return false; - return test->received_requests.back().request_type == LiftRequest::REQUEST_END_SESSION; - }); - } - } } data->node->stop(); diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index ac83f9a68..99c028e5b 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -155,6 +155,11 @@ PYBIND11_MODULE(rmf_adapter, m) { { return self.maximum_delay(); }) + .def("current_task_id", + [&](agv::RobotUpdateHandle& self) + { + return self.current_task_id(); + }) .def("set_infinite_delay", [&](agv::RobotUpdateHandle& self) { @@ -241,6 +246,12 @@ PYBIND11_MODULE(rmf_adapter, m) { { return self.unstable().be_stubborn(); }) + .def("unstable_debug_positions", + [&](agv::RobotUpdateHandle& self, bool on) + { + self.unstable().debug_positions(on); + }, + py::arg("on")) .def("set_action_executor", &agv::RobotUpdateHandle::set_action_executor, py::arg("action_executor")) @@ -291,7 +302,8 @@ PYBIND11_MODULE(rmf_adapter, m) { return lhs == rhs; }); - py::class_( + py::class_>( m_robot_update_handle, "ActionExecution") .def("update_remaining_time", &ActionExecution::update_remaining_time, @@ -812,7 +824,12 @@ PYBIND11_MODULE(rmf_adapter, m) { &agv::EasyFullControl::RobotCallbacks::stop) .def_property_readonly( "action_executor", - &agv::EasyFullControl::RobotCallbacks::action_executor); + &agv::EasyFullControl::RobotCallbacks::action_executor) + .def_property( + "localize", + &agv::EasyFullControl::RobotCallbacks::localize, + &agv::EasyFullControl::RobotCallbacks::with_localization + ); py::class_(m_easy_full_control, "CommandExecution") .def("finished", &agv::EasyFullControl::CommandExecution::finished) @@ -820,7 +837,7 @@ PYBIND11_MODULE(rmf_adapter, m) { .def( "override_schedule", []( - ActionExecution& self, + agv::EasyFullControl::CommandExecution& self, std::string map, std::vector path, double hold) @@ -841,8 +858,10 @@ PYBIND11_MODULE(rmf_adapter, m) { .def_property_readonly("xy", &agv::EasyFullControl::Destination::xy) .def_property_readonly("yaw", &agv::EasyFullControl::Destination::yaw) .def_property_readonly("graph_index", &agv::EasyFullControl::Destination::graph_index) + .def_property_readonly("name", &agv::EasyFullControl::Destination::name) .def_property_readonly("dock", &agv::EasyFullControl::Destination::dock) - .def_property_readonly("speed_limit", &agv::EasyFullControl::Destination::speed_limit); + .def_property_readonly("speed_limit", &agv::EasyFullControl::Destination::speed_limit) + .def_property_readonly("inside_lift", &agv::EasyFullControl::Destination::inside_lift); py::class_(m_easy_full_control, "FleetConfiguration") .def(py::init([]( // Lambda function to convert reference to shared ptr @@ -1031,7 +1050,13 @@ PYBIND11_MODULE(rmf_adapter, m) { .def_property( "default_min_lane_length", &agv::EasyFullControl::FleetConfiguration::default_min_lane_length, - &agv::EasyFullControl::FleetConfiguration::set_default_min_lane_length); + &agv::EasyFullControl::FleetConfiguration::set_default_min_lane_length) + .def_property_readonly( + "lift_emergency_lanes", + &agv::EasyFullControl::FleetConfiguration::lift_emergency_levels) + .def( + "set_lift_emergency_level", + &agv::EasyFullControl::FleetConfiguration::set_lift_emergency_level); // Transformation ============================================================= py::class_(m, "Transformation") diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index a540c2408..d65f880de 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -25,6 +25,17 @@ using OrientationConstraint = Graph::OrientationConstraint; void bind_graph(py::module& m) { auto m_graph = m.def_submodule("graph"); + py::class_>(m_graph, "LiftProperties") + .def_property_readonly("name", + &Graph::LiftProperties::name) + .def_property_readonly("location", + &Graph::LiftProperties::location) + .def_property_readonly("orientation", + &Graph::LiftProperties::orientation) + .def_property_readonly("dimensions", + &Graph::LiftProperties::dimensions) + .def("is_in_lift", &Graph::LiftProperties::is_in_lift); // WAYPOINT ================================================================== py::class_(m_graph, "Waypoint") @@ -103,7 +114,7 @@ void bind_graph(py::module& m) bind_lane(m_graph); // GRAPH ===================================================================== - py::class_(m_graph, "Graph") + py::class_>(m_graph, "Graph") .def(py::init<>()) // Waypoints