From 0ac38e6311c8f630b629aab13ed96bc55e8b69a8 Mon Sep 17 00:00:00 2001 From: Xiyu Date: Fri, 18 Aug 2023 13:17:25 +0800 Subject: [PATCH] EasyFullControl (#235) Signed-off-by: Xi Yu Oh Signed-off-by: Yadunund Signed-off-by: Luca Della Vedova Signed-off-by: Michael X. Grey Signed-off-by: Xi Yu Oh Co-authored-by: Yadunund Co-authored-by: Luca Della Vedova Co-authored-by: Michael X. Grey --- .../include/rmf_fleet_adapter/agv/Adapter.hpp | 10 + .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 754 +++++ .../agv/FleetUpdateHandle.hpp | 14 +- .../agv/RobotUpdateHandle.hpp | 101 +- .../rmf_fleet_adapter/agv/Transformation.hpp | 73 + rmf_fleet_adapter/rmf_rxcpp/CMakeLists.txt | 78 +- .../Rx/v2/src/rxcpp/rx-scheduler.hpp | 4 +- .../src/rmf_fleet_adapter/ScheduleManager.cpp | 5 +- .../src/rmf_fleet_adapter/ScheduleManager.hpp | 1 - .../src/rmf_fleet_adapter/TaskManager.cpp | 35 +- .../src/rmf_fleet_adapter/TaskManager.hpp | 2 +- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 174 ++ .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 2724 +++++++++++++++++ .../agv/FleetUpdateHandle.cpp | 65 +- .../rmf_fleet_adapter/agv/RobotContext.cpp | 149 +- .../rmf_fleet_adapter/agv/RobotContext.hpp | 135 +- .../agv/RobotUpdateHandle.cpp | 358 ++- .../rmf_fleet_adapter/agv/Transformation.cpp | 108 + .../agv/internal_EasyFullControl.hpp | 71 + .../agv/internal_EasyTrafficLight.hpp | 2 +- .../agv/internal_FleetUpdateHandle.hpp | 20 +- .../agv/internal_RobotUpdateHandle.hpp | 226 +- .../rmf_fleet_adapter/events/GoToPlace.cpp | 80 + .../rmf_fleet_adapter/events/GoToPlace.hpp | 1 + .../events/PerformAction.cpp | 2 +- .../events/ResponsiveWait.cpp | 9 + .../events/WaitForTraffic.cpp | 22 +- .../rmf_fleet_adapter/phases/MoveRobot.hpp | 67 +- rmf_fleet_adapter_python/src/adapter.cpp | 367 ++- .../rmf_traffic_ros2/schedule/Negotiation.cpp | 15 +- 30 files changed, 5444 insertions(+), 228 deletions(-) create mode 100644 rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp create mode 100644 rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Transformation.hpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp index 7360e530d..c4d90ac68 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -77,6 +78,15 @@ class Adapter : public std::enable_shared_from_this const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions(), std::optional discovery_timeout = std::nullopt); + /// Add an Easy Full Control fleet to be adapted. + /// + /// \param[in] config + /// Configuration for the new Easy Full Control fleet. + /// + /// \return The handle for adding new robots to the fleet. + std::shared_ptr add_easy_fleet( + const EasyFullControl::FleetConfiguration& config); + /// Add a fleet to be adapted. /// /// If a single real-life fleet needs to integrate robots with varying traits diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp new file mode 100644 index 000000000..b5f9531c9 --- /dev/null +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -0,0 +1,754 @@ +/* + * 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__AGV__EASYFULLCONTROL_HPP +#define RMF_FLEET_ADAPTER__AGV__EASYFULLCONTROL_HPP + +// ROS 2 headers +#include + +// rmf_fleet_adapter headers +#include +#include +#include + +// rmf_battery headers +#include +#include +#include +#include + +// rmf_traffic headers +#include +#include + +#include + +// System headers +#include + +namespace rmf_fleet_adapter { +namespace agv { + +/// An easy to initialize full_control fleet adapter. +/// By default the adapter will be configured to accept all tasks. +/// To disable specific tasks, call the respective consider_*_requests() method +/// on the FleetUpdateHandle that can be accessed within this adapter. +//============================================================================== +class EasyFullControl : public std::enable_shared_from_this +{ +public: + + // Aliases + using Graph = rmf_traffic::agv::Graph; + using VehicleTraits = rmf_traffic::agv::VehicleTraits; + using ActionExecutor = RobotUpdateHandle::ActionExecutor; + using ActivityIdentifier = RobotUpdateHandle::ActivityIdentifier; + using ActivityIdentifierPtr = RobotUpdateHandle::ActivityIdentifierPtr; + using ConstActivityIdentifierPtr = + RobotUpdateHandle::ConstActivityIdentifierPtr; + using Stubbornness = RobotUpdateHandle::Unstable::Stubbornness; + using ConsiderRequest = FleetUpdateHandle::ConsiderRequest; + + // Nested class declarations + class EasyRobotUpdateHandle; + class RobotState; + class RobotConfiguration; + class RobotCallbacks; + class Destination; + class FleetConfiguration; + class CommandExecution; + + /// Signature for a function that handles navigation requests. The request + /// will specify an (x, y) location and yaw on a map. + /// + /// \param[in] execution + /// The command execution progress updater. Use this to keep the fleet + /// adapter updated on the progress of the command. + using NavigationRequest = + std::function; + + /// Signature for a function to handle stop requests. + using StopRequest = std::function; + + /// Add a robot to the fleet once it is available. + /// + /// \param[in] name + /// Name of the robot. This must be unique per fleet. + /// + /// \param[in] initial_state + /// The initial state of the robot when it is added to the fleet. + /// + /// \param[in] configuration + /// The configuration of the robot. + /// + /// \param[in] callbacks + /// The callbacks that will be used to issue commands for the robot. + /// + /// \return an easy robot update handle on success. A nullptr if an error + /// occurred. + std::shared_ptr add_robot( + std::string name, + RobotState initial_state, + RobotConfiguration configuration, + RobotCallbacks callbacks); + + /// Get the FleetUpdateHandle that this adapter will be using. + /// This may be used to perform more specialized customizations using the + /// base FleetUpdateHandle API. + std::shared_ptr more(); + + /// Immutable reference to the base FleetUpdateHandle API. + std::shared_ptr more() const; + + class Implementation; +private: + EasyFullControl(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +using EasyFullControlPtr = std::shared_ptr; + +/// Handle used to update information about one robot +class EasyFullControl::EasyRobotUpdateHandle +{ +public: + /// Recommended function for updating information about a robot in an + /// EasyFullControl fleet. + /// + /// \param[in] state + /// The current state of the robot + /// + /// \param[in] current_action + /// The action that the robot is currently executing + void update( + RobotState state, + ConstActivityIdentifierPtr current_activity); + + /// Get the maximum allowed merge waypoint distance for this robot. + double max_merge_waypoint_distance() const; + + /// Modify the maximum allowed merge distance between the robot and a waypoint. + /// + /// \param[in] max_merge_waypoint_distance + /// The maximum merge waypoint distance for this robot. + void set_max_merge_waypoint_distance(double distance); + + /// Get the maximum allowed merge lane distance for this robot. + double max_merge_lane_distance() const; + + /// Modify the maximum allowed merge distance between the robot and a lane. + /// + /// \param[in] max_merge_lane_distance + /// The maximum merge lane distance for this robot. + void set_max_merge_lane_distance(double distance); + + /// Get the minimum lane length for this robot. + double min_lane_length() const; + + /// Modify the minimum lane length for this robot. + /// + /// \param[in] min_lane_length + /// The minimum length of a lane. + void set_min_lane_length(double length); + + /// Get more options for updating the robot's state + std::shared_ptr more(); + + /// Immutable reference to the base robot update API + std::shared_ptr more() const; + + class Implementation; +private: + EasyRobotUpdateHandle(); + rmf_utils::unique_impl_ptr _pimpl; +}; + +/// The current state of a robot, passed into EasyRobotUpdateHandle::update +class EasyFullControl::RobotState +{ +public: + /// Constructor + /// + /// \param[in] map_name + /// The name of the map the robot is currently on + /// + /// \param[in] position + /// The current position of the robot + /// + /// \param[in] battery_soc + /// the current battery level of the robot, specified by its state of + /// charge as a fraction of its total charge capacity, i.e. a value from 0.0 + /// to 1.0. + RobotState( + std::string map_name, + Eigen::Vector3d position, + double battery_soc); + + /// Current map the robot is on + const std::string& map() const; + + /// Set the current map the robot is on + void set_map(std::string value); + + /// Current position of the robot + Eigen::Vector3d position() const; + + /// Set the current position of the robot + void set_position(Eigen::Vector3d value); + + /// Current state of charge of the battery, as a fraction from 0.0 to 1.0. + double battery_state_of_charge() const; + + /// Set the state of charge of the battery, as a fraction from 0.0 to 1.0. + void set_battery_state_of_charge(double value); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +/// The configuration of a robot. These are parameters that typically do not +/// change over time. +class EasyFullControl::RobotConfiguration +{ +public: + + /// Constructor + /// + /// \param[in] compatible_chargers + /// List of chargers that this robot is compatible with + /// + /// \param[in] responsive_wait + /// Should this robot use the responsive wait behavior? true / false / fleet default. + /// + /// \warning This must contain a single string value until a later release of + /// RMF. We are using a vector for forward API compatibility. For now, make + /// sure each robot has only one unique compatible charger to avoid charging + /// conflicts. + RobotConfiguration( + std::vector compatible_chargers, + std::optional responsive_wait = std::nullopt, + std::optional max_merge_waypoint_distance = 1e-3, + std::optional max_merge_lane_distance = 0.3, + std::optional min_lane_length = 1e-8); + + /// List of chargers that this robot is compatible with + const std::vector& compatible_chargers() const; + + /// Set the list of chargers compatible with this robot. + void set_compatible_chargers(std::vector chargers); + + /// Should this robot use the responsive wait behavior? Responsive wait means + /// that when the robot is idle on a point, it will report to the traffic + /// schedule that it is waiting on that point, and it will negotiate with + /// other robots to let them pass while ultimately remaining on the point. + /// + /// If std::nullopt is used, then the fleet-wide responsive wait behavior will + /// be used. + std::optional responsive_wait() const; + + /// Toggle responsive wait on (true), off (false), or use fleet default + /// (std::nullopt). + void set_responsive_wait(std::optional enable); + + /// Get the maximum merge distance between a robot and a waypoint. This refers + /// to the maximum distance allowed to consider a robot to be on a particular + /// waypoint. + /// + /// If std::nullopt is used, then the fleet-wide default merge waypoint + /// distance will be used. + std::optional max_merge_waypoint_distance() const; + + /// Set the maximum merge distance between a robot and a waypoint. + void set_max_merge_waypoint_distance(std::optional distance); + + /// Get the maximum merge distance between a robot and a lane. This refers + /// to the maximum distance allowed to consider a robot to be on a particular + /// lane. + /// + /// If std::nullopt is used, then the fleet-wide default merge lane + /// distance will be used. + std::optional max_merge_lane_distance() const; + + /// Set the maximum merge distance between a robot and a lane. + void set_max_merge_lane_distance(std::optional distance); + + /// Get the minimum lane length. + /// + /// If std::nullopt is used, then the fleet-wide default minimum lane length + /// will be used. + std::optional min_lane_length() const; + + /// Set the minimum lane length. + void set_min_lane_length(std::optional distance); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +class EasyFullControl::RobotCallbacks +{ +public: + + /// Constructor + /// + /// \param[in] navigate + /// A function that can be used to request the robot to navigate to a location. + /// The function returns a handle which can be used to track the progress of the navigation. + /// + /// \param[in] stop + /// A function to stop the robot. + /// + /// \param[in] action_executor + /// The ActionExecutor callback to request the robot to perform an action. + RobotCallbacks( + NavigationRequest navigate, + StopRequest stop, + ActionExecutor action_executor); + + /// Get the callback for navigation + NavigationRequest navigate() const; + + /// Get the callback for stopping + StopRequest stop() const; + + /// Get the action executor. + ActionExecutor action_executor() const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +/// Used by system integrators to give feedback on the progress of executing a +/// navigation or docking command. +class EasyFullControl::CommandExecution +{ +public: + + /// Trigger this when the command is successfully finished. No other function + /// in this CommandExecution instance will be usable after this. + void finished(); + + /// Returns false if the command has been stopped. + bool okay() const; + + /// Use this to override the traffic schedule for the agent while it performs + /// this command. + /// + /// If the given trajectory results in a traffic conflict then a negotiation + /// will be triggered. Hold onto the `Stubbornness` returned by this function + /// to ask other agents to plan around your trajectory, otherwise the + /// negotiation may result in a replan for this agent and a new command will + /// be issued. + /// + /// \note Using this will function always trigger a replan once the agent + /// finishes the command. + /// + /// \warning Too many overridden/stubborn agents can cause a deadlock. It's + /// recommended to use this API sparingly and only over short distances or + /// small deviations. + /// + /// \param[in] map + /// Name of the map where the trajectory will take place + /// + /// \param[in] path + /// The path of the agent + /// + /// \param[in] hold + /// How long the agent will wait at the end of the path + /// + /// \return a Stubbornness handle that tells the fleet adapter to not let the + /// overridden path be negotiated. The returned handle will stop having an + /// effect after this command execution is finished. + Stubbornness override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold = rmf_traffic::Duration(0)); + + /// Activity handle for this command. Pass this into + /// EasyRobotUpdateHandle::update_position while executing this command. + ConstActivityIdentifierPtr identifier() const; + + class Implementation; +private: + CommandExecution(); + rmf_utils::impl_ptr _pimpl; +}; + +class EasyFullControl::Destination +{ +public: + /// The name of the map where the destination is located. + const std::string& map() const; + + /// The (x, y, yaw) position of the destination. + Eigen::Vector3d position() const; + + /// The (x, y) position of the destination. + Eigen::Vector2d xy() const; + + /// The intended orientation of the robot at the destination, represented in + /// radians. + double yaw() const; + + /// If the destination has an index in the navigation graph, you can get it + /// from this field. + std::optional graph_index() const; + + /// If there is a speed limit that should be respected while approaching the + /// destination, this will indicate it. + std::optional speed_limit() const; + + /// If the destination should be reached by performing a dock maneuver, this + /// will contain the name of the dock. + std::optional dock() const; + + class Implementation; +private: + Destination(); + rmf_utils::impl_ptr _pimpl; +}; + +/// The Configuration class contains parameters necessary to initialize an +/// EasyFullControl fleet instance and add fleets to the adapter. +class EasyFullControl::FleetConfiguration +{ +public: + + /// Constructor + /// + /// \param[in] fleet_name + /// The name of the fleet that is being added. + /// + /// \param[in] transformations_to_robot_coordinates + /// A dictionary of transformations from RMF canonical coordinates to the + /// the coordinate system used by the robot. Each map should be assigned its + /// own transformation. If this is not nullptr, then a warning will be + /// logged whenever the dictionary is missing a transform for a map, and the + /// canonical RMF coordinates will be used. + /// + /// \param[in] traits + /// Specify the approximate traits of the vehicles in this fleet. + /// + /// \param[in] navigation_graph + /// Specify the navigation graph used by the vehicles in this fleet. + /// + /// \param[in] battery_system + /// Specify the battery system used by the vehicles in this fleet. + /// + /// \param[in] motion_sink + /// Specify the motion sink that describes the vehicles in this fleet. + /// + /// \param[in] ambient_sink + /// Specify the device sink for ambient sensors used by the vehicles in this fleet. + /// + /// \param[in] tool_sink + /// Specify the device sink for special tools used by the vehicles in this fleet. + /// + /// \param[in] recharge_threshold + /// The threshold for state of charge below which robots in this fleet + /// will cease to operate and require recharging. A value between 0.0 and + /// 1.0 should be specified. + /// + /// \param[in] recharge_soc + /// The state of charge to which robots in this fleet should be charged up + /// to by automatic recharging tasks. A value between 0.0 and 1.0 should be + /// specified. + /// + /// \param[in] account_for_battery_drain + /// Specify whether battery drain is to be considered while allocating tasks. + /// If false, battery drain will not be considered when planning for tasks. + /// As a consequence, charging tasks will not be automatically assigned to + /// vehicles in this fleet when battery levels fall below the + /// recharge_threshold. + /// + /// \param[in] task_categories + /// Provide callbacks for considering tasks belonging to each category. + /// + /// \param[in] action_categories + /// List of actions that this fleet can perform. Each item represents a + /// category in the PerformAction description. + /// + /// \param[in] finishing_request + /// A factory for a request that should be performed by each robot in this + /// fleet at the end of its assignments. + /// + /// \param[in] skip_rotation_commands + /// If true, navigation requests which would only have the robot rotate in + /// place will not be sent. Instead, navigation requests will always have + /// the final orientation for the destination. + /// + /// \param[in] server_uri + /// The URI for the websocket server that receives updates on tasks and + /// states. If nullopt, data will not be published. + /// + /// \param[in] max_delay + /// Specify the default value for how high the delay of the current itinerary + /// can become before it gets interrupted and replanned. + /// + /// \param[in] update_interval + /// The duration between positional state updates that are sent to + /// the fleet adapter. + /// + /// \param[in] default_responsive_wait + /// Should the robots in this fleet have responsive wait enabled (true) or + /// disabled (false) by default? + /// + /// \param[in] default_max_merge_waypoint_distance + /// The maximum merge distance between a robot position and a waypoint. + /// + /// \param[in] default_max_merge_lane_distance + /// The maximum merge distance between a robot position and a lane. + /// + /// \param[in] default_min_lane_length + /// The minimum length that a lane should have. + FleetConfiguration( + const std::string& fleet_name, + std::optional> + transformations_to_robot_coordinates, + std::unordered_map + known_robot_configurations, + std::shared_ptr traits, + std::shared_ptr graph, + rmf_battery::agv::ConstBatterySystemPtr battery_system, + rmf_battery::ConstMotionPowerSinkPtr motion_sink, + rmf_battery::ConstDevicePowerSinkPtr ambient_sink, + rmf_battery::ConstDevicePowerSinkPtr tool_sink, + double recharge_threshold, + double recharge_soc, + bool account_for_battery_drain, + std::unordered_map task_consideration, + std::unordered_map action_consideration, + rmf_task::ConstRequestFactoryPtr finishing_request = nullptr, + bool skip_rotation_commands = true, + std::optional server_uri = std::nullopt, + rmf_traffic::Duration max_delay = rmf_traffic::time::from_seconds(10.0), + rmf_traffic::Duration update_interval = rmf_traffic::time::from_seconds( + 0.5), + bool default_responsive_wait = false, + double default_max_merge_waypoint_distance = 1e-3, + double default_max_merge_lane_distance = 0.3, + double min_lane_length = 1e-8 + ); + + /// Create a FleetConfiguration object using a set of configuration parameters + /// imported from YAML files that follow the defined schema. This is an + /// alternative to constructing the FleetConfiguration using the RMF objects if + /// users do not require specific tool systems for their fleets. The + /// FleetConfiguration object will be instantiated with instances of + /// SimpleMotionPowerSink and SimpleDevicePowerSink. + /// + /// \param[in] config_file + /// The path to a configuration YAML file containing data about the fleet's + /// vehicle traits and task capabilities. This file needs to follow the pre-defined + /// config.yaml structure to successfully load the parameters into the FleetConfiguration + /// object. + /// + /// \param[in] nav_graph_path + /// The path to a navigation path file that includes map information necessary + /// to create a rmf_traffic::agv::Graph object + /// + /// \param[in] server_uri + /// The URI for the websocket server that receives updates on tasks and + /// states. If nullopt, data will not be published. + /// + /// \return A FleetConfiguration object with the essential config parameters loaded. + static std::optional from_config_files( + const std::string& config_file, + const std::string& nav_graph_path, + std::optional server_uri = std::nullopt); + + /// Get the fleet name. + const std::string& fleet_name() const; + + /// Set the fleet name. + void set_fleet_name(std::string value); + + /// Get the transformations into robot coordinates for this fleet. + const std::optional>& + transformations_to_robot_coordinates() const; + + /// Set the transformation into robot coordinates for a map. This will replace + /// any transformation previously set for the map. If the transformation + /// dictionary was previously nullopt, this will initialize it with an empty + /// value before inserting this transformation. + void add_robot_coordinate_transformation( + std::string map, + Transformation transformation); + + /// Get a dictionary of known robot configurations. The key is the name of the + /// robot belonging to this fleet. These configurations are usually parsed + /// from a fleet configuration file. + const std::unordered_map& + known_robot_configurations() const; + + /// Get the names of all robots with known robot configurations. + std::vector known_robots() const; + + /// Provide a known configuration for a named robot. + /// + /// \param[in] robot_name + /// The unique name of the robot. + /// + /// \param[in] configuration + /// The configuration for the robot. + void add_known_robot_configuration( + std::string robot_name, + RobotConfiguration configuration); + + /// Get a known configuration for a robot based on its name. + std::optional get_known_robot_configuration( + const std::string& robot_name) const; + + /// Get the fleet vehicle traits. + const std::shared_ptr& vehicle_traits() const; + + /// Set the vehicle traits. + void set_vehicle_traits(std::shared_ptr value); + + /// Get the fleet navigation graph. + const std::shared_ptr& graph() const; + + /// Set the fleet navigation graph. + void set_graph(std::shared_ptr value); + + /// Get the battery system. + rmf_battery::agv::ConstBatterySystemPtr battery_system() const; + + /// Set the battery system. + void set_battery_system(rmf_battery::agv::ConstBatterySystemPtr value); + + /// Get the motion sink. + rmf_battery::ConstMotionPowerSinkPtr motion_sink() const; + + /// Set the motion sink. + void set_motion_sink(rmf_battery::ConstMotionPowerSinkPtr value); + + /// Get the ambient sink. + rmf_battery::ConstDevicePowerSinkPtr ambient_sink() const; + + /// Set the ambient sink. + void set_ambient_sink(rmf_battery::ConstDevicePowerSinkPtr value); + + /// Get the tool sink. + rmf_battery::ConstDevicePowerSinkPtr tool_sink() const; + + /// Set the tool sink. + void set_tool_sink(rmf_battery::ConstDevicePowerSinkPtr value); + + /// Get the recharge threshold. + double recharge_threshold() const; + + /// Set the recharge threshold. + void set_recharge_threshold(double value); + + /// Get the recharge state of charge. If the robot's state of charge dips + /// below this value then its next task will be to recharge. + double recharge_soc() const; + + /// Set the recharge state of charge. + void set_recharge_soc(double value); + + /// Get whether or not to account for battery drain during task planning. + bool account_for_battery_drain() const; + + /// Set whether or not to account for battery drain during task planning. + void set_account_for_battery_drain(bool value); + + /// Get the task categories + const std::unordered_map& + task_consideration() const; + + /// Mutable access to the task consideration map. + std::unordered_map& task_consideration(); + + /// Get the action categories + const std::unordered_map& + action_consideration() const; + + /// Mutable access to the action consideration map. + std::unordered_map& action_consideration(); + + /// Get the finishing request. + rmf_task::ConstRequestFactoryPtr finishing_request() const; + + /// Set the finishing request. + void set_finishing_request(rmf_task::ConstRequestFactoryPtr value); + + /// Check whether rotation commands will be skipped. + bool skip_rotation_commands() const; + + /// Set whether rotation commands will be skipped. + void set_skip_rotation_commands(bool value); + + /// Get the server uri. + std::optional server_uri() const; + + /// Set the server uri. + void set_server_uri(std::optional value); + + /// Get the max delay. + rmf_traffic::Duration max_delay() const; + + /// Set the max delay. + void set_max_delay(rmf_traffic::Duration value); + + /// Get the update interval. + rmf_traffic::Duration update_interval() const; + + /// Set the update interval. + void set_update_interval(rmf_traffic::Duration value); + + /// Should robots in this fleet have responsive wait enabled by default? + bool default_responsive_wait() const; + + /// Set whether robots in this fleet should have responsive wait enabled by + /// default. + void set_default_responsive_wait(bool enable); + + /// Get the maximum merge distance between a robot position and a waypoint. + double default_max_merge_waypoint_distance() const; + + /// Set the maximum merge distance between a robot position and a waypoint. + void set_default_max_merge_waypoint_distance(double distance); + + /// Get the maximum merge distance between a robot position and a lane. + double default_max_merge_lane_distance() const; + + /// Set the maximum merge distance between a robot position and a lane. + void set_default_max_merge_lane_distance(double distance); + + /// Get the minimum lane length allowed. + double default_min_lane_length() const; + + /// Set the minimum lane length. + void set_default_min_lane_length(double distance); + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__EASYFULLCONTROL_HPP 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 b3b0b6942..4bd5ad23d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -44,6 +44,8 @@ namespace agv { class FleetUpdateHandle : public std::enable_shared_from_this { public: + /// Get the name of the fleet that his handle is managing. + const std::string& fleet_name() const; /// Add a robot to this fleet adapter. /// @@ -279,14 +281,14 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// /// \return true if task planner parameters were successfully updated. bool set_task_planner_params( - std::shared_ptr battery_system, - std::shared_ptr motion_sink, - std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, + rmf_battery::agv::ConstBatterySystemPtr battery_system, + rmf_battery::ConstMotionPowerSinkPtr motion_sink, + rmf_battery::ConstDevicePowerSinkPtr ambient_sink, + rmf_battery::ConstDevicePowerSinkPtr tool_sink, double recharge_threshold, double recharge_soc, bool account_for_battery_drain, - rmf_task::ConstRequestFactoryPtr finishing_requst = nullptr); + rmf_task::ConstRequestFactoryPtr finishing_request = nullptr); /// A callback function that evaluates whether a fleet will accept a task /// request @@ -389,6 +391,8 @@ class FleetUpdateHandle : public std::enable_shared_from_this using FleetUpdateHandlePtr = std::shared_ptr; using ConstFleetUpdateHandlePtr = std::shared_ptr; +FleetUpdateHandle::ConsiderRequest consider_all(); + } // namespace agv } // namespace rmf_fleet_adapter 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 afd33bd20..d0aa2c22e 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -103,7 +103,8 @@ class RobotUpdateHandle RobotUpdateHandle& set_charger_waypoint(const std::size_t charger_wp); /// Update the current battery level of the robot by specifying its state of - /// charge as a fraction of its total charge capacity + /// charge as a fraction of its total charge capacity, i.e. a value from 0.0 + /// to 1.0. void update_battery_soc(const double battery_soc); /// Use this function to override the robot status. The string provided must @@ -125,12 +126,49 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; + /// Unique identifier for an activity that the robot is performing. Used by + /// the EasyFullControl API. + class ActivityIdentifier + { + public: + + /// Compare whether two activity handles are referring to the same activity. + bool operator==(const ActivityIdentifier&) const; + + class Implementation; + private: + ActivityIdentifier(); + rmf_utils::unique_impl_ptr _pimpl; + }; + using ActivityIdentifierPtr = std::shared_ptr; + using ConstActivityIdentifierPtr = std::shared_ptr; + + /// Hold onto this class to tell the robot to behave as a "stubborn + /// negotiator", meaning it will always refuse to accommodate the schedule + /// of any other agent. This could be used when teleoperating a robot, to + /// tell other robots that the agent is unable to negotiate. + /// + /// When the object is destroyed, the stubbornness will automatically be + /// released. + class Stubbornness + { + public: + /// Stop being stubborn + void release(); + + class Implementation; + private: + Stubbornness(); + rmf_utils::impl_ptr _pimpl; + }; + /// The ActionExecution class should be used to manage the execution of and /// provide updates on ongoing actions. class ActionExecution { public: - /// Update the amount of time remaining for this action + /// Update the amount of time remaining for this action. + /// This does not need to be used for navigation requests. void update_remaining_time(rmf_traffic::Duration remaining_time_estimate); /// Set task status to underway and optionally log a message (info tier) @@ -147,12 +185,50 @@ class RobotUpdateHandle /// (warning tier) void blocked(std::optional text); - /// Trigger this when the action is successfully finished + /// Use this to override the traffic schedule for the agent while it performs + /// this command. + /// + /// If the given trajectory results in a traffic conflict then a negotiation + /// will be triggered. Hold onto the `Stubbornness` returned by this function + /// to ask other agents to plan around your trajectory, otherwise the + /// negotiation may result in a replan for this agent and a new command will + /// be issued. + /// + /// \note Using this will function always trigger a replan once the agent + /// finishes the command. + /// + /// \warning Too many overridden/stubborn agents can cause a deadlock. It's + /// recommended to use this API sparingly and only over short distances or + /// small deviations. + /// + /// \param[in] map + /// Name of the map where the trajectory will take place + /// + /// \param[in] path + /// The path of the agent + /// + /// \param[in] hold + /// How long the agent will wait at the end of the path + /// + /// \return a Stubbornness handle that tells the fleet adapter to not let the + /// overridden path be negotiated. The returned handle will stop having an + /// effect after this command execution is finished. + Stubbornness override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold = rmf_traffic::Duration(0)); + + /// Trigger this when the action is successfully finished. + /// No other functions in this ActionExecution instance will + /// be usable after this. void finished(); /// Returns false if the Action has been killed or cancelled bool okay() const; + /// Activity identifier for this action. Used by the EasyFullControl API. + ConstActivityIdentifierPtr identifier() const; + class Implementation; private: ActionExecution(); @@ -368,24 +444,7 @@ class RobotUpdateHandle /// Get the current Plan ID that this robot has sent to the traffic schedule rmf_traffic::PlanId current_plan_id() const; - /// Hold onto this class to tell the robot to behave as a "stubborn - /// negotiator", meaning it will always refuse to accommodate the schedule - /// of any other agent. This could be used when teleoperating a robot, to - /// tell other robots that the agent is unable to negotiate. - /// - /// When the object is destroyed, the stubbornness will automatically be - /// released. - class Stubbornness - { - public: - /// Stop being stubborn - void release(); - - class Implementation; - private: - Stubbornness(); - rmf_utils::impl_ptr _pimpl; - }; + using Stubbornness = RobotUpdateHandle::Stubbornness; /// Tell this robot to be a stubborn negotiator. Stubbornness be_stubborn(); diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Transformation.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Transformation.hpp new file mode 100644 index 000000000..9b8c53895 --- /dev/null +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Transformation.hpp @@ -0,0 +1,73 @@ +/* + * 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__AGV__TRANSFORMATION_HPP +#define RMF_FLEET_ADAPTER__AGV__TRANSFORMATION_HPP + +#include + +#include + +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +/// A Transformation object that stores the transformation data needed to +/// perform transformation between robot and RMF cartesian frames. +class Transformation +{ +public: + + /// Constructor + /// + /// \param[in] rotation + /// The rotation angle (radians) between the two cartesian frames. + /// + /// \param[in] scale + /// The scaling factor between the cartesian frames. + /// + /// \param[in] translation + /// The 2D translation from one coordinate to another. + Transformation( + double rotation, + double scale, + Eigen::Vector2d translation); + + /// Get the rotation of this Transformation + double rotation() const; + + /// Get the scale of this Transformation + double scale() const; + + /// Get the translation of this Transformation + const Eigen::Vector2d& translation() const; + + /// Apply this transformation to an (x, y, yaw) position + Eigen::Vector3d apply(const Eigen::Vector3d& position) const; + + /// Apply the inverse of this transformation to an (x, y, yaw) position + Eigen::Vector3d apply_inverse(const Eigen::Vector3d& position) const; + + class Implementation; +private: + rmf_utils::impl_ptr _pimpl; +}; + +} // namespace agv +} // namespace rmf_fleet_adapter + +#endif // RMF_FLEET_ADAPTER__AGV__TRANSFORMATION_HPP diff --git a/rmf_fleet_adapter/rmf_rxcpp/CMakeLists.txt b/rmf_fleet_adapter/rmf_rxcpp/CMakeLists.txt index 0fdcb9184..aa56f9720 100644 --- a/rmf_fleet_adapter/rmf_rxcpp/CMakeLists.txt +++ b/rmf_fleet_adapter/rmf_rxcpp/CMakeLists.txt @@ -24,40 +24,46 @@ target_link_libraries(rmf_rxcpp ${rclcpp_LIBRARIES} ) -if(BUILD_TESTING) - find_package(ament_cmake_catch2 REQUIRED) - find_package(rmf_utils REQUIRED) - find_package(std_msgs REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_file(uncrustify_config_file NAMES - NAMES "rmf_code_style.cfg" - PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - - ament_add_catch2( - test_rmf_rxcpp - test/main.cpp - test/test_RxJobs.cpp - test/test_Transport.cpp - ) - target_include_directories(test_rmf_rxcpp - PRIVATE - ${std_msgs_INCLUDE_DIRS} - ) - target_link_libraries(test_rmf_rxcpp - PRIVATE - rmf_rxcpp - rmf_utils::rmf_utils - ${std_msgs_LIBRARIES} - ) - - find_package(rmf_traffic REQUIRED) - - add_executable(rmf_rxcpp_example_plan_path examples/PlanPath.cpp) - - target_link_libraries(rmf_rxcpp_example_plan_path - PUBLIC - rmf_traffic::rmf_traffic - rmf_rxcpp - ) +# Issues in RMW implementations are causing spurious failures for these tests, +# possibly due to race conditions in the memory management of the underlying +# middleware implementations while the test spins up and tears down rclcpp nodes +# too quickly. This is giving us spurious test failures in the CI, so we will +# simply disable these tests since this code is heading towards retirement +# anyway. +# if(BUILD_TESTING) +# find_package(ament_cmake_catch2 REQUIRED) +# find_package(rmf_utils REQUIRED) +# find_package(std_msgs REQUIRED) +# find_package(ament_cmake_uncrustify REQUIRED) +# find_file(uncrustify_config_file NAMES +# NAMES "rmf_code_style.cfg" +# PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") -endif() +# ament_add_catch2( +# test_rmf_rxcpp +# test/main.cpp +# test/test_RxJobs.cpp +# test/test_Transport.cpp +# ) +# target_include_directories(test_rmf_rxcpp +# PRIVATE +# ${std_msgs_INCLUDE_DIRS} +# ) +# target_link_libraries(test_rmf_rxcpp +# PRIVATE +# rmf_rxcpp +# rmf_utils::rmf_utils +# ${std_msgs_LIBRARIES} +# ) + +# find_package(rmf_traffic REQUIRED) + +# add_executable(rmf_rxcpp_example_plan_path examples/PlanPath.cpp) + +# target_link_libraries(rmf_rxcpp_example_plan_path +# PUBLIC +# rmf_traffic::rmf_traffic +# rmf_rxcpp +# ) + +# endif() diff --git a/rmf_fleet_adapter/rmf_rxcpp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-scheduler.hpp b/rmf_fleet_adapter/rmf_rxcpp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-scheduler.hpp index b6fd83136..428eddbb0 100644 --- a/rmf_fleet_adapter/rmf_rxcpp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-scheduler.hpp +++ b/rmf_fleet_adapter/rmf_rxcpp/RxCpp-4.1.0/Rx/v2/src/rxcpp/rx-scheduler.hpp @@ -917,8 +917,8 @@ class schedulable_queue { int64_t ordinal; public: - schedulable_queue() - : ordinal(0) + schedulable_queue() + : ordinal(0) { } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.cpp index 224ac6a54..6b5f37e2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.cpp @@ -24,11 +24,10 @@ namespace rmf_fleet_adapter { //============================================================================== ScheduleManager::ScheduleManager( - rclcpp::Node& node, + rclcpp::Node&, rmf_traffic::schedule::Participant participant, rmf_traffic_ros2::schedule::Negotiation* negotiation) -: _node(&node), - _participant(std::move(participant)), +: _participant(std::move(participant)), _negotiator(nullptr) { if (negotiation) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.hpp index 8a369115d..006a9df17 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/ScheduleManager.hpp @@ -75,7 +75,6 @@ class ScheduleManager const ResponderPtr&)> callback; }; - rclcpp::Node* _node; rmf_traffic::schedule::Participant _participant; Negotiator* _negotiator; std::shared_ptr _negotiator_handle; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index a9e64d415..326f00704 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -849,10 +849,6 @@ std::string TaskManager::robot_status() const //============================================================================== auto TaskManager::expected_finish_state() const -> State { - rmf_task::State current_state = - _context->make_get_state()() - .time(rmf_traffic_ros2::convert(_context->node()->now())); - std::lock_guard lock(_mutex); if (!_direct_queue.empty()) { @@ -862,6 +858,9 @@ auto TaskManager::expected_finish_state() const -> State if (_active_task) return _context->current_task_end_state(); + rmf_task::State current_state = + _context->make_get_state()() + .time(rmf_traffic_ros2::convert(_context->node()->now())); return current_state; } @@ -1254,12 +1253,6 @@ void TaskManager::_begin_next_task() return; } - if (_waiting) - { - _waiting.cancel({"New task ready"}, _context->now()); - return; - } - // The next task should one in the direct assignment queue if present const bool is_next_task_direct = !_direct_queue.empty(); const auto assignment = is_next_task_direct ? @@ -1281,8 +1274,20 @@ void TaskManager::_begin_next_task() if (now >= deployment_time) { + if (_waiting) + { + _waiting.cancel({"New task ready"}, _context->now()); + return; + } + // Update state in RobotContext and Assign active task const auto& id = assignment.request()->booking()->id(); + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning next task [%s] for robot [%s]", + id.c_str(), + _context->requester_id().c_str()); + _context->current_task_end_state(assignment.finish_state()); _context->current_task_id(id); _active_task = ActiveTask::start( @@ -1434,6 +1439,16 @@ void TaskManager::_begin_waiting() if (!_responsive_wait_enabled) return; + if (_context->location().empty()) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Unable to perform responsive wait for [%s] because its position on its " + "navigation graph is unknown. This may require operator intervention.", + _context->requester_id().c_str()); + return; + } + // Determine the waypoint closest to the robot std::size_t waiting_point = _context->location().front().waypoint(); double min_dist = std::numeric_limits::max(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 2a8a117f2..1d8759bcc 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -308,7 +308,7 @@ class TaskManager : public std::enable_shared_from_this std::weak_ptr _fleet_handle; rmf_task::ConstActivatorPtr _task_activator; ActiveTask _active_task; - bool _responsive_wait_enabled = true; + bool _responsive_wait_enabled = false; bool _emergency_active = false; std::optional _emergency_pullover_interrupt_token; ActiveTask _emergency_pullover; 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 2eebd8259..de6eb68e1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -20,6 +20,7 @@ #include "Node.hpp" #include "internal_FleetUpdateHandle.hpp" +#include "internal_EasyFullControl.hpp" #include #include @@ -203,6 +204,179 @@ std::shared_ptr Adapter::make( return nullptr; } +namespace { +class DuplicateDockFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + DuplicateDockFinder() + { + // Do nothing + } + + void execute(const DoorOpen&) override {} + void execute(const DoorClose&) override {} + void execute(const LiftSessionBegin&) override {} + void execute(const LiftDoorOpen&) override {} + void execute(const LiftSessionEnd&) override {} + void execute(const LiftMove&) override {} + void execute(const Wait&) override {} + void execute(const Dock& dock) override + { + if (!visited_docks.insert(dock.dock_name()).second) + { + duplicate_docks.insert(dock.dock_name()); + } + } + + std::unordered_set visited_docks; + std::unordered_set duplicate_docks; +}; +} // anonymous namespace + +//============================================================================== +std::shared_ptr Adapter::add_easy_fleet( + const EasyFullControl::FleetConfiguration& config) +{ + if (!config.graph()) + { + RCLCPP_ERROR( + this->node()->get_logger(), + "Graph missing in the configuration for fleet [%s]. The fleet will not " + "be added.", + config.fleet_name().c_str()); + return nullptr; + } + + if (!config.vehicle_traits()) + { + RCLCPP_ERROR( + this->node()->get_logger(), + "Vehicle traits missing in the configuration for fleet [%s]. The fleet " + "will not be added.", + config.fleet_name().c_str()); + return nullptr; + } + + DuplicateDockFinder finder; + for (std::size_t i = 0; i < config.graph()->num_lanes(); ++i) + { + const auto* entry = config.graph()->get_lane(i).entry().event(); + if (entry) + entry->execute(finder); + + const auto* exit = config.graph()->get_lane(i).entry().event(); + if (exit) + exit->execute(finder); + } + + if (!finder.duplicate_docks.empty()) + { + RCLCPP_ERROR( + this->node()->get_logger(), + "Graph provided for fleet [%s] has %lu duplicate lanes:", + config.fleet_name().c_str(), + finder.duplicate_docks.size()); + + for (const auto& dock : finder.duplicate_docks) + { + RCLCPP_ERROR( + this->node()->get_logger(), + "- [%s]", + dock.c_str()); + } + + RCLCPP_ERROR( + this->node()->get_logger(), + "Each dock name on a graph must be unique, so we cannot add fleet [%s]", + config.fleet_name().c_str()); + return nullptr; + } + + auto fleet_handle = this->add_fleet( + config.fleet_name(), + *config.vehicle_traits(), + *config.graph(), + config.server_uri()); + + auto planner_params_ok = fleet_handle->set_task_planner_params( + config.battery_system(), + config.motion_sink(), + config.ambient_sink(), + config.tool_sink(), + config.recharge_threshold(), + config.recharge_soc(), + config.account_for_battery_drain(), + config.finishing_request()); + + if (!planner_params_ok) + { + RCLCPP_WARN( + this->node()->get_logger(), + "Failed to initialize task planner parameters for fleet [%s]. " + "It will not respond to bid requests for tasks", + config.fleet_name().c_str()); + } + + for (const auto& [task, consider] : config.task_consideration()) + { + if (task == "delivery" && consider) + { + fleet_handle->consider_delivery_requests(consider, consider); + RCLCPP_INFO( + this->node()->get_logger(), + "Fleet [%s] is configured to perform delivery tasks", + config.fleet_name().c_str()); + } + + if (task == "patrol" && consider) + { + fleet_handle->consider_patrol_requests(consider); + RCLCPP_INFO( + this->node()->get_logger(), + "Fleet [%s] is configured to perform patrol tasks", + config.fleet_name().c_str()); + } + + if (task == "clean" && consider) + { + fleet_handle->consider_cleaning_requests(consider); + RCLCPP_INFO( + this->node()->get_logger(), + "Fleet [%s] is configured to perform cleaning tasks", + config.fleet_name().c_str()); + } + } + + for (const auto& [action, consider] : config.action_consideration()) + { + fleet_handle->add_performable_action(action, consider); + } + + fleet_handle->default_maximum_delay(config.max_delay()); + fleet_handle->fleet_state_topic_publish_period(config.update_interval()); + + RCLCPP_INFO( + this->node()->get_logger(), + "Finished configuring Easy Full Control adapter for fleet [%s]", + config.fleet_name().c_str()); + + std::shared_ptr tf_dict; + if (config.transformations_to_robot_coordinates().has_value()) + { + tf_dict = std::make_shared( + *config.transformations_to_robot_coordinates()); + } + + return EasyFullControl::Implementation::make( + fleet_handle, + config.skip_rotation_commands(), + tf_dict, + config.default_responsive_wait(), + config.default_max_merge_waypoint_distance(), + config.default_max_merge_lane_distance(), + config.default_min_lane_length()); +} + //============================================================================== std::shared_ptr Adapter::add_fleet( const std::string& fleet_name, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp new file mode 100644 index 000000000..a60f57043 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -0,0 +1,2724 @@ +/* + * 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 "internal_EasyFullControl.hpp" +#include "internal_RobotUpdateHandle.hpp" +#include "internal_FleetUpdateHandle.hpp" + +// Public rmf_task API headers +#include +#include +#include +#include + +// ROS2 utilities for rmf_traffic +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +//============================================================================== +namespace rmf_fleet_adapter { +namespace agv { + +using ConsiderRequest = EasyFullControl::ConsiderRequest; + +//============================================================================== +class EasyFullControl::RobotState::Implementation +{ +public: + std::string map_name; + Eigen::Vector3d position; + double battery_soc; +}; + +//============================================================================== +EasyFullControl::RobotState::RobotState( + std::string map_name, + Eigen::Vector3d position, + double battery_soc) +: _pimpl(rmf_utils::make_impl(Implementation{ + std::move(map_name), + position, + battery_soc + })) +{ + // Do nothing +} + +//============================================================================== +const std::string& EasyFullControl::RobotState::map() const +{ + return _pimpl->map_name; +} + +//============================================================================== +void EasyFullControl::RobotState::set_map(std::string value) +{ + _pimpl->map_name = std::move(value); +} + +//============================================================================== +Eigen::Vector3d EasyFullControl::RobotState::position() const +{ + return _pimpl->position; +} + +//============================================================================== +void EasyFullControl::RobotState::set_position(Eigen::Vector3d value) +{ + _pimpl->position = value; +} + +//============================================================================== +double EasyFullControl::RobotState::battery_state_of_charge() const +{ + return _pimpl->battery_soc; +} + +//============================================================================== +void EasyFullControl::RobotState::set_battery_state_of_charge(double value) +{ + _pimpl->battery_soc = value; +} + +//============================================================================== +class EasyFullControl::RobotConfiguration::Implementation +{ +public: + std::vector compatible_chargers; + std::optional responsive_wait; + std::optional max_merge_waypoint_distance; + std::optional max_merge_lane_distance; + std::optional min_lane_length; +}; + +//============================================================================== +EasyFullControl::RobotConfiguration::RobotConfiguration( + std::vector compatible_chargers, + std::optional responsive_wait, + std::optional max_merge_waypoint_distance, + std::optional max_merge_lane_distance, + std::optional min_lane_length) +: _pimpl(rmf_utils::make_impl(Implementation{ + std::move(compatible_chargers), + responsive_wait, + max_merge_waypoint_distance, + max_merge_lane_distance, + min_lane_length + })) +{ + // Do nothing +} + +//============================================================================== +const std::vector& +EasyFullControl::RobotConfiguration::compatible_chargers() const +{ + return _pimpl->compatible_chargers; +} + +//============================================================================== +void EasyFullControl::RobotConfiguration::set_compatible_chargers( + std::vector chargers) +{ + _pimpl->compatible_chargers = std::move(chargers); +} + +//============================================================================== +std::optional EasyFullControl::RobotConfiguration::responsive_wait() const +{ + return _pimpl->responsive_wait; +} + +//============================================================================== +void EasyFullControl::RobotConfiguration::set_responsive_wait( + std::optional enable) +{ + _pimpl->responsive_wait = enable; +} + +//============================================================================== +std::optional EasyFullControl::RobotConfiguration:: +max_merge_waypoint_distance() const +{ + return _pimpl->max_merge_waypoint_distance; +} + +//============================================================================== +void EasyFullControl::RobotConfiguration::set_max_merge_waypoint_distance( + std::optional distance) +{ + _pimpl->max_merge_waypoint_distance = distance; +} + +//============================================================================== +std::optional EasyFullControl::RobotConfiguration:: +max_merge_lane_distance() const +{ + return _pimpl->max_merge_lane_distance; +} + +//============================================================================== +void EasyFullControl::RobotConfiguration::set_max_merge_lane_distance( + std::optional distance) +{ + _pimpl->max_merge_lane_distance = distance; +} + +//============================================================================== +std::optional EasyFullControl::RobotConfiguration::min_lane_length() +const +{ + return _pimpl->min_lane_length; +} + +//============================================================================== +void EasyFullControl::RobotConfiguration::set_min_lane_length( + std::optional distance) +{ + _pimpl->min_lane_length = distance; +} + +//============================================================================== +class EasyFullControl::RobotCallbacks::Implementation +{ +public: + NavigationRequest navigate; + StopRequest stop; + ActionExecutor action_executor; +}; + +//============================================================================== +EasyFullControl::RobotCallbacks::RobotCallbacks( + NavigationRequest navigate, + StopRequest stop, + ActionExecutor action_executor) +: _pimpl(rmf_utils::make_impl(Implementation{ + std::move(navigate), + std::move(stop), + std::move(action_executor) + })) +{ + // Do nothing +} + +//============================================================================== +auto EasyFullControl::RobotCallbacks::navigate() const -> NavigationRequest +{ + return _pimpl->navigate; +} + +//============================================================================== +auto EasyFullControl::RobotCallbacks::stop() const -> StopRequest +{ + return _pimpl->stop; +} + +//============================================================================== +auto EasyFullControl::RobotCallbacks::action_executor() const -> ActionExecutor +{ + return _pimpl->action_executor; +} + +//============================================================================== +class EasyFullControl::CommandExecution::Implementation +{ +public: + struct Data + { + 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; + + void release_stubbornness() + { + if (schedule_override.has_value()) + { + schedule_override->release_stubbornness(); + } + } + + void update_location( + const std::shared_ptr& context, + const std::string& map, + Eigen::Vector3d location) + { + if (schedule_override.has_value()) + { + return schedule_override->overridden_update( + context, + map, + location); + } + + 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& 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) + { + if (wp >= graph.num_waypoints()) + { + 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.", + context->requester_id().c_str(), + wp, + graph.num_waypoints()); + // 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 (!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 (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; + } + + if (closed_lanes.is_closed(lane_id)) + { + // Don't use a lane that's closed + 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)); + } + } + else + { + std::optional> on_lane; + for (auto lane_id : lanes) + { + 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); + } + } + } + + if (on_lane.has_value()) + { + 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 + { + starts = nav_params->compute_plan_starts(graph, map, location, now); + } + } + + if (!starts.empty()) + { + context->set_location(starts); + } + else + { + context->set_lost(Location { now, map, location }); + } + + if (!waypoints.empty()) + { + 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()) + { + 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); + } + } + + 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); + } + } + }; + 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()) + { + context->worker().schedule( + [ + context = context, + data = this->data, + identifier = this->identifier, + finisher = this->finisher + ](const auto&) + { + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) + { + // This activity has already finished + return; + } + + // 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(); + } + }); + } + } + + Stubbornness override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold) + { + 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&) + { + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) + { + // Don't do anything because this command is finished + return; + } + + data->release_stubbornness(); + data->schedule_override = ScheduleOverride::make( + context, map, path, hold, stubborn); + }); + } + + return Stubbornness::Implementation::make(stubborn); + } + + static CommandExecution make( + const std::shared_ptr& context, + Data data_, + std::function begin) + { + 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; + } + + static Implementation& get(CommandExecution& cmd) + { + return *cmd._pimpl; + } +}; + +//============================================================================== +void EasyFullControl::CommandExecution::finished() +{ + _pimpl->finish(); +} + +//============================================================================== +bool EasyFullControl::CommandExecution::okay() const +{ + if (!_pimpl->identifier) + { + return false; + } + + if (!ActivityIdentifier::Implementation::get(*_pimpl->identifier).update_fn) + { + return false; + } + + return true; +} + +//============================================================================== +auto EasyFullControl::CommandExecution::override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold) -> Stubbornness +{ + return _pimpl->override_schedule(std::move(map), std::move(path), hold); +} + +//============================================================================== +auto EasyFullControl::CommandExecution::identifier() const +-> ConstActivityIdentifierPtr +{ + return _pimpl->identifier; +} + +//============================================================================== +EasyFullControl::CommandExecution::CommandExecution() +{ + // Do nothing +} + +//============================================================================== +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 +{ + return _pimpl->map; +} + +//============================================================================== +Eigen::Vector3d EasyFullControl::Destination::position() const +{ + return _pimpl->position; +} + +//============================================================================== +Eigen::Vector2d EasyFullControl::Destination::xy() const +{ + return _pimpl->position.block<2, 1>(0, 0); +} + +//============================================================================== +double EasyFullControl::Destination::yaw() const +{ + return _pimpl->position[2]; +} + +//============================================================================== +std::optional EasyFullControl::Destination::graph_index() const +{ + return _pimpl->graph_index; +} + +//============================================================================== +std::optional EasyFullControl::Destination::speed_limit() const +{ + return _pimpl->speed_limit; +} + +//============================================================================== +std::optional EasyFullControl::Destination::dock() const +{ + return _pimpl->dock; +} + +//============================================================================== +EasyFullControl::Destination::Destination() +{ + // Do nothing +} + +//============================================================================== +struct ProgressTracker : std::enable_shared_from_this +{ + /// The queue of commands to execute while following this path, in reverse + /// order so that the next command can always be popped off the back. + std::vector reverse_queue; + EasyFullControl::ActivityIdentifierPtr current_identifier; + TriggerOnce finished; + + void next() + { + if (reverse_queue.empty()) + { + current_identifier = nullptr; + finished.trigger(); + return; + } + + auto current_activity = reverse_queue.back(); + reverse_queue.pop_back(); + current_identifier = EasyFullControl::CommandExecution::Implementation + ::get(current_activity).identifier; + auto& current_activity_impl = + EasyFullControl::CommandExecution::Implementation::get(current_activity); + current_activity_impl.finisher = [w_progress = weak_from_this()]() + { + if (const auto progress = w_progress.lock()) + { + progress->next(); + } + }; + const auto begin = current_activity_impl.begin; + if (begin) + { + begin(std::move(current_activity)); + } + } + + static std::shared_ptr make( + std::vector queue, + std::function finished) + { + std::reverse(queue.begin(), queue.end()); + auto tracker = std::make_shared(); + tracker->reverse_queue = std::move(queue); + tracker->finished = TriggerOnce(std::move(finished)); + return tracker; + } +}; + +//============================================================================== +/// Implements a state machine to send waypoints from follow_new_path() one +/// at a time to the robot via its API. Also updates state of robot via a timer. +class EasyCommandHandle : public RobotCommandHandle, + public std::enable_shared_from_this +{ +public: + using Planner = rmf_traffic::agv::Planner; + using Graph = rmf_traffic::agv::Graph; + using VehicleTraits = rmf_traffic::agv::VehicleTraits; + using ActionExecution = RobotUpdateHandle::ActionExecution; + using ActionExecutor = RobotUpdateHandle::ActionExecutor; + using NavigationRequest = EasyFullControl::NavigationRequest; + using StopRequest = EasyFullControl::StopRequest; + using Status = rmf_task::Event::Status; + using ActivityIdentifierPtr = EasyFullControl::ActivityIdentifierPtr; + + // State machine values. + enum class InternalRobotState : uint8_t + { + IDLE = 0, + MOVING = 1 + }; + + EasyCommandHandle( + std::shared_ptr nav_params, + NavigationRequest handle_nav_request, + StopRequest handle_stop); + + // Implement base class methods. + void stop() final; + + void follow_new_path( + const std::vector& waypoints, + ArrivalEstimator next_arrival_estimator, + RequestCompleted path_finished_callback) final; + + void dock( + const std::string& dock_name, + RequestCompleted docking_finished_callback) final; + + Eigen::Vector3d to_robot_coordinates( + const std::string& map, + Eigen::Vector3d position) const + { + if (!nav_params->transforms_to_robot_coords) + { + return position; + } + + const auto tf_it = nav_params->transforms_to_robot_coords->find(map); + if (tf_it == nav_params->transforms_to_robot_coords->end()) + { + 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; + } + + return tf_it->second.apply(position); + } + + std::weak_ptr w_context; + std::shared_ptr nav_params; + std::shared_ptr current_progress; + + // Callbacks from user + NavigationRequest handle_nav_request; + StopRequest handle_stop; +}; + +//============================================================================== +EasyCommandHandle::EasyCommandHandle( + std::shared_ptr nav_params_, + NavigationRequest handle_nav_request_, + StopRequest handle_stop_) +: nav_params(std::move(nav_params_)), + handle_nav_request(std::move(handle_nav_request_)), + handle_stop(std::move(handle_stop_)) +{ + // Do nothing +} + +//============================================================================== +void EasyCommandHandle::stop() +{ + if (!this->current_progress) + { + return; + } + + const auto activity_identifier = this->current_progress->current_identifier; + if (!activity_identifier) + { + return; + } + + /// Prevent any further specialized updates. + EasyFullControl::ActivityIdentifier::Implementation + ::get(*activity_identifier).update_fn = nullptr; + + this->current_progress = nullptr; + this->handle_stop(activity_identifier); +} + +//============================================================================== +void EasyCommandHandle::follow_new_path( + const std::vector& waypoints, + ArrivalEstimator next_arrival_estimator_, + RequestCompleted path_finished_callback_) +{ + const auto context = w_context.lock(); + if (!context) + { + return; + } + + RCLCPP_DEBUG( + context->node()->get_logger(), + "follow_new_path for robot [%s] with PlanId [%ld]", + context->requester_id().c_str(), + context->itinerary().current_plan_id()); + + if (waypoints.empty() || + next_arrival_estimator_ == nullptr || + path_finished_callback_ == nullptr) + { + RCLCPP_WARN( + context->node()->get_logger(), + "Received a new path for robot [%s] with invalid parameters. " + " Ignoring...", + context->requester_id().c_str()); + return; + } + + const auto& planner = context->planner(); + if (!planner) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Planner missing for [%s], cannot follow new path commands", + context->requester_id().c_str()); + return; + } + const auto& graph = planner->get_configuration().graph(); + std::optional opt_initial_map; + for (const auto& wp : waypoints) + { + if (wp.graph_index().has_value()) + { + const std::size_t i = *wp.graph_index(); + opt_initial_map = graph.get_waypoint(i).get_map_name(); + break; + } + } + + if (!opt_initial_map.has_value()) + { + for (const auto& l : context->location()) + { + opt_initial_map = graph.get_waypoint(l.waypoint()).get_map_name(); + break; + } + } + + if (!opt_initial_map.has_value()) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Could not find an initial map in follow_new_path command for robot [%s]." + "This means the robot is lost and may need an operator to intervene.", + context->requester_id().c_str()); + // Stop to prevent cascading problems. + stop(); + return; + } + std::string initial_map = *opt_initial_map; + + std::vector queue; + const auto& current_location = context->location(); + + bool found_connection = false; + std::size_t i0 = 0; + for (std::size_t i = 0; i < waypoints.size(); ++i) + { + const auto& wp = waypoints[i]; + if (wp.graph_index().has_value()) + { + for (const auto& l : current_location) + { + if (*wp.graph_index() == l.waypoint() && !l.lane().has_value()) + { + if (l.location().has_value()) + { + if (i > 0) + { + found_connection = true; + i0 = i - 1; + } + else + { + const double dist = + (*l.location() - wp.position().block<2, 1>(0, 0)).norm(); + if (dist <= nav_params->max_merge_lane_distance) + { + found_connection = true; + i0 = 0; + } + } + } + else + { + found_connection = true; + i0 = i; + } + } + } + } + else + { + for (const auto& l : current_location) + { + Eigen::Vector2d p_wp; + if (l.location().has_value()) + { + p_wp = l.location()->block<2, 1>(0, 0); + } + else + { + p_wp = graph.get_waypoint(l.waypoint()).get_location(); + } + const double dist = (*l.location() - p_wp).norm(); + if (dist <= nav_params->max_merge_lane_distance) + { + found_connection = true; + i0 = i; + } + } + } + + if (i > 0) + { + for (const auto lane : wp.approach_lanes()) + { + for (const auto& l : current_location) + { + if (l.lane().has_value()) + { + if (lane == *l.lane()) + { + found_connection = true; + i0 = i - 1; + } + } + } + } + } + } + + if (!found_connection) + { + // The robot has drifted away from the starting point since the plan started + // so we'll ask for a new plan. + RCLCPP_INFO( + context->node()->get_logger(), + "Requesting replan for [%s] because it is too far from its commanded path", + context->requester_id().c_str()); + + // Stop the robot to prevent it from diverging too far while a replan + // happens. + stop(); + context->request_replan(); + return; + } + + if (i0 >= waypoints.size() - 1) + { + // Always issue at least one command to approach the final waypoint. + i0 = waypoints.size() - 2; + } + + std::size_t i1 = i0 + 1; + while (i1 < waypoints.size()) + { + std::vector cmd_wps; + std::vector cmd_lanes; + const auto& wp0 = waypoints[i0]; + const auto& wp1 = waypoints[i1]; + if (wp0.graph_index().has_value()) + { + cmd_wps.push_back(*wp0.graph_index()); + } + + for (auto lane_id : wp1.approach_lanes()) + { + cmd_lanes.push_back(lane_id); + const auto& lane = graph.get_lane(lane_id); + const std::size_t entry_wp = lane.entry().waypoint_index(); + const std::size_t exit_wp = lane.exit().waypoint_index(); + for (auto wp : {entry_wp, exit_wp}) + { + if (std::find(cmd_wps.begin(), cmd_wps.end(), wp) == cmd_wps.end()) + { + cmd_wps.push_back(wp); + } + } + } + + std::string map = [&]() + { + if (wp1.graph_index().has_value()) + { + return graph.get_waypoint(*wp1.graph_index()).get_map_name(); + } + + return initial_map; + }(); + if (initial_map != map) + { + initial_map = map; + } + + std::optional speed_limit; + if (!wp1.approach_lanes().empty()) + { + const auto arrival_lane = wp1.approach_lanes().back(); + speed_limit = graph.get_lane(arrival_lane).properties().speed_limit(); + } + + Eigen::Vector3d target_position = wp1.position(); + std::size_t target_index = i1; + rmf_traffic::Duration planned_wait_time = rmf_traffic::Duration(0); + if (nav_params->skip_rotation_commands) + { + std::size_t i2 = i1 + 1; + while (i2 < waypoints.size()) + { + const auto& wp2 = waypoints[i2]; + if (wp1.graph_index().has_value() && wp2.graph_index().has_value()) + { + 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) + { + // The plan had a wait between these points. + planned_wait_time += wp2.time() - wp1.time(); + } + ++i2; + continue; + } + } + 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); + + queue.push_back( + EasyFullControl::CommandExecution::Implementation::make( + context, + EasyFullControl::CommandExecution::Implementation::Data{ + cmd_wps, + cmd_lanes, + target_position[2], + planned_wait_time, + std::nullopt, + nav_params, + [next_arrival_estimator_, target_index](rmf_traffic::Duration dt) + { + next_arrival_estimator_(target_index, dt); + } + }, + [ + handle_nav_request = this->handle_nav_request, + destination = std::move(destination) + ](EasyFullControl::CommandExecution execution) + { + handle_nav_request(destination, execution); + } + )); + + i0 = target_index; + i1 = i0 + 1; + } + + this->current_progress = ProgressTracker::make( + queue, + path_finished_callback_); + this->current_progress->next(); +} + +//============================================================================== +namespace { +class DockFinder : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + DockFinder(std::string dock_name) + : looking_for(std::move(dock_name)) + { + // Do nothing + } + + void execute(const DoorOpen&) override {} + void execute(const DoorClose&) override {} + void execute(const LiftSessionBegin&) override {} + void execute(const LiftDoorOpen&) override {} + void execute(const LiftSessionEnd&) override {} + void execute(const LiftMove&) override {} + void execute(const Wait&) override {} + void execute(const Dock& dock) override + { + if (looking_for == dock.dock_name()) + { + found = true; + } + } + + std::string looking_for; + bool found = false; +}; +} // anonymous namespace + +//============================================================================== +void EasyCommandHandle::dock( + const std::string& dock_name_, + RequestCompleted docking_finished_callback_) +{ + const auto context = w_context.lock(); + if (!context) + { + return; + } + + RCLCPP_DEBUG( + context->node()->get_logger(), + "Received a request to dock robot [%s] at [%s]...", + context->requester_id().c_str(), + dock_name_.c_str()); + + const auto plan_id = context->itinerary().current_plan_id(); + auto planner = context->planner(); + if (!planner) + { + RCLCPP_ERROR( + context->node()->get_logger(), + "Planner unavailable for robot [%s], cannot execute docking command [%s]", + context->requester_id().c_str(), + dock_name_.c_str()); + return; + } + + const auto& graph = planner->get_configuration().graph(); + DockFinder finder(dock_name_); + std::optional found_lane; + for (std::size_t i = 0; i < graph.num_lanes(); ++i) + { + const auto& lane = graph.get_lane(i); + if (const auto event = lane.entry().event()) + { + event->execute(finder); + } + + if (const auto event = lane.exit().event()) + { + event->execute(finder); + } + + if (finder.found) + { + found_lane = i; + break; + } + } + + if (!found_lane.has_value()) + { + RCLCPP_WARN( + context->node()->get_logger(), + "Unable to find a dock named [%s] in the graph for robot [%s]. We " + "will skip this command as finished to avoid permanently blocking.", + dock_name_.c_str(), + context->requester_id().c_str()); + docking_finished_callback_(); + return; + } + + const auto& lane = graph.get_lane(*found_lane); + const std::size_t i0 = lane.entry().waypoint_index(); + const std::size_t i1 = lane.exit().waypoint_index(); + const auto& wp0 = graph.get_waypoint(i0); + const auto& wp1 = graph.get_waypoint(i1); + const Eigen::Vector2d p0 = wp0.get_location(); + const Eigen::Vector2d p1 = wp1.get_location(); + 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); + + auto data = EasyFullControl::CommandExecution::Implementation::Data{ + {i0, i1}, + {*found_lane}, + std::nullopt, + rmf_traffic::Duration(0), + std::nullopt, + nav_params, + [w_context = context->weak_from_this(), expected_arrival, plan_id]( + rmf_traffic::Duration dt) + { + const auto context = w_context.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)); + } + }; + + const double dy = p1.y() - p0.y(); + const double dx = p1.x() - p0.x(); + double angle = 0.0; + if (dx*dx + dy*dy > 1e-6) + { + angle = std::atan2(dy, dx); + } + + const Eigen::Vector3d position(p1.x(), p1.y(), angle); + const auto command_position = to_robot_coordinates( + wp1.get_map_name(), position); + + auto destination = EasyFullControl::Destination::Implementation::make( + wp1.get_map_name(), + command_position, + i1, + lane.properties().speed_limit(), + dock_name_); + + auto cmd = EasyFullControl::CommandExecution::Implementation::make( + context, + data, + [ + handle_nav_request = this->handle_nav_request, + destination = std::move(destination) + ]( + EasyFullControl::CommandExecution execution) + { + handle_nav_request(destination, execution); + }); + this->current_progress = ProgressTracker::make( + {std::move(cmd)}, + std::move(docking_finished_callback_)); + this->current_progress->next(); +} + +//============================================================================== +ConsiderRequest consider_all() +{ + return [](const nlohmann::json&, FleetUpdateHandle::Confirmation& confirm) + { + confirm.accept(); + }; +} + +//============================================================================== +class EasyFullControl::EasyRobotUpdateHandle::Implementation +{ +public: + + struct Updater + { + std::shared_ptr handle; + std::shared_ptr nav_params; + + Updater(std::shared_ptr params_) + : handle(nullptr), + nav_params(std::move(params_)) + { + // Do nothing + } + + Eigen::Vector3d to_rmf_coordinates( + const std::string& map, + Eigen::Vector3d position, + const RobotContext& context) const + { + const auto p = nav_params->to_rmf_coordinates(map, position); + if (p.has_value()) + { + return *p; + } + + 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; + } + }; + + std::shared_ptr updater; + rxcpp::schedulers::worker worker; + + static Implementation& get(EasyRobotUpdateHandle& handle) + { + return *handle._pimpl; + } + + Implementation( + std::shared_ptr params_, + rxcpp::schedulers::worker worker_) + : updater(std::make_shared(params_)), + worker(worker_) + { + // Do nothing + } + + static std::shared_ptr make( + std::shared_ptr params_, + rxcpp::schedulers::worker worker_) + { + auto handle = std::shared_ptr( + new EasyRobotUpdateHandle); + handle->_pimpl = rmf_utils::make_unique_impl( + std::move(params_), std::move(worker_)); + return handle; + } +}; + +//============================================================================== +void EasyFullControl::EasyRobotUpdateHandle::update( + RobotState state, + ConstActivityIdentifierPtr current_activity) +{ + _pimpl->worker.schedule( + [ + state = std::move(state), + current_activity = std::move(current_activity), + updater = _pimpl->updater + ](const auto&) + { + if (!updater->handle) + { + return; + } + + auto context = RobotUpdateHandle::Implementation + ::get(*updater->handle).get_context(); + + context->current_battery_soc(state.battery_state_of_charge()); + + const auto position = updater->to_rmf_coordinates( + state.map(), state.position(), *context); + + if (current_activity) + { + const auto update_fn = + ActivityIdentifier::Implementation::get(*current_activity).update_fn; + if (update_fn) + { + update_fn(state.map(), position); + return; + } + } + + 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& 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 }); + } + }); +} + +//============================================================================== +double EasyFullControl::EasyRobotUpdateHandle::max_merge_waypoint_distance() +const +{ + return _pimpl->updater->nav_params->max_merge_waypoint_distance; +} + +//============================================================================== +void EasyFullControl::EasyRobotUpdateHandle::set_max_merge_waypoint_distance( + double distance) +{ + _pimpl->updater->nav_params->max_merge_waypoint_distance = distance; +} + +//============================================================================== +double EasyFullControl::EasyRobotUpdateHandle::max_merge_lane_distance() const +{ + return _pimpl->updater->nav_params->max_merge_lane_distance; +} + +//============================================================================== +void EasyFullControl::EasyRobotUpdateHandle::set_max_merge_lane_distance( + double distance) +{ + _pimpl->updater->nav_params->max_merge_lane_distance = distance; +} + +//============================================================================== +double EasyFullControl::EasyRobotUpdateHandle::min_lane_length() const +{ + return _pimpl->updater->nav_params->min_lane_length; +} + +//============================================================================== +void EasyFullControl::EasyRobotUpdateHandle::set_min_lane_length( + double length) +{ + _pimpl->updater->nav_params->min_lane_length = length; +} + +//============================================================================== +std::shared_ptr +EasyFullControl::EasyRobotUpdateHandle::more() +{ + if (_pimpl->updater) + { + return _pimpl->updater->handle; + } + return nullptr; +} + +//============================================================================== +std::shared_ptr +EasyFullControl::EasyRobotUpdateHandle::more() const +{ + if (_pimpl->updater) + { + return _pimpl->updater->handle; + } + + return nullptr; +} + +//============================================================================== +EasyFullControl::EasyRobotUpdateHandle::EasyRobotUpdateHandle() +{ + // Do nothing +} + +//============================================================================== +class EasyFullControl::FleetConfiguration::Implementation +{ +public: + std::string fleet_name; + std::optional> transformations_to_robot_coordinates; + std::unordered_map known_robot_configurations; + std::shared_ptr traits; + std::shared_ptr graph; + rmf_battery::agv::ConstBatterySystemPtr battery_system; + rmf_battery::ConstMotionPowerSinkPtr motion_sink; + rmf_battery::ConstDevicePowerSinkPtr ambient_sink; + rmf_battery::ConstDevicePowerSinkPtr tool_sink; + double recharge_threshold; + double recharge_soc; + bool account_for_battery_drain; + std::unordered_map task_consideration; + std::unordered_map action_consideration; + rmf_task::ConstRequestFactoryPtr finishing_request; + bool skip_rotation_commands; + std::optional server_uri; + rmf_traffic::Duration max_delay; + rmf_traffic::Duration update_interval; + bool default_responsive_wait; + double default_max_merge_waypoint_distance; + double default_max_merge_lane_distance; + double default_min_lane_length; +}; + +//============================================================================== +EasyFullControl::FleetConfiguration::FleetConfiguration( + const std::string& fleet_name, + std::optional> + transformations_to_robot_coordinates, + std::unordered_map + known_robot_configurations, + std::shared_ptr traits, + std::shared_ptr graph, + rmf_battery::agv::ConstBatterySystemPtr battery_system, + rmf_battery::ConstMotionPowerSinkPtr motion_sink, + rmf_battery::ConstDevicePowerSinkPtr ambient_sink, + rmf_battery::ConstDevicePowerSinkPtr tool_sink, + double recharge_threshold, + double recharge_soc, + bool account_for_battery_drain, + std::unordered_map task_consideration, + std::unordered_map action_consideration, + rmf_task::ConstRequestFactoryPtr finishing_request, + bool skip_rotation_commands, + std::optional server_uri, + rmf_traffic::Duration max_delay, + rmf_traffic::Duration update_interval, + bool default_responsive_wait, + double default_max_merge_waypoint_distance, + double default_max_merge_lane_distance, + double default_min_lane_length) +: _pimpl(rmf_utils::make_impl( + Implementation{ + std::move(fleet_name), + std::move(transformations_to_robot_coordinates), + std::move(known_robot_configurations), + std::move(traits), + std::move(graph), + std::move(battery_system), + std::move(motion_sink), + std::move(ambient_sink), + std::move(tool_sink), + std::move(recharge_threshold), + std::move(recharge_soc), + std::move(account_for_battery_drain), + std::move(task_consideration), + std::move(action_consideration), + std::move(finishing_request), + skip_rotation_commands, + std::move(server_uri), + std::move(max_delay), + std::move(update_interval), + default_responsive_wait, + std::move(default_max_merge_waypoint_distance), + std::move(default_max_merge_lane_distance), + std::move(default_min_lane_length) + })) +{ + // Do nothing +} + +//============================================================================== +std::optional +EasyFullControl::FleetConfiguration::from_config_files( + const std::string& config_file, + const std::string& nav_graph_path, + std::optional server_uri) +{ + // Load fleet config file + const auto fleet_config = YAML::LoadFile(config_file); + // Check that config file is valid and contains all necessary nodes + const YAML::Node rmf_fleet = fleet_config["rmf_fleet"]; + if (!rmf_fleet) + { + std::cout + << "rmf_fleet dictionary is not provided in the configuration file [" + << config_file << "] so we cannot parse a fleet configuration." + << std::endl; + return std::nullopt; + } + + // Fleet name + if (!rmf_fleet["name"]) + { + std::cout << "Fleet name is not provided" << std::endl; + return std::nullopt; + } + const std::string fleet_name = rmf_fleet["name"].as(); + + // Profile + if (!rmf_fleet["profile"] || !rmf_fleet["profile"]["footprint"] || + !rmf_fleet["profile"]["vicinity"]) + { + std::cout << "Fleet profile is not provided" << std::endl; + return std::nullopt; + } + const YAML::Node profile = rmf_fleet["profile"]; + const double footprint_rad = profile["footprint"].as(); + const double vicinity_rad = profile["vicinity"].as(); + + // Traits + if (!rmf_fleet["limits"] || !rmf_fleet["limits"]["linear"] || + !rmf_fleet["limits"]["angular"]) + { + std::cout << "Fleet traits are not provided" << std::endl; + return std::nullopt; + } + const YAML::Node limits = rmf_fleet["limits"]; + const YAML::Node linear = limits["linear"]; + const double v_nom = linear[0].as(); + const double a_nom = linear[1].as(); + const YAML::Node angular = limits["angular"]; + const double w_nom = angular[0].as(); + const double b_nom = angular[1].as(); + + // Reversibility, defaults to false + bool reversible = false; + if (rmf_fleet["reversible"]) + { + reversible = rmf_fleet["reversible"].as(); + } + + auto traits = std::make_shared(VehicleTraits{ + {v_nom, a_nom}, + {w_nom, b_nom}, + rmf_traffic::Profile{ + rmf_traffic::geometry::make_final_convex( + footprint_rad), + rmf_traffic::geometry::make_final_convex( + vicinity_rad) + } + }); + traits->get_differential()->set_reversible(reversible); + + // Graph + const auto graph = parse_graph(nav_graph_path, *traits); + + // Set up parameters required for task planner + // Battery system + if (!rmf_fleet["battery_system"] || !rmf_fleet["battery_system"]["voltage"] || + !rmf_fleet["battery_system"]["capacity"] || + !rmf_fleet["battery_system"]["charging_current"]) + { + std::cout << "Fleet battery system is not provided" << std::endl; + return std::nullopt; + } + const YAML::Node battery = rmf_fleet["battery_system"]; + const double voltage = battery["voltage"].as(); + const double capacity = battery["capacity"].as(); + const double charging_current = battery["charging_current"].as(); + + const auto battery_system_optional = rmf_battery::agv::BatterySystem::make( + voltage, capacity, charging_current); + if (!battery_system_optional.has_value()) + { + std::cout << "Invalid battery parameters" << std::endl; + return std::nullopt; + } + const auto battery_system = std::make_shared( + *battery_system_optional); + + // Mechanical system + if (!rmf_fleet["mechanical_system"]) + { + std::cout << "Fleet [mechanical_system] information is not provided" + << std::endl; + return std::nullopt; + } + + if ( + !rmf_fleet["mechanical_system"]["mass"] || + !rmf_fleet["mechanical_system"]["moment_of_inertia"] || + !rmf_fleet["mechanical_system"]["friction_coefficient"]) + { + std::cout << "Fleet [mechanical_system] information is not valid" + << std::endl; + return std::nullopt; + } + + const YAML::Node mechanical = rmf_fleet["mechanical_system"]; + const double mass = mechanical["mass"].as(); + const double moment_of_inertia = mechanical["moment_of_inertia"].as(); + const double friction = mechanical["friction_coefficient"].as(); + + auto mechanical_system_optional = rmf_battery::agv::MechanicalSystem::make( + mass, moment_of_inertia, friction); + if (!mechanical_system_optional.has_value()) + { + std::cout << "Invalid mechanical parameters" << std::endl; + return std::nullopt; + } + rmf_battery::agv::MechanicalSystem& mechanical_system = + *mechanical_system_optional; + + const auto motion_sink = + std::make_shared( + *battery_system, mechanical_system); + + // Ambient power system + if (!rmf_fleet["ambient_system"] || !rmf_fleet["ambient_system"]["power"]) + { + std::cout << "Fleet ambient system is not provided" << std::endl; + return std::nullopt; + } + const YAML::Node ambient_system = rmf_fleet["ambient_system"]; + const double ambient_power_drain = ambient_system["power"].as(); + auto ambient_power_system = rmf_battery::agv::PowerSystem::make( + ambient_power_drain); + if (!ambient_power_system) + { + std::cout << "Invalid values supplied for ambient power system" + << std::endl; + return std::nullopt; + } + const auto ambient_sink = + std::make_shared( + *battery_system, *ambient_power_system); + + // Tool power system + if (!rmf_fleet["tool_system"] || !rmf_fleet["tool_system"]["power"]) + { + std::cout << "Fleet tool system is not provided" << std::endl; + return std::nullopt; + } + const YAML::Node tool_system = rmf_fleet["tool_system"]; + const double tool_power_drain = ambient_system["power"].as(); + auto tool_power_system = rmf_battery::agv::PowerSystem::make( + tool_power_drain); + if (!tool_power_system) + { + std::cout << "Invalid values supplied for tool power system" << std::endl; + return std::nullopt; + } + const auto tool_sink = + std::make_shared( + *battery_system, *tool_power_system); + + // Drain battery + bool account_for_battery_drain = true; + if (!rmf_fleet["account_for_battery_drain"]) + { + std::cout << "[account_for_battery_drain] value is not provided, " + << "default to True" << std::endl; + } + else + { + account_for_battery_drain = + rmf_fleet["account_for_battery_drain"].as(); + } + // Recharge threshold + double recharge_threshold = 0.2; + if (!rmf_fleet["recharge_threshold"]) + { + std::cout + << "Recharge threshold is not provided, default to 0.2" << std::endl; + } + else + { + recharge_threshold = rmf_fleet["recharge_threshold"].as(); + } + // Recharge state of charge + double recharge_soc = 0.2; + if (!rmf_fleet["recharge_soc"]) + { + std::cout << "Recharge state of charge is not provided, default to 1.0" + << std::endl; + } + else + { + recharge_soc = rmf_fleet["recharge_soc"].as(); + } + + // Task capabilities + std::unordered_map task_consideration; + const YAML::Node task_capabilities = rmf_fleet["task_capabilities"]; + if (!task_capabilities) + { + std::cout + << "[task_capabilities] dictionary was not provided in config file [" + << config_file << "]" << std::endl; + } + else + { + for (const auto& capability : task_capabilities) + { + std::string task = capability.first.as(); + if (task == "loop") + { + // Always change loop to patrol to support legacy terminology + task = "patrol"; + } + + if (capability.second.as()) + { + task_consideration[task] = consider_all(); + } + } + + if (task_consideration.empty()) + { + std::cout + << "No known task capabilities found in config file [" + << config_file << "]" << std::endl; + } + } + + // Action considerations + std::unordered_map action_consideration; + const auto& actions_yaml = rmf_fleet["actions"]; + if (actions_yaml) + { + const auto actions = actions_yaml.as>(); + for (const std::string& action : actions) + { + action_consideration[action] = consider_all(); + } + } + + // Finishing tasks + std::string finishing_request_string; + const auto& finishing_request_yaml = rmf_fleet["finishing_request"]; + if (!finishing_request_yaml) + { + std::cout + << "Finishing request is not provided. The valid finishing requests " + "are [charge, park, nothing]. The task planner will default to [nothing]." + << std::endl; + } + else + { + finishing_request_string = finishing_request_yaml.as(); + } + std::cout << "Finishing request: " << finishing_request_string << std::endl; + rmf_task::ConstRequestFactoryPtr finishing_request; + if (finishing_request_string == "charge") + { + finishing_request = + std::make_shared(); + std::cout + << "Fleet is configured to perform ChargeBattery as finishing request" + << std::endl; + } + else if (finishing_request_string == "park") + { + finishing_request = + std::make_shared(); + std::cout + << "Fleet is configured to perform ParkRobot as finishing request" + << std::endl; + } + else if (finishing_request_string == "nothing") + { + std::cout << "Fleet is not configured to perform any finishing request" + << std::endl; + } + else + { + std::cout + << "Provided finishing request " << finishing_request_string + << "is unsupported. The valid finishing requests are" + "[charge, park, nothing]. The task planner will default to [nothing]."; + } + + // Ignore rotations within path commands + bool skip_rotation_commands = true; + if (rmf_fleet["skip_rotation_commands"]) + { + skip_rotation_commands = rmf_fleet["skip_rotation_commands"].as(); + } + + // Set the fleet state topic publish period + double fleet_state_frequency = 2.0; + if (!rmf_fleet["publish_fleet_state"]) + { + std::cout + << "Fleet state publish frequency is not provided, default to 2.0" + << std::endl; + } + else + { + fleet_state_frequency = rmf_fleet["publish_fleet_state"].as(); + } + const double update_interval = 1.0/fleet_state_frequency; + + // Set the maximum delay + double max_delay = 10.0; + if (!rmf_fleet["max_delay"]) + { + std::cout << "Maximum delay is not provided, default to 10.0" << std::endl; + } + else + { + max_delay = rmf_fleet["max_delay"].as(); + } + + std::optional> tf_dict; + const auto transforms = rmf_fleet["transforms"]; + if (transforms) + { + if (!transforms.IsMap()) + { + std::cerr + << "[transforms] entry should be a dictionary in config file [" + << config_file << "]" << std::endl; + } + else + { + tf_dict = std::unordered_map(); + for (const auto& node : transforms) + { + const auto map = node.first.as(); + const auto& transform_yaml = node.second; + const auto translation_yaml = transform_yaml["translation"]; + Eigen::Vector2d translation = Eigen::Vector2d(0.0, 0.0); + if (translation_yaml) + { + const auto xy_vec = translation_yaml.as>(); + if (xy_vec.size() != 2) + { + std::cerr + << "[traslation] element has invalid number of elements [" + << xy_vec.size() << "] (it should be exactly 2) in config file [" + << config_file << "]" << std::endl; + } + + std::size_t N = std::min(xy_vec.size(), (std::size_t)2); + for (std::size_t i = 0; i < N; ++i) + { + translation[i] = xy_vec[i]; + } + } + + const auto rotation_yaml = transform_yaml["rotation"]; + double rotation = 0.0; + if (rotation_yaml) + { + rotation = rotation_yaml.as(); + } + + const auto scale_yaml = transform_yaml["scale"]; + double scale = 1.0; + if (scale_yaml) + { + scale = scale_yaml.as(); + } + + tf_dict->insert({map, Transformation(rotation, scale, translation)}); + } + } + } + + bool default_responsive_wait = false; + const YAML::Node& default_responsive_wait_yaml = rmf_fleet["responsive_wait"]; + if (default_responsive_wait_yaml) + { + default_responsive_wait = default_responsive_wait_yaml.as(); + } + + double default_max_merge_waypoint_distance = 1e-3; + const YAML::Node& default_max_merge_waypoint_distance_yaml = + rmf_fleet["max_merge_waypoint_distance"]; + if (default_max_merge_waypoint_distance_yaml) + { + default_max_merge_waypoint_distance = + default_max_merge_waypoint_distance_yaml.as(); + } + + double default_max_merge_lane_distance = 0.3; + const YAML::Node& default_max_merge_lane_distance_yaml = + rmf_fleet["max_merge_lane_distance"]; + if (default_max_merge_lane_distance_yaml) + { + default_max_merge_lane_distance = + default_max_merge_lane_distance_yaml.as(); + } + + double default_min_lane_length = 1e-8; + const YAML::Node& default_min_lane_length_yaml = rmf_fleet["min_lane_length"]; + if (default_min_lane_length_yaml) + { + default_min_lane_length = + default_min_lane_length_yaml.as(); + } + + std::unordered_map known_robot_configurations; + const YAML::Node& robots = rmf_fleet["robots"]; + if (robots) + { + if (!robots.IsMap()) + { + std::cerr + << "[robots] element is not a map in config file [" << config_file + << "] so we cannot parse any known robot configurations." << std::endl; + return std::nullopt; + } + else + { + for (const auto& robot : robots) + { + const auto robot_name = robot.first.as(); + const YAML::Node& robot_config_yaml = robot.second; + + if (!robot_config_yaml.IsMap()) + { + std::cerr + << "Entry for [" << robot_name << "] in [robots] dictionary is not " + << "a dictionary, so it cannot be parsed." << std::endl; + return std::nullopt; + } + + std::string charger; + const YAML::Node& charger_yaml = robot_config_yaml["charger"]; + if (charger_yaml) + { + charger = charger_yaml.as(); + } + else + { + 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; + } + + 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_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(); + } + + auto config = RobotConfiguration({std::move(charger)}, + responsive_wait, + max_merge_waypoint_distance, + max_merge_lane_distance, + min_lane_length); + known_robot_configurations.insert_or_assign(robot_name, config); + } + } + } + + return FleetConfiguration( + fleet_name, + std::move(tf_dict), + std::move(known_robot_configurations), + std::move(traits), + std::make_shared(std::move(graph)), + battery_system, + motion_sink, + ambient_sink, + tool_sink, + recharge_threshold, + recharge_soc, + account_for_battery_drain, + task_consideration, + action_consideration, + finishing_request, + skip_rotation_commands, + server_uri, + rmf_traffic::time::from_seconds(max_delay), + rmf_traffic::time::from_seconds(update_interval), + default_responsive_wait, + default_max_merge_waypoint_distance, + default_max_merge_lane_distance, + default_min_lane_length); +} + +//============================================================================== +const std::string& EasyFullControl::FleetConfiguration::fleet_name() const +{ + return _pimpl->fleet_name; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_fleet_name(std::string value) +{ + _pimpl->fleet_name = std::move(value); +} + +//============================================================================== +const std::optional& +EasyFullControl::FleetConfiguration::transformations_to_robot_coordinates() +const +{ + return _pimpl->transformations_to_robot_coordinates; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::add_robot_coordinate_transformation( + std::string map, + Transformation transformation) +{ + if (!_pimpl->transformations_to_robot_coordinates.has_value()) + { + _pimpl->transformations_to_robot_coordinates = TransformDictionary(); + } + + _pimpl->transformations_to_robot_coordinates + ->insert_or_assign(std::move(map), transformation); +} + +//============================================================================== +auto EasyFullControl::FleetConfiguration::known_robot_configurations() const +-> const std::unordered_map& +{ + return _pimpl->known_robot_configurations; +} + +//============================================================================== +std::vector +EasyFullControl::FleetConfiguration::known_robots() const +{ + std::vector names; + for (const auto& [name, _] : _pimpl->known_robot_configurations) + { + names.push_back(name); + } + return names; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::add_known_robot_configuration( + std::string robot_name, + RobotConfiguration configuration) +{ + _pimpl->known_robot_configurations.insert_or_assign( + std::move(robot_name), std::move(configuration)); +} + +//============================================================================== +auto EasyFullControl::FleetConfiguration::get_known_robot_configuration( + const std::string& robot_name) const -> std::optional +{ + const auto r_it = _pimpl->known_robot_configurations.find(robot_name); + if (r_it == _pimpl->known_robot_configurations.end()) + return std::nullopt; + + return r_it->second; +} + +//============================================================================== +auto EasyFullControl::FleetConfiguration::vehicle_traits() const +-> const std::shared_ptr& +{ + return _pimpl->traits; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_vehicle_traits( + std::shared_ptr value) +{ + _pimpl->traits = std::move(value); +} + +//============================================================================== +auto EasyFullControl::FleetConfiguration::graph() const +-> const std::shared_ptr& +{ + return _pimpl->graph; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_graph( + std::shared_ptr value) +{ + _pimpl->graph = std::move(value); +} + +//============================================================================== +rmf_battery::agv::ConstBatterySystemPtr +EasyFullControl::FleetConfiguration::battery_system() const +{ + return _pimpl->battery_system; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_battery_system( + rmf_battery::agv::ConstBatterySystemPtr value) +{ + _pimpl->battery_system = std::move(value); +} + +//============================================================================== +rmf_battery::ConstMotionPowerSinkPtr +EasyFullControl::FleetConfiguration::motion_sink() const +{ + return _pimpl->motion_sink; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_motion_sink( + rmf_battery::ConstMotionPowerSinkPtr value) +{ + _pimpl->motion_sink = std::move(value); +} + +//============================================================================== +rmf_battery::ConstDevicePowerSinkPtr +EasyFullControl::FleetConfiguration::ambient_sink() const +{ + return _pimpl->ambient_sink; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_ambient_sink( + rmf_battery::ConstDevicePowerSinkPtr value) +{ + _pimpl->ambient_sink = std::move(value); +} + +//============================================================================== +rmf_battery::ConstDevicePowerSinkPtr +EasyFullControl::FleetConfiguration::tool_sink() const +{ + return _pimpl->tool_sink; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_tool_sink( + rmf_battery::ConstDevicePowerSinkPtr value) +{ + _pimpl->tool_sink = std::move(value); +} + +//============================================================================== +double EasyFullControl::FleetConfiguration::recharge_threshold() const +{ + return _pimpl->recharge_threshold; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_recharge_threshold(double value) +{ + _pimpl->recharge_threshold = value; +} + +//============================================================================== +double EasyFullControl::FleetConfiguration::recharge_soc() const +{ + return _pimpl->recharge_soc; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_recharge_soc(double value) +{ + _pimpl->recharge_soc = value; +} + +//============================================================================== +bool EasyFullControl::FleetConfiguration::account_for_battery_drain() const +{ + return _pimpl->account_for_battery_drain; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_account_for_battery_drain( + bool value) +{ + _pimpl->account_for_battery_drain = value; +} + +//============================================================================== +const std::unordered_map& +EasyFullControl::FleetConfiguration::task_consideration() const +{ + return _pimpl->task_consideration; +} + +//============================================================================== +std::unordered_map& +EasyFullControl::FleetConfiguration::task_consideration() +{ + return _pimpl->task_consideration; +} + +//============================================================================== +const std::unordered_map& +EasyFullControl::FleetConfiguration::action_consideration() const +{ + return _pimpl->action_consideration; +} + +//============================================================================== +std::unordered_map& +EasyFullControl::FleetConfiguration::action_consideration() +{ + return _pimpl->action_consideration; +} + +//============================================================================== +rmf_task::ConstRequestFactoryPtr +EasyFullControl::FleetConfiguration::finishing_request() const +{ + return _pimpl->finishing_request; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_finishing_request( + rmf_task::ConstRequestFactoryPtr value) +{ + _pimpl->finishing_request = std::move(value); +} + +//============================================================================== +bool EasyFullControl::FleetConfiguration::skip_rotation_commands() const +{ + return _pimpl->skip_rotation_commands; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_skip_rotation_commands(bool value) +{ + _pimpl->skip_rotation_commands = value; +} + +//============================================================================== +std::optional EasyFullControl::FleetConfiguration::server_uri() +const +{ + return _pimpl->server_uri; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_server_uri( + std::optional value) +{ + _pimpl->server_uri = std::move(value); +} + +//============================================================================== +rmf_traffic::Duration EasyFullControl::FleetConfiguration::max_delay() const +{ + return _pimpl->max_delay; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_max_delay( + rmf_traffic::Duration value) +{ + _pimpl->max_delay = value; +} + +//============================================================================== +rmf_traffic::Duration EasyFullControl::FleetConfiguration::update_interval() +const +{ + return _pimpl->update_interval; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_update_interval( + rmf_traffic::Duration value) +{ + _pimpl->update_interval = value; +} + +//============================================================================== +bool EasyFullControl::FleetConfiguration::default_responsive_wait() const +{ + return _pimpl->default_responsive_wait; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_default_responsive_wait( + bool enable) +{ + _pimpl->default_responsive_wait = enable; +} + +//============================================================================== +double EasyFullControl::FleetConfiguration::default_max_merge_waypoint_distance() +const +{ + return _pimpl->default_max_merge_waypoint_distance; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration:: +set_default_max_merge_waypoint_distance(double distance) +{ + _pimpl->default_max_merge_waypoint_distance = distance; +} + +//============================================================================== +double EasyFullControl::FleetConfiguration::default_max_merge_lane_distance() +const +{ + return _pimpl->default_max_merge_lane_distance; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_default_max_merge_lane_distance( + double distance) +{ + _pimpl->default_max_merge_lane_distance = distance; +} + +//============================================================================== +double EasyFullControl::FleetConfiguration::default_min_lane_length() const +{ + return _pimpl->default_min_lane_length; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_default_min_lane_length( + double distance) +{ + _pimpl->default_min_lane_length = distance; +} + +//============================================================================== +using EasyCommandHandlePtr = std::shared_ptr; + +//============================================================================== +std::shared_ptr EasyFullControl::more() +{ + return _pimpl->fleet_handle; +} + +//============================================================================== +std::shared_ptr EasyFullControl::more() const +{ + return _pimpl->fleet_handle; +} + +//============================================================================== +auto EasyFullControl::add_robot( + std::string robot_name, + RobotState initial_state, + RobotConfiguration configuration, + RobotCallbacks callbacks) -> std::shared_ptr +{ + const auto node = _pimpl->node(); + + auto worker = + FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle).worker; + auto robot_nav_params = std::make_shared(_pimpl->nav_params); + auto easy_updater = EasyRobotUpdateHandle::Implementation::make( + robot_nav_params, worker); + + const auto& fleet_impl = + FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle); + const auto& planner = *fleet_impl.planner; + const auto& graph = planner->get_configuration().graph(); + const auto& traits = planner->get_configuration().vehicle_traits(); + const auto& fleet_name = _pimpl->fleet_handle->fleet_name(); + + RCLCPP_INFO( + node->get_logger(), + "Adding robot [%s] to fleet [%s].", + robot_name.c_str(), + fleet_name.c_str()); + + auto insertion = _pimpl->cmd_handles.insert({robot_name, nullptr}); + if (!insertion.second) + { + RCLCPP_WARN( + node->get_logger(), + "Robot [%s] was previously added to fleet [%s]. Ignoring request...", + robot_name.c_str(), + fleet_name.c_str()); + return nullptr; + } + + if (configuration.compatible_chargers().size() != 1) + { + RCLCPP_ERROR( + 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_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) + { + 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; + } + 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())); + + const auto position_opt = robot_nav_params->to_rmf_coordinates( + initial_state.map(), initial_state.position()); + if (!position_opt.has_value()) + { + RCLCPP_ERROR( + node->get_logger(), + "[EasyFullControl] Unable to find a robot transform for map [%s] while " + "adding robot [%s]. We cannot initialize this robot.", + initial_state.map().c_str(), + robot_name.c_str()); + + _pimpl->cmd_handles.erase(robot_name); + return nullptr; + } + + if (configuration.max_merge_waypoint_distance().has_value()) + { + robot_nav_params->max_merge_waypoint_distance = + *configuration.max_merge_waypoint_distance(); + } + if (configuration.max_merge_lane_distance().has_value()) + { + robot_nav_params->max_merge_lane_distance = + *configuration.max_merge_lane_distance(); + } + + if (configuration.min_lane_length().has_value()) + { + robot_nav_params->min_lane_length = *configuration.min_lane_length(); + } + + const Eigen::Vector3d position = *position_opt; + auto starts = robot_nav_params->compute_plan_starts( + graph, initial_state.map(), position, now); + + if (starts.empty()) + { + const auto& p = position; + RCLCPP_ERROR( + node->get_logger(), + "Unable to compute a location on the navigation graph for robot [%s] " + "being added to fleet [%s] using map [%s] and position [%.3f, %.3f, %.3f] " + "specified in the initial_state argument. This can happen if the map in " + "initial_state does not match any of the map names in the navigation " + "graph supplied or if the position reported in the initial_state is far " + "way from the navigation graph. This robot will not be added to the " + "fleet.", + robot_name.c_str(), + fleet_name.c_str(), + initial_state.map().c_str(), + p[0], p[1], p[2]); + + _pimpl->cmd_handles.erase(robot_name); + return nullptr; + } + + const auto handle_nav_request = callbacks.navigate(); + const auto handle_stop = callbacks.stop(); + if (handle_nav_request == nullptr || handle_stop == nullptr) + { + RCLCPP_ERROR( + node->get_logger(), + "One or more required callbacks given to [EasyFullControl::add_robot] " + "were null. The robot [%s] will not be added to fleet [%s].", + robot_name.c_str(), + _pimpl->fleet_handle->fleet_name().c_str()); + + _pimpl->cmd_handles.erase(robot_name); + return nullptr; + } + + const auto cmd_handle = std::make_shared( + robot_nav_params, + std::move(handle_nav_request), + std::move(handle_stop)); + insertion.first->second = cmd_handle; + + bool enable_responsive_wait = _pimpl->default_responsive_wait; + if (configuration.responsive_wait().has_value()) + { + enable_responsive_wait = *configuration.responsive_wait(); + } + + _pimpl->fleet_handle->add_robot( + insertion.first->second, + robot_name, + traits.profile(), + starts, + [ + cmd_handle, + easy_updater, + node, + robot_name = robot_name, + fleet_name = fleet_name, + charger_index, + action_executor = callbacks.action_executor(), + nav_params = robot_nav_params, + enable_responsive_wait + ](const RobotUpdateHandlePtr& updater) + { + auto context = RobotUpdateHandle::Implementation::get(*updater) + .get_context(); + + context->worker().schedule( + [ + cmd_handle, + easy_updater, + node, + handle = updater, + robot_name, + fleet_name, + charger_index, + action_executor, + context, + nav_params, + enable_responsive_wait + ](const auto&) + { + cmd_handle->w_context = context; + context->set_nav_params(nav_params); + EasyRobotUpdateHandle::Implementation::get(*easy_updater) + .updater->handle = handle; + handle->set_action_executor(action_executor); + handle->set_charger_waypoint(charger_index); + handle->enable_responsive_wait(enable_responsive_wait); + + RCLCPP_INFO( + node->get_logger(), + "Successfully added robot [%s] to the fleet [%s].", + robot_name.c_str(), + fleet_name.c_str()); + }); + }); + + return easy_updater; +} + +} // namespace agv +} // namespace rmf_fleet_adapter 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 eb40d6e2c..61365bf03 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -795,8 +795,24 @@ std::optional convert_location( { if (context.location().empty()) { - // TODO(MXG): We should emit some kind of critical error if this ever - // happens + const auto& lost = context.lost(); + if (lost.has_value() && lost->location.has_value()) + { + const auto& l = *lost->location; + return rmf_fleet_msgs::build() + .t(rmf_traffic_ros2::convert(l.time)) + .x(l.position[0]) + .y(l.position[1]) + .yaw(l.position[2]) + .obey_approach_speed_limit(false) + .approach_speed_limit(0.0) + .level_name(l.map) + .index(0); + } + + // TODO(MXG): We should emit some kind of critical error if there is no + // location and also no lost information, because that means an issue ticket + // has not been created. return std::nullopt; } @@ -900,15 +916,15 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const context->now().time_since_epoch()).count(); json["battery"] = context->current_battery_soc(); - nlohmann::json& location = json["location"]; const auto location_msg = convert_location(*context); - if (!location_msg.has_value()) - continue; - - location["map"] = location_msg->level_name; - location["x"] = location_msg->x; - location["y"] = location_msg->y; - location["yaw"] = location_msg->yaw; + if (location_msg.has_value()) + { + nlohmann::json& location = json["location"]; + location["map"] = location_msg->level_name; + location["x"] = location_msg->x; + location["y"] = location_msg->y; + location["yaw"] = location_msg->yaw; + } std::lock_guard lock(context->reporting().mutex()); const auto& issues = context->reporting().open_issues(); @@ -1281,6 +1297,12 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( return assignments; } +//============================================================================== +const std::string& FleetUpdateHandle::fleet_name() const +{ + return _pimpl->name; +} + //============================================================================== void FleetUpdateHandle::add_robot( std::shared_ptr command, @@ -1377,7 +1399,7 @@ void FleetUpdateHandle::add_robot( using namespace std::chrono_literals; auto last_interrupt_time = std::make_shared>(std::nullopt); - context->_negotiation_license = + auto negotiation_license = fleet->_pimpl->negotiation ->register_negotiator( context->itinerary().id(), @@ -1412,9 +1434,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(); } }); + context->_set_negotiation_license(std::move(negotiation_license)); } RCLCPP_INFO( @@ -1585,6 +1612,12 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) self->_pimpl->task_parameters->planner(*self->_pimpl->planner); self->_pimpl->publish_lane_states(); + + RobotContext::GraphChange changes{lane_indices}; + for (auto& [ctx, _] : self->_pimpl->task_managers) + { + ctx->notify_graph_change(changes); + } }); } @@ -1690,7 +1723,7 @@ auto FleetUpdateHandle::limit_lane_speeds( { RCLCPP_WARN( self->_pimpl->node->get_logger(), - "Ignoring speed limit request %f for lane %d in fleet %s as it is " + "Ignoring speed limit request %f for lane %lu in fleet %s as it is " "not greater than zero. If you would like to close the lane, use " "the FleetUpdateHandle::close_lanes(~) API instead.", request.speed_limit(), @@ -2035,10 +2068,10 @@ std::shared_ptr FleetUpdateHandle::node() const //============================================================================== bool FleetUpdateHandle::set_task_planner_params( - std::shared_ptr battery_system, - std::shared_ptr motion_sink, - std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, + rmf_battery::agv::ConstBatterySystemPtr battery_system, + rmf_battery::ConstMotionPowerSinkPtr motion_sink, + rmf_battery::ConstDevicePowerSinkPtr ambient_sink, + rmf_battery::ConstDevicePowerSinkPtr tool_sink, double recharge_threshold, double recharge_soc, bool account_for_battery_drain, 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 47f6ca384..efc92a5d2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -75,11 +75,104 @@ std::function RobotContext::clock() const } //============================================================================== -const std::vector& RobotContext::location() const +const rmf_traffic::agv::Plan::StartSet& RobotContext::location() const { return _location; } +//============================================================================== +void RobotContext::set_location(rmf_traffic::agv::Plan::StartSet location_) +{ + _location = std::move(location_); + filter_closed_lanes(); + if (_location.empty()) + { + set_lost(std::nullopt); + return; + } + else if (_lost) + { + nlohmann::json resolve; + resolve["robot"] = name(); + resolve["group"] = group(); + resolve["msg"] = "The robot [" + requester_id() + "] has found a " + "connection to the navigation graph."; + _lost->ticket->resolve(resolve); + _lost = std::nullopt; + // If the robot has switched from lost to found, we should go ahead and + // replan. + RCLCPP_INFO( + node()->get_logger(), + "Requesting a replan for [%s] because it has been found after being lost", + requester_id().c_str()); + request_replan(); + } + + _most_recent_valid_location = _location; +} + +//============================================================================== +const std::optional& RobotContext::lost() const +{ + return _lost; +} + +//============================================================================== +void RobotContext::set_lost(std::optional location) +{ + _location.clear(); + if (!_lost.has_value()) + { + nlohmann::json detail; + detail["robot"] = name(); + detail["group"] = group(); + detail["msg"] = "The robot [" + requester_id() + "] is too far from the " + "navigation graph and may need an operator to help bring it back."; + + auto ticket = _reporting.create_issue( + rmf_task::Log::Tier::Error, "lost", detail); + _lost = Lost { location, std::move(ticket) }; + } + else + { + _lost->location = location; + } +} + +//============================================================================== +void RobotContext::filter_closed_lanes() +{ + if (const auto planner = *_planner) + { + 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 (_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; + } + } + } + ++i; + } + } +} + //============================================================================== rmf_traffic::schedule::Participant& RobotContext::itinerary() { @@ -142,6 +235,18 @@ RobotContext::planner() const return *_planner; } +//============================================================================== +std::shared_ptr RobotContext::nav_params() const +{ + return _nav_params; +} + +//============================================================================== +void RobotContext::set_nav_params(std::shared_ptr value) +{ + _nav_params = std::move(value); +} + //============================================================================== class RobotContext::NegotiatorLicense { @@ -207,6 +312,20 @@ void RobotContext::request_replan() _replan_publisher.get_subscriber().on_next(Empty{}); } +//============================================================================== +const rxcpp::observable& +RobotContext::observe_graph_change() const +{ + return _graph_change_obs; +} + +//============================================================================== +void RobotContext::notify_graph_change(GraphChange changes) +{ + filter_closed_lanes(); + _graph_change_publisher.get_subscriber().on_next(std::move(changes)); +} + //============================================================================== const std::shared_ptr& RobotContext::node() { @@ -270,9 +389,17 @@ std::function RobotContext::make_get_state() { return [self = shared_from_this()]() { + if (self->_most_recent_valid_location.empty()) + { + throw std::runtime_error( + "Missing a _most_recent_valid_location for robot [" + + self->requester_id() + "]. This is an internal RMF error, " + "please report it to the RMF developers."); + } + rmf_task::State state; state.load_basic( - self->_location.front(), + self->_most_recent_valid_location.front(), self->_charger_wp, self->_current_battery_soc); @@ -306,6 +433,16 @@ double RobotContext::current_battery_soc() const //============================================================================== RobotContext& RobotContext::current_battery_soc(const double battery_soc) { + if (battery_soc < 0.0 || battery_soc > 1.0) + { + RCLCPP_ERROR( + _node->get_logger(), + "Invalid battery state of charge given for [%s]: %0.3f", + requester_id().c_str(), + battery_soc); + return *this; + } + _current_battery_soc = battery_soc; _battery_soc_publisher.get_subscriber().on_next(battery_soc); @@ -496,10 +633,12 @@ RobotContext::RobotContext( _task_planner(std::move(task_planner)), _reporting(_worker) { + _most_recent_valid_location = _location; _profile = std::make_shared( _itinerary.description().profile()); _replan_obs = _replan_publisher.get_observable(); + _graph_change_obs = _graph_change_publisher.get_observable(); _battery_soc_obs = _battery_soc_publisher.get_observable(); @@ -515,5 +654,11 @@ void RobotContext::_set_task_manager(std::shared_ptr mgr) _task_manager = std::move(mgr); } +//============================================================================== +void RobotContext::_set_negotiation_license(std::shared_ptr license) +{ + _negotiation_license = std::move(license); +} + } // 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 02c74fd44..bafd331de 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,83 @@ class TaskManager; namespace agv { +using TransformDictionary = std::unordered_map; + +//============================================================================== +struct NavParams +{ + bool skip_rotation_commands; + std::shared_ptr transforms_to_robot_coords; + double max_merge_waypoint_distance = 1e-3; + double max_merge_lane_distance = 0.3; + double min_lane_length = 1e-8; + /// We iterate over these multipliers, applying them to the + /// max_merge_lane_distance until at least one plan start is found. + std::vector multipliers = {1.0, 2.0, 3.0, 5.0, 10.0}; + + std::optional to_rmf_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_inverse(position); + } + + rmf_traffic::agv::Plan::StartSet compute_plan_starts( + const rmf_traffic::agv::Graph& graph, + const std::string& map_name, + const Eigen::Vector3d position, + const rmf_traffic::Time start_time) const + { + for (const double m : multipliers) + { + auto starts = rmf_traffic::agv::compute_plan_starts( + graph, + map_name, + position, + start_time, + max_merge_waypoint_distance, + m * max_merge_lane_distance, + min_lane_length); + + if (!starts.empty()) + return starts; + } + + return {}; + } +}; + +//============================================================================== +struct Location +{ + rmf_traffic::Time time; + std::string map; + Eigen::Vector3d position; +}; + +/// Store position information when the robot is lost, i.e. it has diverged too +/// far from its navigation graph. +struct Lost +{ + /// We may have localization information for the robot, even if it's too far + /// from the navigation graph. + std::optional location; + + /// An issue ticket to track when the robot is lost + std::unique_ptr ticket; +}; + //============================================================================== class RobotContext : public std::enable_shared_from_this, @@ -77,7 +155,22 @@ class RobotContext /// This is the current "location" of the robot, which can be used to initiate /// a planning job - const std::vector& location() const; + const rmf_traffic::agv::Plan::StartSet& location() const; + + /// Set the current location for the robot in terms of a planner start set + void set_location(rmf_traffic::agv::Plan::StartSet location_); + + /// If the robot is lost, this will let you view its localization and issue + /// ticket + const std::optional& lost() const; + + /// Set that the robot is currently lost + void set_lost(std::optional location); + + /// Filter closed lanes out of the planner start set. At least one start will + /// be retained so that the planner can offer some solution, even if all + /// plan starts are using closed lanes. + void filter_closed_lanes(); /// Get a mutable reference to the schedule of this robot rmf_traffic::schedule::Participant& itinerary(); @@ -111,6 +204,14 @@ class RobotContext /// Get a mutable reference to the planner for this robot const std::shared_ptr& 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; + + /// Set the navigation params for this robot. This is used by EasyFullControl + /// robots. + void set_nav_params(std::shared_ptr value); + class NegotiatorLicense; /// Set the schedule negotiator that will take responsibility for this robot. @@ -129,8 +230,18 @@ class RobotContext struct Empty {}; const rxcpp::observable& observe_replan_request() const; + /// Request this robot to replan void request_replan(); + struct GraphChange + { + std::vector closed_lanes; + }; + const rxcpp::observable& observe_graph_change() const; + + /// Notify this robot that the graph has changed. + void notify_graph_change(GraphChange changes); + /// Get a reference to the rclcpp node const std::shared_ptr& node(); @@ -241,10 +352,13 @@ class RobotContext const Reporting& reporting() const; -private: - friend class FleetUpdateHandle; - friend class RobotUpdateHandle; - friend class rmf_fleet_adapter::TaskManager; + /// 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); + + /// Set the negotiation license for this robot. This should only be called in + /// the FleetUpdateHandle::add_robot function. + void _set_negotiation_license(std::shared_ptr license); RobotContext( std::shared_ptr command_handle, @@ -260,15 +374,15 @@ class RobotContext rmf_task::State state, std::shared_ptr task_planner); - /// 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); +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; + std::shared_ptr _nav_params; rmf_task::ConstActivatorPtr _task_activator; rmf_task::ConstParametersPtr _task_parameters; std::shared_ptr _profile; @@ -279,6 +393,9 @@ class RobotContext rxcpp::subjects::subject _replan_publisher; rxcpp::observable _replan_obs; + rxcpp::subjects::subject _graph_change_publisher; + rxcpp::observable _graph_change_obs; + std::shared_ptr _node; rxcpp::schedulers::worker _worker; rmf_utils::optional _maximum_delay; @@ -306,6 +423,8 @@ class RobotContext RobotUpdateHandle::ActionExecutor _action_executor; Reporting _reporting; + /// Keep track of a lost robot + std::optional _lost; }; 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 4ad01f33c..1a617ebe9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -22,6 +22,8 @@ #include +#include + #include namespace rmf_fleet_adapter { @@ -75,11 +77,11 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, waypoint, orientation](const auto&) { - context->_location = { + context->set_location({ rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, orientation) - }; + }); }); } } @@ -115,7 +117,7 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(starts)](const auto&) { - context->_location = std::move(starts); + context->set_location(std::move(starts)); }); } } @@ -130,11 +132,11 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, position, waypoint](const auto&) { - context->_location = { + context->set_location({ rmf_traffic::agv::Plan::Start( rmf_traffic_ros2::convert(context->node()->now()), waypoint, position[2], Eigen::Vector2d(position.block<2, 1>(0, 0))) - }; + }); }); } } @@ -163,13 +165,19 @@ void RobotUpdateHandle::update_position( "from its navigation graph, currently located at <%f, %f, %f> on " "map [%s]", context->requester_id().c_str(), position[0], position[1], position[2], map_name.c_str()); + + context->worker().schedule( + [context, now, map_name, position](const auto&) + { + context->set_lost(Location { now, map_name, position }); + }); return; } context->worker().schedule( [context, starts = std::move(starts)](const auto&) { - context->_location = std::move(starts); + context->set_location(std::move(starts)); }); } } @@ -183,7 +191,7 @@ void RobotUpdateHandle::update_position( context->worker().schedule( [context, starts = std::move(position)](const auto&) { - context->_location = starts; + context->set_location(starts); }); } } @@ -210,9 +218,6 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( //============================================================================== void RobotUpdateHandle::update_battery_soc(const double battery_soc) { - if (battery_soc < 0.0 || battery_soc > 1.0) - return; - if (const auto context = _pimpl->get_context()) { context->worker().schedule( @@ -226,10 +231,8 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc) //============================================================================== void RobotUpdateHandle::override_status(std::optional status) { - if (const auto context = _pimpl->get_context()) { - if (status.has_value()) { // Here we capture [this] to avoid potential costly copy of @@ -305,6 +308,19 @@ RobotUpdateHandle& RobotUpdateHandle::maximum_delay( return *this; } +//============================================================================== +bool RobotUpdateHandle::ActivityIdentifier::operator==( + const ActivityIdentifier& other) const +{ + return _pimpl == other._pimpl; +} + +//============================================================================== +RobotUpdateHandle::ActivityIdentifier::ActivityIdentifier() +{ + // Do nothing +} + //============================================================================== rmf_utils::optional RobotUpdateHandle::maximum_delay() const @@ -761,22 +777,6 @@ rmf_traffic::PlanId RobotUpdateHandle::Unstable::current_plan_id() const return _pimpl->get_context()->itinerary().current_plan_id(); } -//============================================================================== -class RobotUpdateHandle::Unstable::Stubbornness::Implementation -{ -public: - std::shared_ptr stubbornness; - - static Stubbornness make(std::shared_ptr stubbornness) - { - Stubbornness output; - output._pimpl = rmf_utils::make_impl( - Implementation{stubbornness}); - - return output; - } -}; - //============================================================================== void RobotUpdateHandle::Unstable::Stubbornness::release() { @@ -857,19 +857,57 @@ void RobotUpdateHandle::ActionExecution::blocked( } //============================================================================== -void RobotUpdateHandle::ActionExecution::finished() +auto RobotUpdateHandle::ActionExecution::override_schedule( + std::string map, + std::vector path, + rmf_traffic::Duration hold) -> Stubbornness { - if (_pimpl->data) + auto stubborn = std::make_shared(); + if (const auto context = _pimpl->data->w_context.lock()) { - _pimpl->data->worker.schedule( - [data = _pimpl->data](const rxcpp::schedulers::schedulable&) + context->worker().schedule( + [ + context, + stubborn, + data = _pimpl->data, + identifier = _pimpl->identifier, + map = std::move(map), + path = std::move(path), + hold + ](const auto&) { - if (data->finished) + if (!ActivityIdentifier::Implementation::get(*identifier).update_fn) { - data->finished(); - data->finished = nullptr; + // Don't do anything because this command is finished + return; } - }); + + if (data->schedule_override.has_value()) + { + data->schedule_override->release_stubbornness(); + } + data->schedule_override = ScheduleOverride::make( + context, map, path, hold, stubborn); + } + ); + } + + return Stubbornness::Implementation::make(stubborn); +} + +//============================================================================== +void RobotUpdateHandle::ActionExecution::finished() +{ + if (_pimpl->data) + { + if (const auto context = _pimpl->data->w_context.lock()) + { + context->worker().schedule( + [finished = _pimpl->data->finished](const auto&) + { + finished.trigger(); + }); + } } } @@ -879,12 +917,260 @@ bool RobotUpdateHandle::ActionExecution::okay() const return _pimpl->data->okay; } +//============================================================================== +auto RobotUpdateHandle::ActionExecution::identifier() const +-> ConstActivityIdentifierPtr +{ + return _pimpl->identifier; +} + //============================================================================== RobotUpdateHandle::ActionExecution::ActionExecution() { // Do nothing } +//============================================================================== +void ScheduleOverride::overridden_update( + const std::shared_ptr& context, + const std::string& map, + Eigen::Vector3d location) +{ + const auto nav_params = context->nav_params(); + if (!nav_params) + return; + + auto p = Eigen::Vector2d(location[0], location[1]); + std::optional> closest_lane; + std::size_t i0 = 0; + std::size_t i1 = 1; + const double proj_lower_bound = -nav_params->max_merge_lane_distance; + for (; i1 < route.trajectory().size(); ++i0, ++i1) + { + // We approximate the trajectory as linear with constant velocity even + // though it could technically be a cubic spline. The linear + // approximation simplifies the math considerably, and we will be + // phasing out support for cubic splines in the future. + const Eigen::Vector2d p0 = + route.trajectory().at(i0).position().block<2, 1>(0, 0); + const Eigen::Vector2d p1 = + route.trajectory().at(i1).position().block<2, 1>(0, 0); + const auto lane_length = (p1 - p0).norm(); + if (lane_length < 1e-6) + { + const double dist_to_lane = (p - p0).norm(); + if (dist_to_lane > nav_params->max_merge_lane_distance) + { + continue; + } + + if (!closest_lane.has_value() || dist_to_lane < closest_lane->second) + { + closest_lane = std::make_pair(i0, dist_to_lane); + } + } + else + { + const auto lane_u = (p1 - p0)/lane_length; + const auto proj = (p - p0).dot(lane_u); + const double proj_upper_bound = + lane_length + nav_params->max_merge_lane_distance; + if (proj < proj_lower_bound || proj_upper_bound < proj) + { + continue; + } + + double dist_to_lane = (p - p0 - proj * lane_u).norm(); + if (proj < 0.0) + dist_to_lane += std::abs(proj); + else if (lane_length < proj) + dist_to_lane += std::abs(proj - lane_length); + + if (!closest_lane.has_value() || dist_to_lane < closest_lane->second) + { + closest_lane = std::make_pair(i0, dist_to_lane); + } + } + } + + const auto now = rmf_traffic_ros2::convert(context->node()->now()); + const auto delay_thresh = std::chrono::milliseconds(100); + if (closest_lane.has_value()) + { + const auto& wp0 = route.trajectory().at(closest_lane->first); + const auto& wp1 = route.trajectory().at(closest_lane->first + 1); + const Eigen::Vector2d p0 = wp0.position().block<2, 1>(0, 0); + const Eigen::Vector2d p1 = wp1.position().block<2, 1>(0, 0); + rmf_traffic::Time t_expected = wp0.time(); + const auto lane_length = (p1 - p0).norm(); + if (lane_length > 1e-6) + { + const auto lane_u = (p1 - p0)/lane_length; + const auto proj = (p - p0).dot(lane_u); + const auto s = proj/lane_length; + const double dt = rmf_traffic::time::to_seconds(wp1.time() - wp0.time()); + t_expected += rmf_traffic::time::from_seconds(s*dt); + } + else + { + const double total_delta_yaw = + rmf_utils::wrap_to_pi(wp1.position()[2] - wp0.position()[2]); + const double remaining_delta_yaw = + rmf_utils::wrap_to_pi(wp1.position()[2] - location[2]); + const double s = remaining_delta_yaw / total_delta_yaw; + const double dt = rmf_traffic::time::to_seconds(wp1.time() - wp0.time()); + t_expected += rmf_traffic::time::from_seconds(s*dt); + } + const auto delay = now - t_expected; + context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); + } + else + { + // Find the waypoint that the agent is closest to and estimate the delay + // based on the agent being at that waypoint. This is a very fallible + // estimation, but it's the best we can do with limited information. + std::optional> closest_time; + for (std::size_t i = 0; i < route.trajectory().size(); ++i) + { + const auto& wp = route.trajectory().at(i); + const Eigen::Vector2d p_wp = wp.position().block<2, 1>(0, 0); + const double dist = (p - p_wp).norm(); + if (!closest_time.has_value() || dist < closest_time->second) + { + closest_time = std::make_pair(wp.time(), dist); + } + } + + if (closest_time.has_value()) + { + const auto delay = now - closest_time->first; + context->itinerary().cumulative_delay(plan_id, delay, delay_thresh); + } + + // If no closest time was found then there are no waypoints in the + // route. There's no point updating the delay of an empty route. + } + + const auto& itin = context->itinerary().itinerary(); + for (std::size_t i = 0; i < itin.size(); ++i) + { + const auto& traj = itin[i].trajectory(); + const auto t_it = traj.find(now); + if (t_it != traj.end() && t_it != traj.begin()) + { + uint64_t checkpoint = 0; + if (t_it->time() == now) + { + checkpoint = t_it->index(); + } + else + { + checkpoint = t_it->index() - 1; + } + + if (checkpoint < context->itinerary().reached().at(i)) + { + // The robot has backtracked along its path. + // This can mess up traffic dependency relationships, so we need to + // resend the schedule. + + // WARNING: We will have to change this implementation if it is ever + // possible for the ScheduleOverride class to contain multiple routes in + // its itinerary. + const auto cumulative_delay = context->itinerary() + .cumulative_delay(plan_id).value_or(rmf_traffic::Duration(0)); + + plan_id = context->itinerary().assign_plan_id(); + context->itinerary().set(plan_id, {route}); + context->itinerary().cumulative_delay( + plan_id, cumulative_delay, delay_thresh); + } + + context->itinerary().reached(plan_id, i, checkpoint); + } + } + + 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 + { + context->set_lost(Location { now, map, location }); + } +} + +//============================================================================== +void ScheduleOverride::release_stubbornness() +{ + if (const auto stubborn = w_stubborn.lock()) + { + // Clear out the previous stubborn handle + stubborn->stubbornness = nullptr; + } +} + +//============================================================================== +std::optional ScheduleOverride::make( + const std::shared_ptr& context, + const std::string& map, + const std::vector& path, + rmf_traffic::Duration hold, + std::shared_ptr stubborn) +{ + auto planner = context->planner(); + if (!planner) + { + RCLCPP_WARN( + context->node()->get_logger(), + "Planner unavailable for robot [%s], cannot override its " + "schedule", + context->requester_id().c_str()); + return std::nullopt; + } + + const auto now = context->now(); + const auto& traits = planner->get_configuration().vehicle_traits(); + auto trajectory = rmf_traffic::agv::Interpolate::positions( + traits, now, path); + if (hold > rmf_traffic::Duration(0) && !trajectory.empty()) + { + const auto& last_wp = trajectory.back(); + trajectory.insert( + last_wp.time() + hold, + last_wp.position(), + Eigen::Vector3d(0.0, 0.0, 0.0)); + } + std::set checkpoints; + for (uint64_t i = 1; i < trajectory.size(); ++i) + { + checkpoints.insert(i); + } + auto route = rmf_traffic::Route(map, std::move(trajectory)); + route.checkpoints(checkpoints); + + const auto plan_id = context->itinerary().assign_plan_id(); + context->itinerary().set(plan_id, {route}); + stubborn->stubbornness = context->be_stubborn(); + + return ScheduleOverride{ + std::move(route), + plan_id, + stubborn + }; +} } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp new file mode 100644 index 000000000..80f77221d --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Transformation.cpp @@ -0,0 +1,108 @@ +/* + * 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 + +namespace rmf_fleet_adapter { +namespace agv { + +//============================================================================== +class Transformation::Implementation +{ +public: + + double rotation; + double scale; + Eigen::Vector2d translation; + + Eigen::Affine2d transform; + Eigen::Affine2d transform_inv; + + Implementation( + double rotation_, + double scale_, + Eigen::Vector2d translation_) + : rotation(rotation_), + scale(scale_), + translation(translation_) + { + update_transform(); + } + + void update_transform() + { + transform = Eigen::Affine2d::Identity(); + transform.translate(translation); + transform.rotate(Eigen::Rotation2D(rotation)); + transform.scale(scale); + + transform_inv = transform.inverse(); + } +}; + +//============================================================================== +Transformation::Transformation( + double rotation, + double scale, + Eigen::Vector2d translation) +: _pimpl(rmf_utils::make_impl( + std::move(rotation), + std::move(scale), + std::move(translation))) +{ + // Do nothing +} + +//============================================================================== +double Transformation::rotation() const +{ + return _pimpl->rotation; +} + +//============================================================================== +double Transformation::scale() const +{ + return _pimpl->scale; +} + +//============================================================================== +const Eigen::Vector2d& Transformation::translation() const +{ + return _pimpl->translation; +} + +//============================================================================== +Eigen::Vector3d Transformation::apply( + const Eigen::Vector3d& position) const +{ + Eigen::Vector2d p = _pimpl->transform * 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); +} + +//============================================================================== +Eigen::Vector3d Transformation::apply_inverse( + const Eigen::Vector3d& position) const +{ + Eigen::Vector2d p = _pimpl->transform * 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); +} + +} // namespace agv +} // namespace rmf_fleet_adapter 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 new file mode 100644 index 000000000..6d3c9c896 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -0,0 +1,71 @@ +/* + * 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 "internal_FleetUpdateHandle.hpp" + +namespace rmf_fleet_adapter { +namespace agv { + +class EasyCommandHandle; +using EasyCommandHandlePtr = std::shared_ptr; + +//============================================================================== +class EasyFullControl::Implementation +{ +public: + std::shared_ptr fleet_handle; + // Map robot name to its EasyCommandHandle + std::unordered_map cmd_handles; + NavParams nav_params; + bool default_responsive_wait; + + static std::shared_ptr make( + std::shared_ptr fleet_handle, + bool skip_rotation_commands, + std::shared_ptr transforms_to_robot_coords, + bool default_responsive_wait, + double default_max_merge_waypoint_distance, + double default_max_merge_lane_distance, + double default_min_lane_length) + { + auto handle = std::shared_ptr(new EasyFullControl); + handle->_pimpl = rmf_utils::make_unique_impl( + Implementation{ + fleet_handle, + {}, + NavParams{ + skip_rotation_commands, + std::move(transforms_to_robot_coords), + default_max_merge_waypoint_distance, + default_max_merge_lane_distance, + default_min_lane_length + }, + default_responsive_wait + }); + return handle; + } + + const std::shared_ptr& node() const + { + return FleetUpdateHandle::Implementation::get(*fleet_handle).node; + } +}; + +} // namespace agv +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyTrafficLight.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyTrafficLight.hpp index 124496030..79fec04c1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyTrafficLight.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyTrafficLight.hpp @@ -253,4 +253,4 @@ class EasyTrafficLight::Implementation } // namespace agv } // namespace rmf_fleet_adapter -#endif // SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_EASYTRAFFICLIGHT_HPP +#endif // SRC__RMF_FLEET_ADAPTER__AGV__INTERNAL_EASYTRAFFICLIGHT_HPP \ No newline at end of file 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 56e8e5a7a..17bc2c2b1 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 @@ -539,6 +539,16 @@ class FleetUpdateHandle::Implementation return handle; } + static Implementation& get(FleetUpdateHandle& handle) + { + return *handle._pimpl; + } + + static const Implementation& get(const FleetUpdateHandle& handle) + { + return *handle._pimpl; + } + void publish_nav_graph() const; void dock_summary_cb(const DockSummary::SharedPtr& msg); @@ -572,16 +582,6 @@ class FleetUpdateHandle::Implementation /// invalid if one of the assignments has already begun execution. bool is_valid_assignments(Assignments& assignments) const; - static Implementation& get(FleetUpdateHandle& fleet) - { - return *fleet._pimpl; - } - - static const Implementation& get(const FleetUpdateHandle& fleet) - { - return *fleet._pimpl; - } - void publish_fleet_state_topic() const; void publish_lane_states() const; 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 557de5cdd..e8c8b24f4 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 @@ -27,9 +27,120 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { +class TriggerOnce +{ +public: + TriggerOnce(std::function callback) + : _callback(std::make_shared>>( + std::make_shared>(std::move(callback)))) + { + // Do nothing + } + + TriggerOnce() + { + // Do nothing + } + + void trigger() const + { + if (!_callback) + { + return; + } + + // We don't use a mutex because we assume this will always be triggered + // inside the shared worker. + std::shared_ptr> inner = *_callback; + if (inner) + { + if (*inner) + { + (*inner)(); + } + + *inner = nullptr; + } + } + +private: + std::shared_ptr>> _callback; +}; + +//============================================================================== +struct StubbornOverride +{ + /// We use a convoluted multi-layered reference structure for schedule + /// override stubbornness so that we can release the stubbornness of the + /// schedule override after the command is finished, even if the user + /// forgets to release the override stubbornness. + /// + /// If we don't implement it like this, there's a risk that the agent will + /// remain stubborn after it resumes normal operation, which would cause + /// significant traffic management problems. + std::shared_ptr stubbornness; +}; + +//============================================================================== +struct ScheduleOverride +{ + rmf_traffic::Route route; + rmf_traffic::PlanId plan_id; + std::weak_ptr w_stubborn; + + void overridden_update( + const std::shared_ptr& context, + const std::string& map, + Eigen::Vector3d location); + + void release_stubbornness(); + + static std::optional make( + const std::shared_ptr& context, + const std::string& map, + const std::vector& path, + rmf_traffic::Duration hold, + std::shared_ptr stubborn); +}; + +//============================================================================== +using LocationUpdateFn = std::function; + +//============================================================================== +class RobotUpdateHandle::ActivityIdentifier::Implementation +{ +public: + /// A function that EasyFullControl will use to update the robot location info + /// when this activity is being executed. + LocationUpdateFn update_fn; + + static std::shared_ptr make(LocationUpdateFn update_fn_) + { + auto identifier = + std::shared_ptr(new ActivityIdentifier); + identifier->_pimpl = rmf_utils::make_unique_impl( + Implementation{update_fn_}); + return identifier; + } + + static Implementation& get(ActivityIdentifier& self) + { + return *self._pimpl; + } + + static const Implementation& get(const ActivityIdentifier& self) + { + return *self._pimpl; + } +}; + //============================================================================== class RobotUpdateHandle::ActionExecution::Implementation { @@ -37,43 +148,118 @@ class RobotUpdateHandle::ActionExecution::Implementation struct Data { - rxcpp::schedulers::worker worker; - std::function finished; + std::weak_ptr w_context; + TriggerOnce finished; std::shared_ptr state; std::optional remaining_time; + bool request_replan; bool okay; - // TODO: Consider adding a mutex to lock read/write + std::optional schedule_override; + + void update_location( + const std::string& map, + Eigen::Vector3d location) + { + const auto context = w_context.lock(); + if (!context) + return; + + const auto nav_params = context->nav_params(); + if (nav_params) + { + if (const auto p = nav_params->to_rmf_coordinates(map, location)) + { + location = *p; + } + else + { + RCLCPP_ERROR( + context->node()->get_logger(), + "[EasyFullControl] Unable to find a robot transform for map [%s] " + "while updating the location of robot [%s] performing an activity. " + "We cannot update the robot's location.", + map.c_str(), + context->requester_id().c_str()); + return; + } + } + + if (schedule_override.has_value()) + { + return schedule_override->overridden_update( + context, map, location); + } + + 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 + { + context->set_lost(Location { now, map, location }); + } + } + } Data( - rxcpp::schedulers::worker worker_, + const std::shared_ptr& context_, std::function finished_, std::shared_ptr state_, std::optional remaining_time_ = std::nullopt) + : w_context(context_), + finished(std::move(finished_)), + state(std::move(state_)), + remaining_time(remaining_time_), + request_replan(false), + okay(true) { - worker = std::move(worker_); - finished = std::move(finished_); - state = std::move(state_); - remaining_time = remaining_time_; - okay = true; + // Do nothing } ~Data() { - if (finished) - finished(); + if (const auto context = w_context.lock()) + { + context->worker().schedule( + [finished = std::move(this->finished)](const auto&) + { + finished.trigger(); + }); + } } }; static ActionExecution make(std::shared_ptr data) { + auto update_fn = [data]( + const std::string& map, + Eigen::Vector3d location) + { + data->update_location(map, location); + }; + ActionExecution execution; execution._pimpl = rmf_utils::make_impl( - Implementation{std::move(data)}); + Implementation{ + std::move(data), + ActivityIdentifier::Implementation::make(update_fn) + }); return execution; } std::shared_ptr data; + std::shared_ptr identifier; }; //============================================================================== @@ -127,6 +313,22 @@ class RobotUpdateHandle::Implementation std::shared_ptr get_context() const; }; +//============================================================================== +class RobotUpdateHandle::Unstable::Stubbornness::Implementation +{ +public: + std::shared_ptr stubbornness; + + static Stubbornness make(std::shared_ptr stubbornness) + { + Stubbornness output; + output._pimpl = rmf_utils::make_impl( + Implementation{stubbornness}); + + return output; + } +}; + } // namespace agv } // namespace rmf_fleet_adapter 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 cd97c120d..c3b465641 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -111,6 +111,12 @@ auto GoToPlace::Standby::begin( { if (!_active) { + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning a new go_to_place [%lu] for robot [%s]", + _goal.waypoint(), + _context->requester_id().c_str()); + _active = Active::make( _assign_id, _context, @@ -178,6 +184,69 @@ auto GoToPlace::Active::make( } }); + active->_graph_change_subscription = + active->_context->observe_graph_change() + .observe_on(rxcpp::identity_same_worker(active->_context->worker())) + .subscribe( + [w = active->weak_from_this()]( + const agv::RobotContext::GraphChange& changes) + { + const auto self = w.lock(); + if (!self) + { + return; + } + + if (self->_find_path_service) + { + // If we're currently replanning, we should restart the replanning + // because the upcoming solution might involve a closed lane + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Requesting replan for [%s] to account for a newly closed lane", + self->_context->requester_id().c_str()); + self->_context->request_replan(); + return; + } + + if (self->_execution.has_value()) + { + // TODO(@mxgrey): Consider ignoring waypoints that have already been + // passed. + for (const auto& wp : self->_execution->plan.get_waypoints()) + { + for (const std::size_t lane : wp.approach_lanes()) + { + const auto closed = std::find( + changes.closed_lanes.begin(), + changes.closed_lanes.end(), + lane); + if (closed != changes.closed_lanes.end()) + { + // The current plan is using (or did use) a lane which has closed, + // so let's replan. + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Requesting replan for [%s] to avoid a newly closed lane", + self->_context->requester_id().c_str()); + self->_context->request_replan(); + return; + } + } + } + } + else + { + // Strange that there isn't an execution and also isn't a + // _find_path_service, but let's just request a replan. + RCLCPP_INFO( + self->_context->node()->get_logger(), + "Requesting replan for [%s] to account for a newly closed lane (v2)", + self->_context->requester_id().c_str()); + self->_context->request_replan(); + } + }); + active->_find_plan(); return active; } @@ -252,6 +321,11 @@ auto GoToPlace::Active::interrupt(std::function task_is_interrupted) //============================================================================== void GoToPlace::Active::cancel() { + RCLCPP_INFO( + _context->node()->get_logger(), + "Canceling go_to_place [%lu] for robot [%s]", + _goal.waypoint(), + _context->requester_id().c_str()); _stop_and_clear(); _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); @@ -427,6 +501,12 @@ void GoToPlace::Active::_execute_plan( return; } + RCLCPP_INFO( + _context->node()->get_logger(), + "Executing go_to_place [%lu] for robot [%s]", + _goal.waypoint(), + _context->requester_id().c_str()); + _execution = ExecutePlan::make( _context, plan_id, std::move(plan), std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 199db1e9d..89eb829ee 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -139,6 +139,7 @@ class GoToPlace : public rmf_task_sequence::Event rclcpp::TimerBase::SharedPtr _retry_timer; rmf_rxcpp::subscription_guard _replan_request_subscription; + rmf_rxcpp::subscription_guard _graph_change_subscription; bool _is_interrupted = false; }; 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 c7c1fc0f3..cc3eb5371 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp @@ -260,7 +260,7 @@ void PerformAction::Active::_execute_action() }; auto data = std::make_shared( - _context->worker(), std::move(finished), _state, std::nullopt); + _context, std::move(finished), _state, std::nullopt); _execution_data = data; auto action_execution = diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp index 079e921ed..40f89104f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp @@ -252,6 +252,10 @@ auto ResponsiveWait::Active::interrupt( //============================================================================== void ResponsiveWait::Active::cancel() { + RCLCPP_INFO( + _context->node()->get_logger(), + "Canceling responsive wait for [%s]", + _context->requester_id().c_str()); _state->update_status(Status::Canceled); _state->update_log().info("Received signal to cancel"); _cancelled = true; @@ -297,6 +301,11 @@ void ResponsiveWait::Active::_next_cycle() return; } + RCLCPP_DEBUG( + _context->node()->get_logger(), + "Beginning next responsive wait cycle for [%s] and waypoint %lu", + _context->requester_id().c_str(), + _description.waiting_point); _begin_movement(); } 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 3f1c62680..1be020b61 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/WaitForTraffic.cpp @@ -212,27 +212,7 @@ void WaitForTraffic::Active::_consider_going() bool all_dependencies_reached = true; for (const auto& dep : _dependencies) { - if (dep.deprecated()) - { - const auto other = - _context->schedule()->snapshot() - ->get_participant(dep.dependency().on_participant); - if (!other) - { - _state->update_log().info( - "Replanning because a traffic dependency was dropped from the " - "schedule"); - } - else - { - _state->update_log().info( - "Replanning because [robot:" + other->name() + "] changed its plan"); - } - - return _replan(); - } - - if (!dep.reached()) + if (!dep.reached() && !dep.deprecated()) all_dependencies_reached = false; } 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 82c053f87..64f3fd5f5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -23,6 +23,8 @@ #include +#include + namespace rmf_fleet_adapter { namespace phases { @@ -157,6 +159,11 @@ void MoveRobot::Action::operator()(const Subscriber& s) // 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(); }); @@ -219,18 +226,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) + std::to_string(action->_waypoints.size()-1) + "]"; } - if (path_index > 0) - { - const auto& arrival = - action->_waypoints[path_index - 1].arrival_checkpoints(); - - for (const auto& c : arrival) - { - action->_context->itinerary() - .reached(action->_plan_id, c.route_id, c.checkpoint_id); - } - } - s.on_next(msg); } @@ -239,23 +234,6 @@ void MoveRobot::Action::operator()(const Subscriber& s) return; } - const auto& target_wp = action->_waypoints[path_index]; - const auto& itinerary = action->_context->itinerary().itinerary(); - for (const auto& progress : target_wp.progress_checkpoints()) - { - for (const auto& c : progress.checkpoints) - { - const auto& c_wp = - itinerary[c.route_id].trajectory()[c.checkpoint_id]; - - if (estimate < target_wp.time() - c_wp.time()) - { - action->_context->itinerary() - .reached(action->_plan_id, c.route_id, c.checkpoint_id); - } - } - } - if (action->_plan_id != action->_context->itinerary().current_plan_id()) { // If the current Plan ID of the itinerary does not match the Plan ID @@ -263,6 +241,7 @@ void MoveRobot::Action::operator()(const Subscriber& s) 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(); @@ -272,21 +251,43 @@ void MoveRobot::Action::operator()(const Subscriber& s) [ context = action->_context, plan_id = action->_plan_id, + now, new_cumulative_delay ](const auto&) { context->itinerary().cumulative_delay( - plan_id, new_cumulative_delay, 500ms); + plan_id, new_cumulative_delay, 100ms); + + const auto& itin = context->itinerary().itinerary(); + for (std::size_t i = 0; i < itin.size(); ++i) + { + const auto& traj = itin[i].trajectory(); + const auto t_it = traj.find(now); + if (t_it != traj.end() && t_it != traj.begin()) + { + if (t_it->time() == now) + { + context->itinerary().reached(plan_id, i, t_it->index()); + } + else + { + context->itinerary().reached(plan_id, i, t_it->index() - 1); + } + } + } }); }, [s, w = weak_from_this()]() { 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); + } } LegacyTask::StatusMsg msg; diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index f0aace9f9..ac83f9a68 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -9,8 +9,10 @@ #include "rmf_traffic_ros2/Time.hpp" #include "rmf_fleet_adapter/agv/Adapter.hpp" +#include "rmf_fleet_adapter/agv/EasyFullControl.hpp" #include "rmf_fleet_adapter/agv/test/MockAdapter.hpp" #include "rmf_fleet_adapter_python/PyRobotCommandHandle.hpp" +#include #include #include @@ -22,6 +24,8 @@ #include #include +#include +#include namespace py = pybind11; namespace agv = rmf_fleet_adapter::agv; @@ -34,9 +38,25 @@ using TimePoint = std::chrono::time_point; +std::unordered_map convert( + const std::unordered_map& consideration) +{ + std::unordered_map output; + for (const auto& element : consideration) + { + output[element.first] = [consider = element.second]( + const nlohmann::json& description, Confirmation& confirm) + { + confirm = consider(description); + }; + } +} + +using ActivityIdentifier = agv::RobotUpdateHandle::ActivityIdentifier; using ActionExecution = agv::RobotUpdateHandle::ActionExecution; using RobotInterruption = agv::RobotUpdateHandle::Interruption; using IssueTicket = agv::RobotUpdateHandle::IssueTicket; @@ -73,6 +93,15 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("stop", &agv::RobotCommandHandle::stop) .def("dock", &agv::RobotCommandHandle::dock); + m.def("consider_all", []() -> ModifiedConsiderRequest { + return [](const nlohmann::json&) -> Confirmation + { + Confirmation confirm; + confirm.accept(); + return confirm; + }; + }); + // ROBOTUPDATE HANDLE ====================================================== py::class_>( @@ -255,6 +284,13 @@ PYBIND11_MODULE(rmf_adapter, m) { // ACTION EXECUTOR ======================================================= auto m_robot_update_handle = m.def_submodule("robot_update_handle"); + py::class_>( + m_robot_update_handle, "ActivityIdentifier") + .def("is_same", [](ActivityIdentifier& lhs, ActivityIdentifier& rhs) + { + return lhs == rhs; + }); + py::class_( m_robot_update_handle, "ActionExecution") .def("update_remaining_time", @@ -264,8 +300,25 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("error", &ActionExecution::error, py::arg("text")) .def("delayed", &ActionExecution::delayed, py::arg("text")) .def("blocked", &ActionExecution::blocked, py::arg("text")) + .def( + "override_schedule", + [&]( + ActionExecution& self, + std::string map, + std::vector path, + double hold) + { + return self.override_schedule( + map, + path, + rmf_traffic::time::from_seconds(hold)); + }, + py::arg("map"), + py::arg("path"), + py::arg("hold") = 0.0) .def("finished", &ActionExecution::finished) - .def("okay", &ActionExecution::okay); + .def("okay", &ActionExecution::okay) + .def_property_readonly("identifier", &ActionExecution::identifier); // ROBOT INTERRUPTION ==================================================== py::class_( @@ -298,6 +351,9 @@ PYBIND11_MODULE(rmf_adapter, m) { py::class_>( m, "FleetUpdateHandle") + .def_property_readonly( + "fleet_name", + &agv::FleetUpdateHandle::fleet_name) // NOTE(CH3): Might have to publicise this constructor if required // Otherwise, it's constructable via agv::TestScenario // .def(py::init<>()) // Private constructor! @@ -632,6 +688,8 @@ PYBIND11_MODULE(rmf_adapter, m) { py::arg("node_options") = rclcpp::NodeOptions(), py::arg("wait_time") = rmf_utils::optional( rmf_utils::nullopt)) + .def("add_easy_fleet", &agv::Adapter::add_easy_fleet, + py::arg("configuration")) .def("add_fleet", &agv::Adapter::add_fleet, py::arg("fleet_name"), py::arg("traits"), @@ -681,4 +739,311 @@ PYBIND11_MODULE(rmf_adapter, m) { return TimePoint(rmf_traffic_ros2::convert(self.node()->now()) .time_since_epoch()); }); + + // EASY FULL CONTROL ========================================================= + py::class_>( + m, "EasyFullControl") + .def("add_robot", &agv::EasyFullControl::add_robot) + .def("more", [](agv::EasyFullControl& self) + { + return self.more(); + }); + + auto m_easy_full_control = m.def_submodule("easy_full_control"); + + py::class_< + agv::EasyFullControl::EasyRobotUpdateHandle, + std::shared_ptr + >(m_easy_full_control, "EasyRobotUpdateHandle") + .def("update", &agv::EasyFullControl::EasyRobotUpdateHandle::update) + .def("max_merge_waypoint_distance", &agv::EasyFullControl::EasyRobotUpdateHandle::max_merge_waypoint_distance) + .def("set_max_merge_waypoint_distance", &agv::EasyFullControl::EasyRobotUpdateHandle::set_max_merge_waypoint_distance) + .def("max_merge_lane_distance", &agv::EasyFullControl::EasyRobotUpdateHandle::max_merge_lane_distance) + .def("set_max_merge_lane_distance", &agv::EasyFullControl::EasyRobotUpdateHandle::set_max_merge_lane_distance) + .def("min_lane_length", &agv::EasyFullControl::EasyRobotUpdateHandle::min_lane_length) + .def("set_min_lane_length", &agv::EasyFullControl::EasyRobotUpdateHandle::set_min_lane_length) + .def("more", [](agv::EasyFullControl::EasyRobotUpdateHandle& self) + { + return self.more(); + }); + + py::class_(m_easy_full_control, "RobotState") + .def(py::init< + const std::string&, + Eigen::Vector3d, + double>(), + py::arg("map"), + py::arg("position"), + py::arg("battery_soc")) + .def_property( + "map", + &agv::EasyFullControl::RobotState::map, + &agv::EasyFullControl::RobotState::set_map) + .def_property( + "position", + &agv::EasyFullControl::RobotState::position, + &agv::EasyFullControl::RobotState::set_position) + .def_property( + "battery_state_of_charge", + &agv::EasyFullControl::RobotState::battery_state_of_charge, + &agv::EasyFullControl::RobotState::set_battery_state_of_charge); + + py::class_(m_easy_full_control, "RobotConfiguration") + .def(py::init>(), + py::arg("compatible_chargers")) + .def_property( + "compatible_chargers", + &agv::EasyFullControl::RobotConfiguration::compatible_chargers, + &agv::EasyFullControl::RobotConfiguration::set_compatible_chargers); + + py::class_(m_easy_full_control, "RobotCallbacks") + .def(py::init< + agv::EasyFullControl::NavigationRequest, + agv::EasyFullControl::StopRequest, + agv::EasyFullControl::ActionExecutor>(), + py::arg("navigate"), + py::arg("stop"), + py::arg("action_executor")) + .def_property_readonly( + "navigate", + &agv::EasyFullControl::RobotCallbacks::navigate) + .def_property_readonly( + "stop", + &agv::EasyFullControl::RobotCallbacks::stop) + .def_property_readonly( + "action_executor", + &agv::EasyFullControl::RobotCallbacks::action_executor); + + py::class_(m_easy_full_control, "CommandExecution") + .def("finished", &agv::EasyFullControl::CommandExecution::finished) + .def("okay", &agv::EasyFullControl::CommandExecution::okay) + .def( + "override_schedule", + []( + ActionExecution& self, + std::string map, + std::vector path, + double hold) + { + return self.override_schedule( + std::move(map), + std::move(path), + rmf_traffic::time::from_seconds(hold)); + }, + py::arg("map"), + py::arg("path"), + py::arg("hold") = 0.0) + .def_property_readonly("identifier", &agv::EasyFullControl::CommandExecution::identifier); + + py::class_(m_easy_full_control, "Destination") + .def_property_readonly("map", &agv::EasyFullControl::Destination::map) + .def_property_readonly("position", &agv::EasyFullControl::Destination::position) + .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("dock", &agv::EasyFullControl::Destination::dock) + .def_property_readonly("speed_limit", &agv::EasyFullControl::Destination::speed_limit); + + py::class_(m_easy_full_control, "FleetConfiguration") + .def(py::init([]( // Lambda function to convert reference to shared ptr + std::string& fleet_name, + std::optional> transformations_to_robot_coordinates, + std::unordered_map known_robot_configurations, + rmf_traffic::agv::VehicleTraits& traits, + rmf_traffic::agv::Graph& graph, + battery::BatterySystem& battery_system, + battery::SimpleMotionPowerSink& motion_sink, + battery::SimpleDevicePowerSink& ambient_sink, + battery::SimpleDevicePowerSink& tool_sink, + double recharge_threshold, + double recharge_soc, + bool account_for_battery_drain, + std::unordered_map task_consideration, + std::unordered_map action_consideration, + std::string& finishing_request_string, + bool skip_rotation_commands, + std::optional server_uri, + rmf_traffic::Duration max_delay, + rmf_traffic::Duration update_interval, + bool default_responsive_wait, + double default_max_merge_waypoint_distance, + double default_max_merge_lane_distance, + double default_min_lane_length) + { + rmf_task::ConstRequestFactoryPtr finishing_request; + if (finishing_request_string == "charge") + { + finishing_request = + std::make_shared(); + } + else if (finishing_request_string == "park") + { + finishing_request = + std::make_shared(); + } + else + { + finishing_request = nullptr; + } + return agv::EasyFullControl::FleetConfiguration( + fleet_name, + std::move(transformations_to_robot_coordinates), + std::move(known_robot_configurations), + std::make_shared(traits), + std::make_shared(graph), + std::make_shared(battery_system), + std::make_shared(motion_sink), + std::make_shared(ambient_sink), + std::make_shared(tool_sink), + recharge_threshold, + recharge_soc, + account_for_battery_drain, + convert(task_consideration), + convert(action_consideration), + finishing_request, + skip_rotation_commands, + server_uri, + max_delay, + update_interval, + default_responsive_wait, + default_max_merge_waypoint_distance, + default_max_merge_lane_distance, + default_min_lane_length); + } + ), + py::arg("fleet_name"), + py::arg("transformations_to_robot_coordinates"), + py::arg("known_robot_configurations"), + py::arg("traits"), + py::arg("graph"), + py::arg("battery_system"), + py::arg("motion_sink"), + py::arg("ambient_sink"), + py::arg("tool_sink"), + py::arg("recharge_threshold"), + py::arg("recharge_soc"), + py::arg("account_for_battery_drain"), + py::arg("task_categories"), + py::arg("action_categories"), + py::arg("finishing_request") = "nothing", + py::arg("skip_rotation_commands") = true, + py::arg("server_uri") = std::nullopt, + py::arg("max_delay") = rmf_traffic::time::from_seconds(10.0), + py::arg("update_interval") = rmf_traffic::time::from_seconds(0.5), + py::arg("default_responsive_wait") = false, + py::arg("default_max_merge_waypoint_distance") = 1e-3, + py::arg("default_max_merge_lane_distance") = 0.3, + py::arg("default_min_lane_length") = 1e-8) + .def_static("from_config_files", &agv::EasyFullControl::FleetConfiguration::from_config_files, + py::arg("config_file"), + py::arg("nav_graph_path"), + py::arg("server_uri") = std::nullopt) + .def_property( + "fleet_name", + &agv::EasyFullControl::FleetConfiguration::fleet_name, + &agv::EasyFullControl::FleetConfiguration::set_fleet_name) + .def_property( + "vehicle_traits", + &agv::EasyFullControl::FleetConfiguration::vehicle_traits, + &agv::EasyFullControl::FleetConfiguration::set_vehicle_traits) + .def_property_readonly( + "transformations_to_robot_coordinates", + &agv::EasyFullControl::FleetConfiguration::transformations_to_robot_coordinates) + .def( + "add_robot_coordinates_transformation", + &agv::EasyFullControl::FleetConfiguration::add_robot_coordinate_transformation) + .def_property_readonly( + "known_robot_configurations", + &agv::EasyFullControl::FleetConfiguration::known_robot_configurations) + .def_property_readonly( + "known_robots", + &agv::EasyFullControl::FleetConfiguration::known_robots) + .def( + "add_known_robot_configuration", + &agv::EasyFullControl::FleetConfiguration::add_known_robot_configuration) + .def( + "get_known_robot_configuration", + &agv::EasyFullControl::FleetConfiguration::get_known_robot_configuration) + .def_property( + "graph", + &agv::EasyFullControl::FleetConfiguration::graph, + &agv::EasyFullControl::FleetConfiguration::set_graph) + .def_property( + "battery_system", + &agv::EasyFullControl::FleetConfiguration::battery_system, + &agv::EasyFullControl::FleetConfiguration::set_battery_system) + .def_property( + "motion_sink", + &agv::EasyFullControl::FleetConfiguration::motion_sink, + &agv::EasyFullControl::FleetConfiguration::set_motion_sink) + .def_property( + "ambient_sink", + &agv::EasyFullControl::FleetConfiguration::ambient_sink, + &agv::EasyFullControl::FleetConfiguration::set_ambient_sink) + .def_property( + "tool_sink", + &agv::EasyFullControl::FleetConfiguration::tool_sink, + &agv::EasyFullControl::FleetConfiguration::set_tool_sink) + .def_property( + "recharge_threshold", + &agv::EasyFullControl::FleetConfiguration::recharge_threshold, + &agv::EasyFullControl::FleetConfiguration::set_recharge_threshold) + .def_property( + "recharge_soc", + &agv::EasyFullControl::FleetConfiguration::recharge_soc, + &agv::EasyFullControl::FleetConfiguration::set_recharge_soc) + .def_property( + "account_for_battery_drain", + &agv::EasyFullControl::FleetConfiguration::account_for_battery_drain, + &agv::EasyFullControl::FleetConfiguration::set_account_for_battery_drain) + .def_property( + "finishing_request", + &agv::EasyFullControl::FleetConfiguration::finishing_request, + &agv::EasyFullControl::FleetConfiguration::set_finishing_request) + .def_property( + "skip_rotation_commands", + &agv::EasyFullControl::FleetConfiguration::skip_rotation_commands, + &agv::EasyFullControl::FleetConfiguration::set_skip_rotation_commands) + .def_property( + "server_uri", + &agv::EasyFullControl::FleetConfiguration::server_uri, + &agv::EasyFullControl::FleetConfiguration::set_server_uri) + .def_property( + "max_delay", + &agv::EasyFullControl::FleetConfiguration::max_delay, + &agv::EasyFullControl::FleetConfiguration::set_max_delay) + .def_property( + "update_interval", + &agv::EasyFullControl::FleetConfiguration::update_interval, + &agv::EasyFullControl::FleetConfiguration::set_update_interval) + .def_property( + "default_responsive_wait", + &agv::EasyFullControl::FleetConfiguration::default_responsive_wait, + &agv::EasyFullControl::FleetConfiguration::set_default_responsive_wait) + .def_property( + "default_max_merge_waypoint_distance", + &agv::EasyFullControl::FleetConfiguration::default_max_merge_waypoint_distance, + &agv::EasyFullControl::FleetConfiguration::set_default_max_merge_waypoint_distance) + .def_property( + "default_max_merge_lane_distance", + &agv::EasyFullControl::FleetConfiguration::default_max_merge_lane_distance, + &agv::EasyFullControl::FleetConfiguration::set_default_max_merge_lane_distance) + .def_property( + "default_min_lane_length", + &agv::EasyFullControl::FleetConfiguration::default_min_lane_length, + &agv::EasyFullControl::FleetConfiguration::set_default_min_lane_length); + + // Transformation ============================================================= + py::class_(m, "Transformation") + .def(py::init(), + py::arg("rotation"), + py::arg("scale"), + py::arg("translation")) + .def_property_readonly("rotation", &agv::Transformation::rotation) + .def_property_readonly("scale", &agv::Transformation::scale) + .def_property_readonly("translation", &agv::Transformation::translation) + .def("apply", &agv::Transformation::apply) + .def("apply_inverse", &agv::Transformation::apply_inverse); } diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Negotiation.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Negotiation.cpp index 80fe47c26..62f93073b 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Negotiation.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/Negotiation.cpp @@ -934,14 +934,13 @@ class Negotiation::Implementation const Version conflict_version, const Negotiation::Table& table) { - Proposal msg; - msg.conflict_version = conflict_version; - msg.proposal_version = table.version(); - - assert(table.submission()); - msg.itinerary = convert(*table.submission()); - msg.for_participant = table.participant(); - msg.to_accommodate = convert(table.sequence()); + Proposal msg = rmf_traffic_msgs::build() + .conflict_version(conflict_version) + .proposal_version(table.version()) + .for_participant(table.participant()) + .to_accommodate(convert(table.sequence())) + .plan_id(table.proposal().back().plan) + .itinerary(convert(*table.submission())); // Make sure to pop the back off of msg.to_accommodate, because we don't // want to include this table's final sequence key. That final key is