diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index ad93b290b..7753acdfa 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -33,9 +33,16 @@ set(dep_pkgs rmf_task_msgs rmf_traffic rmf_traffic_ros2 + rmf_task_ros2 rmf_battery rmf_task + rmf_task_sequence std_msgs + rmf_api_msgs + websocketpp + nlohmann_json + nlohmann_json_schema_validator_vendor + SQLite3 ) foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) @@ -70,21 +77,29 @@ target_link_libraries(rmf_fleet_adapter rmf_traffic_ros2::rmf_traffic_ros2 rmf_battery::rmf_battery rmf_task::rmf_task + rmf_task_sequence::rmf_task_sequence + rmf_task_ros2::rmf_task_ros2 yaml-cpp ${rmf_fleet_msgs_LIBRARIES} ${rclcpp_LIBRARIES} ${rmf_task_msgs_LIBRARIES} PRIVATE rmf_rxcpp + nlohmann_json::nlohmann_json + rmf_api_msgs::rmf_api_msgs ${rmf_door_msgs_LIBRARIES} ${rmf_lift_msgs_LIBRARIES} ${rmf_dispenser_msgs_LIBRARIES} ${rmf_ingestor_msgs_LIBRARIES} + ${websocketpp_LIBRARIES} + nlohmann_json_schema_validator + ${SQLite3_LIBRARIES} ) target_include_directories(rmf_fleet_adapter PUBLIC $ + $ # for auto-generated schema headers $ ${rmf_traffic_ros2_INCLUDE_DIRS} ${rmf_fleet_msgs_INCLUDE_DIRS} @@ -96,6 +111,10 @@ target_include_directories(rmf_fleet_adapter ${rmf_lift_msgs_INCLUDE_DIRS} ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} + ${rmf_api_msgs_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} + ${nlohmann_json_schema_validator_INCLUDE_DIRS} + ${SQLite3_INCLUDE_DIRS} ) if (BUILD_TESTING) @@ -130,7 +149,10 @@ if (BUILD_TESTING) ${rmf_lift_msgs_INCLUDE_DIRS} ${rmf_dispenser_msgs_INCLUDE_DIRS} ${rmf_ingestor_msgs_INCLUDE_DIRS} + ${rmf_api_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} + ${WEBSOCKETPP_INCLUDE_DIR} + ${nlohmann_json_schema_validator_INCLUDE_DIRS} ) target_link_libraries(test_rmf_fleet_adapter PRIVATE @@ -143,7 +165,10 @@ if (BUILD_TESTING) ${rmf_ingestor_msgs_LIBRARIES} rmf_fleet_adapter rmf_utils::rmf_utils + rmf_api_msgs::rmf_api_msgs ${std_msgs_LIBRARIES} + ${websocketpp_LIBRARIES} + nlohmann_json_schema_validator ) target_compile_definitions(test_rmf_fleet_adapter @@ -453,17 +478,16 @@ target_include_directories(close_lanes # ----------------------------------------------------------------------------- -ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET) -ament_export_dependencies( - rmf_task - rmf_traffic_ros2 - rmf_fleet_msgs - rmf_door_msgs - rmf_lift_msgs - rmf_ingestor_msgs - rmf_dispenser_msgs +rmf_api_generate_schema_headers( + PACKAGE rmf_fleet_adapter + SCHEMAS_DIR ${CMAKE_CURRENT_LIST_DIR}/schemas ) +# ----------------------------------------------------------------------------- + +ament_export_targets(rmf_fleet_adapter HAS_LIBRARY_TARGET) +ament_export_dependencies(${dep_pkgs}) + install( TARGETS rmf_fleet_adapter @@ -499,4 +523,3 @@ install( ) ament_package() - diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index bc76055e8..ff916826f 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -60,6 +60,9 @@ const std::string DockSummaryTopicName = "dock_summary"; const std::string LaneClosureRequestTopicName = "lane_closure_requests"; const std::string ClosedLaneTopicName = "closed_lanes"; +const std::string TaskApiRequests = "task_api_requests"; +const std::string TaskApiResponses = "task_api_responses"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_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 594259f8c..fa813fe40 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp @@ -93,10 +93,27 @@ class Adapter : public std::enable_shared_from_this /// /// \param[in] navigation_graph /// Specify the navigation graph used by the vehicles in this fleet. + /// + /// \param[in] server_uri + /// Specify the URI for the websocket server that receives updates on tasks + /// and states. If nullopt, data will not be published. + /// + /// \param[in] backup_file_path + /// Loads and logs backup data to the specified file. If the file already, + /// exists the fleet handle will be restored to the last backup state. If + /// the file does not exist, a new one will be created. + /// If nullopt, the fleet adapter will not perform any backup/restore. + /// + /// TODO(YV): Add this param + /// \param[in] restore_from_backup + /// If true, fleet adapter will be restored to last known backup state. + /// [Not implemented yet] std::shared_ptr add_fleet( const std::string& fleet_name, rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph); + rmf_traffic::agv::Graph navigation_graph, + std::optional server_uri = std::nullopt, + std::optional backup_file_path = std::nullopt); /// Create a traffic light to help manage robots that can only support pause /// and resume commands. 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 97401637c..8f2716194 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -30,6 +30,11 @@ #include +#include +#include +#include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -70,6 +75,131 @@ class FleetUpdateHandle : public std::enable_shared_from_this rmf_traffic::agv::Plan::StartSet start, std::function handle)> handle_cb); + /// Confirmation is a class used by the task acceptance callbacks to decide if + /// a task description should be accepted. + class Confirmation + { + public: + + /// Constructor + Confirmation(); + + /// Call this function to decide that you want to accept the task request. + /// If this function is never called, it will be assumed that the task is + /// rejected. + Confirmation& accept(); + + /// Check whether + bool is_accepted() const; + + /// Call this function to bring attention to errors related to the task + /// request. Each call to this function will overwrite any past calls, so + /// it is recommended to only call it once. + Confirmation& errors(std::vector error_messages); + + /// Call this function to add errors instead of overwriting the ones that + /// were already there. + Confirmation& add_errors(std::vector error_messages); + + /// Check the errors that have been given to this confirmation. + const std::vector& errors() const; + + class Implementation; + private: + rmf_utils::impl_ptr _pimpl; + }; + + /// Signature for a callback that decides whether to accept a specific + /// category of task request. + /// + /// \param[in] description + /// A description of the task that is being considered + /// + /// \param[in] confirm + /// Use this object to decide if you want to accept the task + using ConsiderRequest = + std::function; + + /// Allow this fleet adapter to consider delivery requests. + /// + /// Pass in nullptrs to disable delivery requests. + /// + /// By default, delivery requests are not accepted until you provide these + /// callbacks. + /// + /// The FleetUpdateHandle will ensure that the requests are feasible for the + /// robots before triggering these callbacks. + /// + /// \param[in] consider_pickup + /// Decide whether to accept a pickup request. The description will satisfy + /// the event_description_PickUp.json schema of rmf_fleet_adapter. + /// + /// \param[in] consider_dropoff + /// Decide whether to accept a dropoff request. The description will satisfy + /// the event_description_DropOff.json schema of rmf_fleet_adapter. + FleetUpdateHandle& consider_delivery_requests( + ConsiderRequest consider_pickup, + ConsiderRequest consider_dropoff); + + /// Allow this fleet adapter to consider cleaning requests. + /// + /// Pass in a nullptr to disable cleaning requests. + /// + /// By default, cleaning requests are not accepted until you provide this + /// callback. + /// + /// \param[in] consider + /// Decide whether to accept a cleaning request. The description will + /// satisfy the event_description_Clean.json schema of rmf_fleet_adapter. + /// The FleetUpdateHandle will ensure that the request is feasible for the + /// robots before triggering this callback. + FleetUpdateHandle& consider_cleaning_requests(ConsiderRequest consider); + + /// Allow this fleet adapter to consider patrol requests. + /// + /// Pass in a nullptr to disable patrol requests. + /// + /// By default, patrol requests are always accepted. + /// + /// \param[in] consider + /// Decide whether to accept a patrol request. The description will satisfy + /// the task_description_Patrol.json schema of rmf_fleet_adapter. The + /// FleetUpdateHandle will ensure that the request is feasible for the + /// robots before triggering this callback. + FleetUpdateHandle& consider_patrol_requests(ConsiderRequest consider); + + /// Allow this fleet adapter to consider composed requests. + /// + /// Pass in a nullptr to disable composed requests. + /// + /// By default, composed requests are always accepted, as long as the events + /// that they are composed of are accepted. + /// + /// \param[in] consider + /// Decide whether to accept a composed request. The description will + /// satisfy the task_description_Compose.json schema of rmf_fleet_adapter. + /// The FleetUpdateHandle will ensure that the request is feasible for the + /// robots before triggering this callback. + FleetUpdateHandle& consider_composed_requests(ConsiderRequest consider); + + /// Allow this fleet adapter to execute a PerformAction activity of specified + /// category which may be present in sequence event. + /// + /// \param[in] category + /// A string that categorizes the action. This value should be used when + /// filling out the category field in event_description_PerformAction.json + /// schema. + /// + /// \param[in] consider + /// Decide whether to accept the action based on the description field in + /// event_description_PerformAction.json schema. + FleetUpdateHandle& add_performable_action( + const std::string& category, + ConsiderRequest consider); + /// Specify a set of lanes that should be closed. void close_lanes(std::vector lane_indices); @@ -142,6 +272,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// compatible with the requested payload, pickup, and dropoff behavior /// settings. The path planning feasibility will be taken care of by the /// adapter internally. + [[deprecated("Use the consider_..._requests functions instead")]] FleetUpdateHandle& accept_task_requests(AcceptTaskRequest check); /// A callback function that evaluates whether a fleet will accept a delivery @@ -164,7 +295,7 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// compatible with the requested payload, pickup, and dropoff behavior /// settings. The path planning feasibility will be taken care of by the /// adapter internally. - [[deprecated("Use accept_task_requests() instead")]] + [[deprecated("Use consider_delivery_requests() instead")]] FleetUpdateHandle& accept_delivery_requests(AcceptDeliveryRequest check); /// Specify the default value for how high the delay of the current itinerary @@ -176,12 +307,31 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// Get the default value for the maximum acceptable delay. std::optional default_maximum_delay() const; + /// The behavior is identical to fleet_state_topic_publish_period + [[deprecated("Use fleet_state_topic_publish_period instead")]] + FleetUpdateHandle& fleet_state_publish_period( + std::optional value); + /// Specify a period for how often the fleet state message is published for /// this fleet. Passing in std::nullopt will disable the fleet state message /// publishing. The default value is 1s. - FleetUpdateHandle& fleet_state_publish_period( + FleetUpdateHandle& fleet_state_topic_publish_period( std::optional value); + /// Specify a period for how often the fleet state is updated in the database + /// and to the API server. This is separate from publishing the fleet state + /// over the ROS2 fleet state topic. Passing in std::nullopt will disable + /// the updating, but this is not recommended unless you intend to provide the + /// API server with the fleet states through some other means. + /// + /// The default value is 1s. + FleetUpdateHandle& fleet_state_update_period( + std::optional value); + + // Do not allow moving + FleetUpdateHandle(FleetUpdateHandle&&) = delete; + FleetUpdateHandle& operator=(FleetUpdateHandle&&) = delete; + class Implementation; private: FleetUpdateHandle(); 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 35b16518f..ee9839344 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -110,6 +111,51 @@ class RobotUpdateHandle /// value that was given to the setter. rmf_utils::optional maximum_delay() const; + /// 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 + void update_remaining_time(rmf_traffic::Duration remaining_time_estimate); + + // Trigger this when the action is finished + void finished(); + + // Returns false if the Action has been killed or cancelled + bool okay() const; + + // TODO: Consider giving access to the participant schedule and + // traffic negotiation + + // The desctructor will trigger finished() if it has not already been called + ~ActionExecution(); + + class Implementation; + private: + ActionExecution(); + rmf_utils::impl_ptr _pimpl; + }; + + /// Signature for a callback to request the robot to perform an action + /// + /// \param[in] category + /// A category of the action to be performed + /// + /// \param[in] description + /// A description of the action to be performed + /// + /// \param[in] execution + /// An ActionExecution object that will be provided to the user for + /// updating the state of the action. + using ActionExecutor = std::function; + + /// Set the ActionExecutor for this robot + void set_action_executor(ActionExecutor action_executor); + class Implementation; /// This API is experimental and will not be supported in the future. Users diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp index 182145305..bf2ecbf83 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/test/MockAdapter.hpp @@ -47,7 +47,8 @@ class MockAdapter : public std::enable_shared_from_this std::shared_ptr add_fleet( const std::string& fleet_name, rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph); + rmf_traffic::agv::Graph navigation_graph, + std::optional server_uri = std::nullopt); TrafficLight::UpdateHandlePtr add_traffic_light( std::shared_ptr command, @@ -71,7 +72,7 @@ class MockAdapter : public std::enable_shared_from_this void stop(); /// Submit a task request - void dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile); + void dispatch_task(std::string task_id, const nlohmann::json& request); ~MockAdapter(); diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml index 85e71a976..e1e0fc727 100644 --- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml +++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml @@ -39,6 +39,8 @@ + + + + diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 731ff6b82..8ac7f0aa7 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -22,12 +22,20 @@ rmf_task_msgs rmf_traffic rmf_traffic_ros2 + rmf_task_ros2 rmf_battery rmf_task + rmf_task_sequence std_msgs + rmf_api_msgs stubborn_buddies stubborn_buddies_msgs + libwebsocketpp-dev + nlohmann-json-dev + nlohmann_json_schema_validator_vendor + libsqlite3-dev + eigen yaml-cpp diff --git a/rmf_fleet_adapter/schemas/Place.json b/rmf_fleet_adapter/schemas/Place.json new file mode 100644 index 000000000..94c2f8a1e --- /dev/null +++ b/rmf_fleet_adapter/schemas/Place.json @@ -0,0 +1,32 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/Place.json", + "title": "Place Description", + "description": "Description of a place that the robot can go to", + "oneOf": [ + { + "$ref": "#/$defs/waypoint" + }, + { + "type": "object", + "properties": { + "waypoint": { "$ref": "#/$defs/waypoint" }, + "orientation": { "type": "number" } + }, + "required": ["waypoint"] + } + ], + "$defs": { + "waypoint": { + "oneOf": [ + { + "type": "string" + }, + { + "type": "integer", + "minimum": 0 + } + ] + } + } +} diff --git a/rmf_fleet_adapter/schemas/event_description_Clean.json b/rmf_fleet_adapter/schemas/event_description_Clean.json new file mode 100644 index 000000000..bb32232d1 --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_Clean.json @@ -0,0 +1,12 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_Clean.json", + "title": "Clean Event", + "description": "Clean a zone", + "type": "object", + "properties": { + "zone": { "type": "string" }, + "type": { "type": "string" } + }, + "required": ["zone"] +} diff --git a/rmf_fleet_adapter/schemas/event_description_DropOff.json b/rmf_fleet_adapter/schemas/event_description_DropOff.json new file mode 100644 index 000000000..02685aa56 --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_DropOff.json @@ -0,0 +1,6 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_DropOff.json", + "title": "Drop Off Event Description", + "$ref": "event_description_PayloadTransfer.json" +} diff --git a/rmf_fleet_adapter/schemas/event_description_GoToPlace.json b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json new file mode 100644 index 000000000..c9a57826d --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_GoToPlace.json @@ -0,0 +1,7 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_GoToPlace.json", + "title": "Go To Place Event Description", + "description": "Have a robot go to a place", + "$ref": "Place.json" +} diff --git a/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json new file mode 100644 index 000000000..0155859f5 --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json @@ -0,0 +1,37 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PayloadTransfer.json", + "title": "Item Transfer Event Description", + "description": "Description for a drop off or a pick up event category", + "type": "object", + "properties": { + "place": { "$ref": "Place.json" }, + "handler": { "type": "string" }, + "payload" : { + "oneOf": [ + { + "$ref": "#/$defs/payload_component" + }, + { + "type": "array", + "items": { "$ref": "#/$defs/payload_component" } + } + ] + } + }, + "required": ["place", "payload"], + "$defs": { + "payload_component": { + "type": "object", + "properties": { + "sku": { "type": "string" }, + "quantity": { + "type": "integer", + "minimum": 0 + }, + "compartment": { "type": "string" } + }, + "required": ["sku", "quantity"] + } + } +} diff --git a/rmf_fleet_adapter/schemas/event_description_PerformAction.json b/rmf_fleet_adapter/schemas/event_description_PerformAction.json new file mode 100644 index 000000000..2422aa0ea --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_PerformAction.json @@ -0,0 +1,29 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PerformAction.json", + "title": "PerformAction event Description", + "description": "Description schema for the perform_action category", + "type": "object", + "properties": { + "category": { + "description": "A string that describes the nature of this action. It should match that passed into FleetUpdateHandle::add_performable_action()", + "type": "string" + }, + "description": { + "description": "Additional information that will be passed along to the action executor." + }, + "unix_millis_action_duration_estimate": { + "description": "(Recommended) The estimated duration for this action", + "type": "integer" + }, + "use_tool_sink": { + "description": "(Optional) Whether the tool on the robot will be powered during the action. Default to false", + "type": "boolean" + }, + "expected_finish_location": { + "description": "(Optional) A place that the robot will end up after the action. Default to last known location", + "$ref": "Place.json" + } + }, + "required": ["category", "description"] +} diff --git a/rmf_fleet_adapter/schemas/event_description_PickUp.json b/rmf_fleet_adapter/schemas/event_description_PickUp.json new file mode 100644 index 000000000..654736429 --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_PickUp.json @@ -0,0 +1,6 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_PickUp.json", + "title": "Pick Up Event Description", + "$ref": "event_description_PayloadTransfer.json" +} diff --git a/rmf_fleet_adapter/schemas/event_description_Sequence.json b/rmf_fleet_adapter/schemas/event_description_Sequence.json new file mode 100644 index 000000000..2f2554191 --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description_Sequence.json @@ -0,0 +1,43 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description_Sequence.json", + "title": "Activity Sequence", + "description": "A sequence of activities", + "oneOf": [ + { + "$ref": "#/$defs/activity_array" + }, + { + "type": "object", + "properties": { + "activities": { "$ref": "#/$defs/activity_array" }, + "category": { + "description": "Customize the category display for this sequence", + "type": "string" + }, + "detail": { + "description": "Customize the detail display for this sequence" + } + }, + "required": ["activities"] + } + ], + "$defs": { + "activity_array": { + "type": "array", + "items": { + "type": "object", + "properties": { + "category": { + "description": "The category of the activity", + "type": "string" + }, + "description": { + "description": "A description of the activity. This must match a schema supported by a fleet for the activity category." + } + } + }, + "minimum": 1 + } + } +} diff --git a/rmf_fleet_adapter/schemas/task_description_Clean.json b/rmf_fleet_adapter/schemas/task_description_Clean.json new file mode 100644 index 000000000..d885eaa19 --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description_Clean.json @@ -0,0 +1,7 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Clean.json", + "title": "Clean Task", + "description": "Clean a zone", + "$ref": "event_description_Clean.json" +} diff --git a/rmf_fleet_adapter/schemas/task_description_Compose.json b/rmf_fleet_adapter/schemas/task_description_Compose.json new file mode 100644 index 000000000..db656347d --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description_Compose.json @@ -0,0 +1,51 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_api_msgs/main/rmf_api_msgs/schemas/compose_task_description.json", + "title": "Compose Task Description", + "description": "Compose a task description based on activities that are supported by fleets. When supported by fleets, this can be used as a task description for task_request.", + "type": "object", + "properties": { + "category": { + "description": "Specify the category for this composed task, as the operators should see it.", + "type": "string" + }, + "detail": { + "description": "Specify the detail for this composed task, as the operators should see it.", + "type": "string" + }, + "phases": { + "description": "List the phases of the task in the order that they should be performed.", + "type": "array", + "items": { + "type": "object", + "properties": { + "activity": { "$ref": "#/$defs/activity" }, + "on_cancel": { + "description": "A list of activities to perform if the task is canceled during this phase. Each activity is given its own phase which can be skipped but not canceled.", + "type": "array", + "items": { "$ref": "#/$defs/activity" } + } + }, + "required": ["activity"] + }, + "minimum": 1 + } + }, + "required": ["phases"], + "$defs": { + "activity": { + "description": "Describe the activity of the phase. This must match an activity description supported by one of the fleets. If there should be multiple activities within a phase, then use a Sequence activity.", + "type": "object", + "properties": { + "category": { + "description": "The category of this phase's activity.", + "type": "string" + }, + "description": { + "description": "A description of the activity. This must match a schema supported by a fleet for the category of this activity." + } + }, + "required": ["category", "description"] + } + } +} diff --git a/rmf_fleet_adapter/schemas/task_description_Delivery.json b/rmf_fleet_adapter/schemas/task_description_Delivery.json new file mode 100644 index 000000000..c2710af12 --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description_Delivery.json @@ -0,0 +1,12 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Delivery.json", + "title": "Delivery Task Description", + "description": "Description for a simple delivery task category", + "type": "object", + "properties": { + "pickup": { "$ref": "event_description_PickUp.json" }, + "dropoff": { "$ref": "event_description_DropOff.json" } + }, + "required": ["pickup", "dropoff"] +} diff --git a/rmf_fleet_adapter/schemas/task_description_Patrol.json b/rmf_fleet_adapter/schemas/task_description_Patrol.json new file mode 100644 index 000000000..6bf738c68 --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description_Patrol.json @@ -0,0 +1,20 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/task_description_Patrol.json", + "title": "Patrol Task Description", + "description": "Description of a patrolling task", + "type": "object", + "properties": { + "places": { + "description": "A list of which places to patrol between", + "type": "array", + "items": { "$ref": "Place.json" } + }, + "rounds": { + "description": "How many times the patrol should be performed. By default this is 1.", + "type": "integer", + "minimum": 1 + } + }, + "required": ["places"] +} diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index d4186bfac..fe3a77a9d 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -58,7 +58,10 @@ #include #include +#include + #include +#include //============================================================================== rmf_fleet_adapter::agv::RobotUpdateHandle::Unstable::Decision @@ -149,7 +152,8 @@ std::optional distance_from_graph( //============================================================================== class FleetDriverRobotCommandHandle - : public rmf_fleet_adapter::agv::RobotCommandHandle + : public rmf_fleet_adapter::agv::RobotCommandHandle, + public std::enable_shared_from_this { public: @@ -159,6 +163,9 @@ class FleetDriverRobotCommandHandle using ModeRequestPub = rclcpp::Publisher::SharedPtr; + using ActionExecution = + rmf_fleet_adapter::agv::RobotUpdateHandle::ActionExecution; + FleetDriverRobotCommandHandle( rclcpp::Node& node, std::string fleet_name, @@ -480,6 +487,21 @@ class FleetDriverRobotCommandHandle void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) { _travel_info.updater = std::move(updater); + // Set the action_executor for the robot + const auto teleop_executioner = + [w = weak_from_this()](const std::string& category, + const nlohmann::json& description, + ActionExecution execution) + { + // We do not do anything here. The user can can move the robot by + // sending PathRequest msgs. Instead we simply store the completed + // callback which will be called when we receive a RobotModeRequest. + const auto self = w.lock(); + if (!self) + return; + self->set_action_execution(execution); + }; + _travel_info.updater->set_action_executor(teleop_executioner); } void newly_closed_lanes(const std::unordered_set& closed_lanes) @@ -566,6 +588,25 @@ class FleetDriverRobotCommandHandle _travel_info.updater->interrupted(); } +void set_action_execution(ActionExecution action_execution) +{ + _action_execution = action_execution; +} + +void complete_robot_action() +{ + if (!_action_execution.has_value()) + return; + + _action_execution->finished(); + _action_execution = std::nullopt; + + RCLCPP_INFO( + _node->get_logger(), + "Robot [%s] has completed the action it was performing", + _travel_info.robot_name.c_str()); +} + private: rclcpp::Node* _node; @@ -589,6 +630,9 @@ class FleetDriverRobotCommandHandle std::mutex _mutex; + // ActionExecution for managing teleop action + std::optional _action_execution = std::nullopt; + std::unique_lock _lock() { std::unique_lock lock(_mutex, std::defer_lock); @@ -639,6 +683,10 @@ struct Connections : public std::enable_shared_from_this rclcpp::Publisher::SharedPtr mode_request_pub; + /// The topic subscription for ending teleop actions + rclcpp::Subscription::SharedPtr + mode_request_sub; + /// The client for listening to whether there is clearance in a lift rclcpp::Client::SharedPtr lift_watchdog_client; @@ -839,12 +887,26 @@ std::shared_ptr make_fleet( for (const auto& key : connections->graph->keys()) std::cout << " -- " << key.first << std::endl; + std::optional server_uri = std::nullopt; + const std::string uri = + node->declare_parameter("server_uri", std::string()); + if (!uri.empty()) + { + RCLCPP_ERROR( + node->get_logger(), + "API server URI: [%s]", uri.c_str()); + + server_uri = uri; + } + + // TODO(YV): Remove this before merging. Replace with declare_param + const std::string& db_path = fleet_name + ".db"; connections->fleet = adapter->add_fleet( - fleet_name, *connections->traits, *connections->graph); + fleet_name, *connections->traits, *connections->graph, server_uri, db_path); // We disable fleet state publishing for this fleet adapter because we expect // the fleet drivers to publish these messages. - connections->fleet->fleet_state_publish_period(std::nullopt); + connections->fleet->fleet_state_topic_publish_period(std::nullopt); connections->closed_lanes_pub = adapter->node()->create_publisher( @@ -894,6 +956,34 @@ std::shared_ptr make_fleet( connections->closed_lanes_pub->publish(state_msg); }); + connections->mode_request_sub = + adapter->node()->create_subscription( + "/action_execution_notice", + rclcpp::SystemDefaultsQoS(), + [w = connections->weak_from_this(), fleet_name]( + rmf_fleet_msgs::msg::ModeRequest::UniquePtr msg) + { + if (msg->fleet_name.empty() || + msg->fleet_name != fleet_name || + msg->robot_name.empty()) + { + return; + } + + if (msg->mode.mode == msg->mode.MODE_IDLE) + { + const auto self = w.lock(); + if (!self) + return; + + const auto command_it = self->robots.find(msg->robot_name); + if (command_it == self->robots.end()) + return; + + command_it->second->complete_robot_action(); + } + }); + // Parameters required for task planner // Battery system auto battery_system_optional = rmf_fleet_adapter::get_battery_system( @@ -1052,6 +1142,20 @@ std::shared_ptr make_fleet( return false; }); + const auto consider = + [](const nlohmann::json& /*description*/, + rmf_fleet_adapter::agv::FleetUpdateHandle::Confirmation& confirm) + { + // We accept all actions since full_control may be used for different + // types of robots. + confirm.accept(); + }; + + // Configure this fleet to perform any kind of teleop action + connections->fleet->add_performable_action( + "teleop", + consider); + if (node->declare_parameter("disable_delay_threshold", false)) { connections->fleet->default_maximum_delay(rmf_utils::nullopt); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp new file mode 100644 index 000000000..38e4e2b84 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.cpp @@ -0,0 +1,186 @@ +/* + * Copyright (C) 2021 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 "BroadcastClient.hpp" + +#include "agv/internal_FleetUpdateHandle.hpp" + +namespace rmf_fleet_adapter { +//============================================================================== +std::shared_ptr BroadcastClient::make( + const std::string& uri, + std::weak_ptr fleet_handle) +{ + std::shared_ptr client(new BroadcastClient()); + client->_uri = std::move(uri); + client->_fleet_handle = fleet_handle; + client->_shutdown = false; + client->_connected = false; + + // Initialize the Asio transport policy + client->_client.clear_access_channels(websocketpp::log::alevel::all); + client->_client.clear_error_channels(websocketpp::log::elevel::all); + client->_client.init_asio(); + client->_client.start_perpetual(); + client->_client_thread = std::thread( + [c = client]() + { + c->_client.run(); + }); + + client->_client.set_open_handler( + [c = client](websocketpp::connection_hdl) + { + c->_connected = true; + const auto fleet = c->_fleet_handle.lock(); + if (!fleet) + return; + const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet); + for (const auto& [conext, mgr] : impl.task_managers) + { + // Publish all task logs to the server + c->publish(mgr->task_log_updates()); + } + RCLCPP_INFO( + impl.node->get_logger(), + "BroadcastClient successfully connected to uri: [%s]", + c->_uri.c_str()); + }); + + client->_client.set_close_handler( + [c = client](websocketpp::connection_hdl) + { + c->_connected = false; + }); + + client->_client.set_fail_handler( + [c = client](websocketpp::connection_hdl) + { + c->_connected = false; + }); + + client->_processing_thread = std::thread( + [c = client]() + { + while (!c->_shutdown) + { + const auto fleet = c->_fleet_handle.lock(); + if (!fleet) + { + continue; + } + const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet); + + // Try to connect to the server if we are not connected yet + if (!c->_connected) + { + websocketpp::lib::error_code ec; + WebsocketClient::connection_ptr con = c->_client.get_connection( + c->_uri, ec); + c->_hdl = con->get_handle(); + c->_client.connect(con); + // TOD(YV): Without sending a test payload, ec seems to be 0 even + // when the client has not connected. Avoid sending this message. + c->_client.send(c->_hdl, "Hello", websocketpp::frame::opcode::text, + ec); + if (ec) + { + RCLCPP_WARN( + impl.node->get_logger(), + "BroadcastClient unable to connect to [%s]. Please make sure " + "server is running.", + c->_uri.c_str()); + c->_connected = false; + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + continue; + } + RCLCPP_INFO( + impl.node->get_logger(), + "BroadcastClient successfully connected to [%s]", + c->_uri.c_str()); + c->_connected = true; + } + + std::unique_lock lock(c->_wait_mutex); + c->_cv.wait(lock, + [c]() + { + return !c->_queue.empty(); + }); + + while (!c->_queue.empty()) + { + std::lock_guard lock(c->_queue_mutex); + websocketpp::lib::error_code ec; + const std::string& msg = c->_queue.front().dump(); + c->_client.send(c->_hdl, msg, websocketpp::frame::opcode::text, ec); + if (ec) + { + RCLCPP_ERROR( + impl.node->get_logger(), + "BroadcastClient unable to publish message"); + // TODO(YV): Check if we should re-connect to server + break; + } + c->_queue.pop(); + } + } + }); + + return client; + +} + +//============================================================================== +void BroadcastClient::publish(const nlohmann::json& msg) +{ + std::lock_guard lock(_queue_mutex); + _queue.push(msg); + _cv.notify_all(); +} + +//============================================================================== +void BroadcastClient::publish(const std::vector& msgs) +{ + std::lock_guard lock(_queue_mutex); + for (const auto& msg : msgs) + _queue.push(msg); + _cv.notify_all(); +} + +//============================================================================== +BroadcastClient::BroadcastClient() +{ + // Do nothing +} + +//============================================================================== +BroadcastClient::~BroadcastClient() +{ + _shutdown = true; + if (_processing_thread.joinable()) + { + _processing_thread.join(); + } + if (_client_thread.joinable()) + { + _client_thread.join(); + } + _client.stop_perpetual(); +} + +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp new file mode 100644 index 000000000..3ef1beecd --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/BroadcastClient.hpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP +#define SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace rmf_fleet_adapter { +//============================================================================== +// A wrapper around a websocket client for broadcasting states and logs for +// fleets, robots and tasks. A queue of json msgs is maintained and published +// in an internal thread whenever connection to a server is established. +class BroadcastClient : public std::enable_shared_from_this +{ +public: + using WebsocketClient = + websocketpp::client; + using WebsocketMessagePtr = WebsocketClient::message_ptr; + using ConnectionHDL = websocketpp::connection_hdl; + using Connections = std::set>; + + /// \param[in] uri + /// "ws://localhost:9000" + static std::shared_ptr make( + const std::string& uri, + std::weak_ptr fleet_handle); + + // Publish a single message + void publish(const nlohmann::json& msg); + + // Publish a vector of messages + void publish(const std::vector& msgs); + + ~BroadcastClient(); + +private: + BroadcastClient(); + std::string _uri; + std::weak_ptr _fleet_handle; + WebsocketClient _client; + websocketpp::connection_hdl _hdl; + std::mutex _wait_mutex; + std::mutex _queue_mutex; + std::condition_variable _cv; + std::queue _queue; + std::thread _processing_thread; + std::thread _client_thread; + std::atomic_bool _connected; + std::atomic_bool _shutdown; +}; + +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__BROADCASTCLIENT_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp new file mode 100644 index 000000000..33a1e3495 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.cpp @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2022 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 "DatabaseLogger.hpp" +#include "agv/internal_FleetUpdateHandle.hpp" + +#include +#include + +// TODO(YV) Remove +#include + +//============================================================================== +// int sqlite3_exec( +// sqlite3*, /* An open database */ +// const char *sql, /* SQL to be evaluated */ +// int (*callback)(void*,int,char**,char**), /* Callback function */ +// void *, /* 1st argument to callback */ +// char **errmsg /* Error msg written here */ +// ); +//============================================================================== + +namespace rmf_fleet_adapter { +//============================================================================== +auto DatabaseLogger::make(const std::string& file_path) -> +std::shared_ptr +{ + std::shared_ptr logger(new DatabaseLogger()); + logger->_file_path = file_path; + + if (!std::filesystem::exists(file_path)) + { + std::filesystem::create_directories( + std::filesystem::absolute(file_path).parent_path()); + } + + // Variables that will be reassigned + int error; + std::string sql; + char* error_msg; + + error = sqlite3_open(file_path.c_str(), &logger->_db); + if (error) + return nullptr; + + // Create table for active task states + sql = "CREATE TABLE IF NOT EXISTS ACTIVE_TASK(" + "ROBOT TEXT PRIMARY KEY NOT NULL, " + "STATE TEXT NOT NULL);"; + error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error from creating ACTIVE_TASK table: " << error_msg << std::endl; + + // Create table for task logs + sql = "CREATE TABLE IF NOT EXISTS TASK_LOGS(" + "ID TEXT PRIMARY KEY NOT NULL, " + "LOGS TEXT NOT NULL);"; + error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error from creating TASK_LOGS table: " << error_msg << std::endl; + + // Create table for task logs + // DISPATCH and DIRECT will be jsons containing deserialized assignment jsons + sql = "CREATE TABLE IF NOT EXISTS TASK_QUEUES(" + "ROBOT TEXT PRIMARY KEY NOT NULL, " + "QUEUES TEXT NOT NULL);"; + error = sqlite3_exec(logger->_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error from creating TASK_QUEUES table: " << error_msg << std::endl; + + + // TODO(YV): Table for bid notice assignments + + // TODO(YV): Also store history of task & state logs along with latest state. + // Proposed key (robot, task, phase, event, seq) + + + // TODO(YV): If db is not empty, generate Restore. For now we will set it to + // nullopt. + + std::cout << "Created database" << std::endl; + + return logger; +} + +//============================================================================== +DatabaseLogger::DatabaseLogger() +{ + // Do nothing +} + +//============================================================================== +auto DatabaseLogger::restore() const -> std::optional +{ + return _restored; +} + +//============================================================================== +DatabaseLogger::~DatabaseLogger() +{ + // Safely destruct the sqlite3 object + sqlite3_close(_db); +} + +//============================================================================== +void DatabaseLogger::backup_bid_notice_assignments( + const BidNoticeAssignments& assignments) +{ + // TODO +} + +//============================================================================== +void DatabaseLogger::backup_task_queues(const TaskManager& mgr) +{ + // We can access private members of TaskManager as DatabaseLogger is a friend + + auto fleet_handle = mgr._fleet_handle.lock(); + if (!fleet_handle) + { + std::cout << "Unable to lock fleet handle inside backup_task_queues" + << std::endl; + return; + } + auto& impl = + agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + const std::string& robot = mgr._context->name(); + const auto& request_jsons = impl.task_request_jsons; + std::vector errors; + + nlohmann::json queues = {{"dispatch",{}}, {"direct",{}}}; + for (const auto& assignment : mgr._queue) + { + queues["dispatch"].push_back(convert(assignment, request_jsons)); + } + + // Direct assignments + + for (const auto& assignment : mgr._direct_queue) + { + queues["direct"].push_back(convert(assignment.assignment, request_jsons)); + } + // Write to db + { + std::lock_guard lock(_mutex); + std::string data = queues.dump(); + std::string sql = "REPLACE INTO TASK_QUEUES (ROBOT,QUEUES) " + "VALUES('" + robot + "', '" + data + "');"; + char* error_msg; + auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error updating task queue: " << error_msg << std::endl; + } + +} + +//============================================================================== +void DatabaseLogger::backup_active_task( + const std::string& robot, + const nlohmann::json& task_state) +{ + std::lock_guard lock(_mutex); + const std::string state = task_state.dump(); + + const std::string sql = "REPLACE INTO ACTIVE_TASK (ROBOT,STATE) " + "VALUES('" + robot + "', '" + state + "');"; + char* error_msg; + auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error updating active task: " << error_msg << std::endl; +} + +//============================================================================== +void DatabaseLogger::backup_task_logs( + const std::string& task_id, + const nlohmann::json& task_logs) +{ + std::lock_guard lock(_mutex); + const std::string logs = task_logs.dump(); + const std::string sql = "REPLACE INTO TASK_LOGS (ID,LOGS) " + "VALUES('" + task_id + "', '" + logs + "');"; + char* error_msg; + auto error = sqlite3_exec(_db, sql.c_str(), NULL, NULL, &error_msg); + if (error) + std::cout << "Error updating active task: " << error_msg << std::endl; +} + +namespace { +//============================================================================== +std::chrono::milliseconds to_millis(rmf_traffic::Duration duration) +{ + return std::chrono::duration_cast(duration); +} + +} // namespace anonymous + +//============================================================================== +nlohmann::json DatabaseLogger::convert(const rmf_task::State& state) const +{ + // State schmea + // { + // waypoint: integer, + // orientation: double,[optional] + // time: integer + // charger: integer + // battery_soc + // } + + nlohmann::json msg; + + // To be safe we check if the optionals have value but they definitely should + auto waypoint_opt = state.waypoint(); + if (waypoint_opt.has_value()) + msg["waypoint"] = waypoint_opt.value(); + auto orientation_opt = state.orientation(); + if (orientation_opt.has_value()) + msg["orientation"] = orientation_opt.value(); + auto time_opt = state.time(); + if (time_opt.has_value()) + msg["time"] = to_millis(time_opt.value().time_since_epoch()).count(); + auto charger_opt = state.dedicated_charging_waypoint(); + if (charger_opt.has_value()) + msg["charger"] = charger_opt.value(); + auto soc_opt = state.battery_soc(); + if (soc_opt.has_value()) + msg["battery_soc"] = soc_opt.value(); + + return msg; +} + +//============================================================================== +nlohmann::json DatabaseLogger::convert( + const Assignment& assignment, + const std::unordered_map request_jsons) const +{ + + // Assignment schema + // { + // request: ref rmf_api_msgs::schemas::task_request, + // state: ref State schema, + // unix_millis_deployment_time: integer, + // } + + nlohmann::json msg; + const auto request = assignment.request(); + const auto& state = assignment.finish_state(); + const auto deployment_time = assignment.deployment_time(); + + msg["state"] = convert(state); + msg["unix_millis_deployment_time"] = + to_millis(deployment_time.time_since_epoch()).count(); + + const auto it = request_jsons.find(request->booking()->id()); + if (it != request_jsons.end()) + { + msg["request"] = it->second; + } + else + { + // TODO(YV): Deserialize this automatic task into schemas::task_request + } + + return msg; +} + +//============================================================================== +auto DatabaseLogger::convert(const nlohmann::json& msg) const -> Assignment +{ + // msg follows the Assignment schema defined above + + // TODO(YV): Fix + rmf_task::State state; + // We can use FleetUpdateHandle::Implementation::convert(json_requst) to get + // ConstRequestPtr + Assignment assignment{nullptr, state, std::chrono::steady_clock::now()}; + return assignment; +} + +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp new file mode 100644 index 000000000..93d9b267e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DatabaseLogger.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP +#define SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP + +#include + +#include "TaskManager.hpp" + +#include +#include + +#include +#include + +namespace rmf_fleet_adapter { +//============================================================================== +// A wrapper around a sqlite3 database. +// TODO(YV): The Logger can inherit from an abstract class so that users can +// use whatever DBS they want. +class DatabaseLogger +{ +public: + using Assignment = rmf_task::TaskPlanner::Assignment; + using Assignments = rmf_task::TaskPlanner::Assignments; + using BidNoticeAssignments = + std::unordered_map; + // Bundle up the restored state of the fleet adapter + // TODO(YV): FleetUpdateHandle::add_robot() can lookup for restored task + // managers. But if a new robot is being added, what do we do with the + // previous bid_notice_assignments? The index order will be incorrect. + // We would need to modify the implementation of dispatch_command_cb to not + // return an error when the number of robots do not match the number in + // Assignments. + struct Restored + { + std::unordered_map> managers; + BidNoticeAssignments bid_notice_assignments; + }; + + // TODO(YV): Should we accept a bool to delete/drop all tables? + static std::shared_ptr make( + const std::string& file_path); + + // Returns nullopt if file_path did not exist previously. + // Does this function require a weak_ptr to generating + // requests/deserialization/validation? + std::optional restore() const; + + void backup_bid_notice_assignments(const BidNoticeAssignments& assignments); + + void backup_task_queues(const TaskManager& mgr); + + // Key used: robot + void backup_active_task( + const std::string& robot, + const nlohmann::json& task_state); + + // Key used: (task_id) + void backup_task_logs( + const std::string& task_id, + const nlohmann::json& task_logs); + + ~DatabaseLogger(); + +private: + DatabaseLogger(); + + // Helper functions to serialize/deserialize assignments + // TODO(YV): Consider formalizing the schema in rmf_fleet_adapter/schemas + nlohmann::json convert(const rmf_task::State& state) const; + + nlohmann::json convert( + const Assignment& assignment, + const std::unordered_map request_jsons) const; + + Assignment convert(const nlohmann::json& msg) const; + + // TODO(YV): Explore using an open-source library to build sql statements + + std::string _file_path; + sqlite3* _db; + std::mutex _mutex; + std::optional _restored = std::nullopt; +}; + +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__DATABASELOGGER_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp new file mode 100644 index 000000000..9874d0aed --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/DeserializeJSON.hpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP +#define SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP + +#include +#include +#include + +namespace rmf_fleet_adapter { + +//============================================================================== +template +class DeserializeJSON +{ +public: + + using Deserializer = std::function; + using Validator = nlohmann::json_schema::json_validator; + + struct Handlers + { + std::shared_ptr validator; + Deserializer deserializer; + }; + + void add( + const std::string& category, + Validator validator, + Deserializer deserializer) + { + handlers.insert_or_assign( + category, + Handlers{ + std::make_shared(std::move(validator)), + std::move(deserializer) + }); + } + + void add( + const std::string& category, + std::shared_ptr validator, + Deserializer deserializer) + { + handlers.insert_or_assign( + category, + Handlers{std::move(validator), std::move(deserializer)}); + } + + std::unordered_map handlers; + +}; + +template +using DeserializeJSONPtr = std::shared_ptr>; + +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__DESERIALIZEJSON_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp similarity index 84% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp index 9eec4e68c..bdbb1847c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.cpp @@ -15,7 +15,7 @@ * */ -#include "Task.hpp" +#include "LegacyTask.hpp" #include #include @@ -27,16 +27,16 @@ namespace rmf_fleet_adapter { //============================================================================== -std::shared_ptr Task::make( +std::shared_ptr LegacyTask::make( std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, - rmf_task::agv::State finish_state, + rmf_task::State finish_state, rmf_task::ConstRequestPtr request) { - return std::make_shared( - Task(std::move(id), + return std::make_shared( + LegacyTask(std::move(id), std::move(phases), std::move(worker), deployment_time, @@ -45,38 +45,38 @@ std::shared_ptr Task::make( } //============================================================================== -void Task::begin() +void LegacyTask::begin() { if (!_active_phase) _start_next_phase(); } //============================================================================== -auto Task::observe() const -> const rxcpp::observable& +auto LegacyTask::observe() const -> const rxcpp::observable& { return _status_obs; } //============================================================================== -auto Task::current_phase() -> const std::shared_ptr& +auto LegacyTask::current_phase() -> const std::shared_ptr& { return _active_phase; } //============================================================================== -auto Task::current_phase() const -> std::shared_ptr +auto LegacyTask::current_phase() const -> std::shared_ptr { return _active_phase; } //============================================================================== -auto Task::pending_phases() const -> const PendingPhases& +auto LegacyTask::pending_phases() const -> const PendingPhases& { return _pending_phases; } //============================================================================== -void Task::cancel() +void LegacyTask::cancel() { _pending_phases.clear(); if (_active_phase) @@ -84,49 +84,49 @@ void Task::cancel() } //============================================================================== -const std::string& Task::id() const +const std::string& LegacyTask::id() const { return _id; } //============================================================================== -const rmf_task::ConstRequestPtr Task::request() const +const rmf_task::ConstRequestPtr LegacyTask::request() const { return _request; } //============================================================================== -const rmf_traffic::Time Task::deployment_time() const +const rmf_traffic::Time LegacyTask::deployment_time() const { return _deployment_time; } //============================================================================== -const rmf_task::agv::State Task::finish_state() const +const rmf_task::State LegacyTask::finish_state() const { return _finish_state; } //============================================================================== -void Task::task_profile(Task::TaskProfileMsg profile) +void LegacyTask::task_profile(LegacyTask::TaskProfileMsg profile) { _profile = profile; return; } //============================================================================== -const Task::TaskProfileMsg& Task::task_profile() const +const LegacyTask::TaskProfileMsg& LegacyTask::task_profile() const { return _profile; } //============================================================================== -Task::Task( +LegacyTask::LegacyTask( std::string id, std::vector> phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, - rmf_task::agv::State finish_state, + rmf_task::State finish_state, rmf_task::ConstRequestPtr request) : _id(std::move(id)), _pending_phases(std::move(phases)), @@ -140,7 +140,7 @@ Task::Task( } //============================================================================== -void Task::_start_next_phase() +void LegacyTask::_start_next_phase() { if (_pending_phases.empty()) { @@ -170,7 +170,7 @@ void Task::_start_next_phase() { // *INDENT-OFF* throw std::runtime_error( - "[Task::_start_next_phase] INTERNAL ERROR: Next phase has a null value"); + "[LegacyTask::_start_next_phase] INTERNAL ERROR: Next phase has a null value"); // *INDENT-ON* } _active_phase = next_pending->begin(); @@ -189,7 +189,7 @@ void Task::_start_next_phase() auto summary = msg; // We have received a status update from the phase. We will forward - // this to whoever is subscribing to the Task. + // this to whoever is subscribing to the LegacyTask. summary.task_id = task->_id; // We don't want to say that the task is complete until the very end. @@ -239,7 +239,7 @@ void Task::_start_next_phase() } //============================================================================== -auto Task::_process_summary(const StatusMsg& input_msg) -> StatusMsg +auto LegacyTask::_process_summary(const StatusMsg& input_msg) -> StatusMsg { auto output = input_msg; if (!_initial_time.has_value()) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp similarity index 84% rename from rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp rename to rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp index 118d57271..c580924da 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/LegacyTask.hpp @@ -15,8 +15,8 @@ * */ -#ifndef SRC__RMF_FLEET_ADAPTER__TASK_HPP -#define SRC__RMF_FLEET_ADAPTER__TASK_HPP +#ifndef SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP +#define SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP #include #include @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -35,14 +35,14 @@ namespace rmf_fleet_adapter { //============================================================================== -class Task : public std::enable_shared_from_this +class LegacyTask : public std::enable_shared_from_this { public: using StatusMsg = rmf_task_msgs::msg::TaskSummary; - /// This class represents the active phase of a Task. It provides an - /// observable that the Task can track to stay up-to-date on the status and to + /// This class represents the active phase of a LegacyTask. It provides an + /// observable that the LegacyTask can track to stay up-to-date on the status and to /// know when to begin the next phase. /// /// The ActivePhase class must be a schedule Negotiator so that it can @@ -52,7 +52,7 @@ class Task : public std::enable_shared_from_this { public: - using StatusMsg = Task::StatusMsg; + using StatusMsg = LegacyTask::StatusMsg; /// Get a reference to an observable for the status of this ActivePhase. /// When this phase is complete, it will trigger on_completed() @@ -98,17 +98,17 @@ class Task : public std::enable_shared_from_this using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile; // Make a new task - static std::shared_ptr make( + static std::shared_ptr make( std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, - rmf_task::agv::State finish_state, + rmf_task::State finish_state, rmf_task::ConstRequestPtr request); void begin(); - /// Get a reference to an observable for the status of this Task + /// Get a reference to an observable for the status of this LegacyTask const rxcpp::observable& observe() const; /// Get the current phase of the task @@ -128,11 +128,11 @@ class Task : public std::enable_shared_from_this /// Get the request used to generate this task const rmf_task::ConstRequestPtr request() const; - /// Get the deployment time of this Task + /// Get the deployment time of this LegacyTask const rmf_traffic::Time deployment_time() const; - /// Get the finish state of this Task - const rmf_task::agv::State finish_state() const; + /// Get the finish state of this LegacyTask + const rmf_task::State finish_state() const; /// Set the TaskProfile of this task void task_profile(TaskProfileMsg profile); @@ -142,12 +142,12 @@ class Task : public std::enable_shared_from_this private: - Task( + LegacyTask( std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, rmf_traffic::Time deployment_time, - rmf_task::agv::State finish_state, + rmf_task::State finish_state, rmf_task::ConstRequestPtr request); std::string _id; @@ -167,7 +167,7 @@ class Task : public std::enable_shared_from_this rmf_utils::optional _initial_time; rmf_traffic::Time _deployment_time; - rmf_task::agv::State _finish_state; + rmf_task::State _finish_state; rmf_task::ConstRequestPtr _request; TaskProfileMsg _profile; @@ -179,4 +179,4 @@ class Task : public std::enable_shared_from_this } -#endif // SRC__RMF_FLEET_ADAPTER__TASK_HPP +#endif // SRC__RMF_FLEET_ADAPTER__LEGACYTASK_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp new file mode 100644 index 000000000..2e05056e1 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.cpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2021 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 "Negotiator.hpp" + +namespace rmf_fleet_adapter { + +//============================================================================== +std::shared_ptr Negotiator::make( + agv::RobotContextPtr context, + Respond respond) +{ + auto negotiator = std::make_shared(); + negotiator->_context = std::move(context); + negotiator->_respond = std::move(respond); + negotiator->claim_license(); + + return negotiator; +} + +//============================================================================== +void Negotiator::respond( + const TableViewerPtr& table_viewer, + const ResponderPtr& responder) +{ + auto service = _respond(table_viewer, responder); + if (!service) + { + // If we didn't get a service back then we will trust that the response was + // already submitted upstream. + return; + } + + auto negotiate_sub = + rmf_rxcpp::make_job(service) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [w = weak_from_this()](const auto& result) + { + if (auto self = w.lock()) + { + result.respond(); + self->_negotiate_services.erase(result.service); + } + else + { + // We need to make sure we respond in some way so that we don't risk + // making a negotiation hang forever. If this task is dead, then we + // should at least respond by forfeiting. + const auto service = result.service; + const auto responder = service->responder(); + responder->forfeit({}); + } + }); + + using namespace std::chrono_literals; + const auto wait_duration = 2s + table_viewer->sequence().back().version * 10s; + auto negotiation_timer = _context->node()->try_create_wall_timer( + wait_duration, + [s = service->weak_from_this()] + { + if (const auto service = s.lock()) + service->interrupt(); + }); + + _negotiate_services[service] = + NegotiationManagers{ + std::move(negotiate_sub), + std::move(negotiation_timer) + }; +} + +//============================================================================== +services::ProgressEvaluator Negotiator::make_evaluator( + const TableViewerPtr& table_viewer) +{ + services::ProgressEvaluator evaluator; + if (table_viewer->parent_id()) + { + const auto& s = table_viewer->sequence(); + assert(s.size() >= 2); + evaluator.compliant_leeway_base *= s[s.size()-2].version + 1; + evaluator.max_cost_threshold = 90.0 + 30.0*s[s.size()-2].version; + } + + return evaluator; +} + +//============================================================================== +void Negotiator::clear_license() +{ + _license = nullptr; +} + +//============================================================================== +void Negotiator::claim_license() +{ + _license = _context->set_negotiator(this); +} + +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp new file mode 100644 index 000000000..551da40b6 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Negotiator.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP +#define SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP + +#include + +#include "agv/RobotContext.hpp" +#include "services/Negotiate.hpp" + +namespace rmf_fleet_adapter { + +//============================================================================== +class Negotiator + : public rmf_traffic::schedule::Negotiator, + public std::enable_shared_from_this +{ +public: + + using NegotiatePtr = std::shared_ptr; + using Respond = + std::function; + + static std::shared_ptr make( + agv::RobotContextPtr context, + Respond respond); + + void respond( + const TableViewerPtr& table_viewer, + const ResponderPtr& responder) final; + + static services::ProgressEvaluator make_evaluator( + const TableViewerPtr& table_viewer); + + void clear_license(); + + void claim_license(); + +private: + + struct NegotiationManagers + { + rmf_rxcpp::subscription_guard subscription; + rclcpp::TimerBase::SharedPtr timer; + }; + using NegotiateServiceMap = + std::unordered_map; + NegotiateServiceMap _negotiate_services; + + agv::RobotContextPtr _context; + std::shared_ptr _license; + Respond _respond; +}; + +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__NEGOTIATOR_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 34b1b78d3..39565e224 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -26,30 +26,109 @@ #include +#include "agv/internal_FleetUpdateHandle.hpp" #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" #include "tasks/Delivery.hpp" #include "tasks/Loop.hpp" #include "phases/ResponsiveWait.hpp" +#include "events/EmergencyPullover.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rmf_fleet_adapter { //============================================================================== -TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) +TaskManagerPtr TaskManager::make( + agv::RobotContextPtr context, + std::optional> broadcast_client, + std::weak_ptr fleet_handle) { - auto mgr = TaskManagerPtr(new TaskManager(std::move(context))); + auto mgr = TaskManagerPtr( + new TaskManager( + std::move(context), + std::move(broadcast_client), + std::move(fleet_handle))); + + auto begin_pullover = [w = mgr->weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_context->worker().schedule( + [w = self->weak_from_this()](const auto&) + { + const auto self = w.lock(); + + if (!self->_emergency_active) + return; + + // TODO(MXG): Consider subscribing to the emergency pullover update + self->_emergency_pullover = events::EmergencyPullover::Standby::make( + rmf_task_sequence::Event::AssignID::make(), self->_context, [](){}) + ->begin([](){}, self->_make_resume_from_emergency()); + }); + }; + mgr->_emergency_sub = mgr->_context->node()->emergency_notice() .observe_on(rxcpp::identity_same_worker(mgr->_context->worker())) .subscribe( - [w = mgr->weak_from_this()](const auto& msg) + [w = mgr->weak_from_this(), begin_pullover](const auto& msg) { if (auto mgr = w.lock()) { - if (auto task = mgr->_active_task) + if (mgr->_emergency_active == msg->data) + return; + + mgr->_emergency_active = msg->data; + if (msg->data) + { + if (mgr->_active_task) + { + mgr->_emergency_pullover_interrupt_token = + mgr->_active_task.add_interruption( + {"emergency pullover"}, + mgr->_context->now(), + [begin_pullover]() { begin_pullover(); }); + } + else + { + begin_pullover(); + } + } + else { - if (auto phase = task->current_phase()) - phase->emergency_alarm(msg->data); + if (auto pullover = mgr->_emergency_pullover) + pullover->cancel(); } } }); @@ -76,625 +155,1961 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) mgr->_begin_waiting(); + mgr->_travel_estimator = std::make_shared( + mgr->_context->task_planner()->configuration().parameters()); + + mgr->_update_timer = mgr->_context->node()->try_create_wall_timer( + std::chrono::milliseconds(100), + [w = mgr->weak_from_this()]() + { + if (const auto self = w.lock()) + self->_consider_publishing_updates(); + }); + + mgr->_task_request_api_sub = mgr->_context->node()->task_api_request() + .observe_on(rxcpp::identity_same_worker(mgr->_context->worker())) + .subscribe( + [w = mgr->weak_from_this()]( + const rmf_task_msgs::msg::ApiRequest::SharedPtr& request) + { + if (const auto self = w.lock()) + self->_handle_request(request->json_msg, request->request_id); + }); + + const std::vector schemas = { + rmf_api_msgs::schemas::task_state, + rmf_api_msgs::schemas::task_log, + rmf_api_msgs::schemas::log_entry, + rmf_api_msgs::schemas::task_state_update, + rmf_api_msgs::schemas::task_log_update, + rmf_api_msgs::schemas::simple_response, + rmf_api_msgs::schemas::token_response, + rmf_api_msgs::schemas::cancel_task_request, + rmf_api_msgs::schemas::cancel_task_response, + rmf_api_msgs::schemas::kill_task_request, + rmf_api_msgs::schemas::kill_task_response, + rmf_api_msgs::schemas::interrupt_task_request, + rmf_api_msgs::schemas::interrupt_task_response, + rmf_api_msgs::schemas::resume_task_request, + rmf_api_msgs::schemas::resume_task_response, + rmf_api_msgs::schemas::rewind_task_request, + rmf_api_msgs::schemas::rewind_task_response, + rmf_api_msgs::schemas::robot_task_request, + rmf_api_msgs::schemas::dispatch_task_response, + rmf_api_msgs::schemas::task_state, + rmf_api_msgs::schemas::error, + rmf_api_msgs::schemas::robot_task_response, + rmf_api_msgs::schemas::skip_phase_request, + rmf_api_msgs::schemas::skip_phase_response, + rmf_api_msgs::schemas::task_request, + rmf_api_msgs::schemas::undo_skip_phase_request, + rmf_api_msgs::schemas::undo_skip_phase_response, + rmf_api_msgs::schemas::error + }; + + for (const auto& schema : schemas) + { + const auto json_uri = nlohmann::json_uri{schema["$id"]}; + mgr->_schema_dictionary.insert({json_uri.url(), schema}); + } + return mgr; } //============================================================================== -TaskManager::TaskManager(agv::RobotContextPtr context) -: _context(std::move(context)) +TaskManager::TaskManager( + agv::RobotContextPtr context, + std::optional> broadcast_client, + std::weak_ptr fleet_handle) +: _context(std::move(context)), + _broadcast_client(std::move(broadcast_client)), + _fleet_handle(std::move(fleet_handle)), + _next_sequence_number(0), + _last_update_time(std::chrono::steady_clock::now() - std::chrono::seconds(1)) { // Do nothing. The make() function does all further initialization. } //============================================================================== -auto TaskManager::expected_finish_location() const -> StartSet +TaskManager::ActiveTask::ActiveTask(rmf_task::Task::ActivePtr task) +: _task(std::move(task)), + _interruption_handler(std::make_shared()) { - if (_expected_finish_location) - return {*_expected_finish_location}; - - return _context->location(); + // Do nothing } //============================================================================== -auto TaskManager::expected_finish_state() const -> State +const std::string& TaskManager::ActiveTask::id() const { - // If an active task exists, return the estimated finish state of that task - /// else update the current time and battery level for the state and return - if (_active_task) - return _context->current_task_end_state(); - - // Update battery soc and finish time in the current state - auto finish_state = _context->current_task_end_state(); - auto location = finish_state.location(); - location.time(rmf_traffic_ros2::convert(_context->node()->now())); - finish_state.location(location); + if (!_task) + { + throw std::runtime_error( + "[TaskManager::ActiveTask::id] Called when there is no active task. " + "This is a serious bug, please report this to the developers of RMF "); + } - const double current_battery_soc = _context->current_battery_soc(); - finish_state.battery_soc(current_battery_soc); + return _task->tag()->booking()->id(); +} - return finish_state; +namespace { +//============================================================================== +std::chrono::milliseconds to_millis(rmf_traffic::Duration duration) +{ + return std::chrono::duration_cast(duration); } //============================================================================== -const agv::RobotContextPtr& TaskManager::context() +std::string status_to_string(rmf_task::Event::Status status) { - return _context; + using Status = rmf_task::Event::Status; + switch (status) + { + case Status::Uninitialized: + return "uninitialized"; + case Status::Blocked: + return "blocked"; + case Status::Error: + return "error"; + case Status::Failed: + return "failed"; + case Status::Standby: + return "standby"; + case Status::Underway: + return "underway"; + case Status::Delayed: + return "delayed"; + case Status::Skipped: + return "skipped"; + case Status::Canceled: + return "canceled"; + case Status::Killed: + return "killed"; + case Status::Completed: + return "completed"; + default: + return "uninitialized"; + } } //============================================================================== -const Task* TaskManager::current_task() const +std::string tier_to_string(rmf_task::Log::Entry::Tier tier) { - return _active_task.get(); + using Tier = rmf_task::Log::Entry::Tier; + switch (tier) + { + case Tier::Info: + return "info"; + case Tier::Warning: + return "warning"; + case Tier::Error: + return "error"; + default: + return "uninitialized"; + } } //============================================================================== -agv::ConstRobotContextPtr TaskManager::context() const +nlohmann::json log_to_json(const rmf_task::Log::Entry& entry) { - return _context; + nlohmann::json output; + output["seq"] = entry.seq(); + output["tier"] = tier_to_string(entry.tier()); + output["unix_millis_time"] = + to_millis(entry.time().time_since_epoch()).count(); + output["text"] = entry.text(); + + return output; } //============================================================================== -void TaskManager::set_queue( - const std::vector& assignments, - const TaskManager::TaskProfiles& task_profiles) +nlohmann::json& copy_phase_data( + nlohmann::json& phases, + const rmf_task::Phase::Active& snapshot, + rmf_task::Log::Reader& reader, + nlohmann::json& all_phase_logs) { - // We indent this block as _mutex is also locked in the _begin_next_task() - // function that is called at the end of this function. + const auto& tag = *snapshot.tag(); + const auto& header = tag.header(); + const auto id = tag.id(); + auto& phase_state = phases[std::to_string(id)]; + phase_state["id"] = id; + phase_state["category"] = header.category(); + phase_state["detail"] = header.detail(); + phase_state["original_estimate_millis"] = + std::max(0l, to_millis(header.original_duration_estimate()).count()); + phase_state["estimate_millis"] = + std::max(0l, to_millis(snapshot.estimate_remaining_time()).count()); + phase_state["final_event_id"] = snapshot.final_event()->id(); + auto& event_states = phase_state["events"]; + + // TODO(MXG): Add in skip request information + + std::vector event_queue; + event_queue.push_back(snapshot.final_event()); + + auto& phase_logs = all_phase_logs[std::to_string(id)]; + auto& event_logs = phase_logs["events"]; + event_logs = std::unordered_map>(); + + while (!event_queue.empty()) { - std::lock_guard guard(_mutex); - _queue.clear(); - - // We use dynamic cast to determine the type of request and then call the - // appropriate make(~) function to convert the request into a task - for (std::size_t i = 0; i < assignments.size(); ++i) - { - const auto& a = assignments[i]; - auto start = _context->current_task_end_state().location(); - if (i != 0) - start = assignments[i-1].state().location(); - start.time(a.deployment_time()); - const auto request = a.request(); - - TaskProfileMsg task_profile; - bool auto_request = request->automatic(); - const auto it = task_profiles.find(request->id()); - if (it != task_profiles.end()) - { - task_profile = it->second; - } - else - { - assert(auto_request); - // We may have an auto-generated request - task_profile.task_id = request->id(); - task_profile.submission_time = _context->node()->now(); - task_profile.description.start_time = rmf_traffic_ros2::convert( - request->earliest_start_time()); - } - - using namespace rmf_task::requests; - - if (std::dynamic_pointer_cast( - request->description()) != nullptr) - { - auto task = rmf_fleet_adapter::tasks::make_clean( - request, - _context, - start, - a.deployment_time(), - a.state()); - - // Populate task_profile for auto-generated Clean request - if (auto_request) - { - std::shared_ptr - description = std::dynamic_pointer_cast< - const Clean::Description>(request->description()); - const auto start_waypoint = description->start_waypoint(); - const auto waypoint_name = - _context->navigation_graph().get_waypoint(start_waypoint).name(); - task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_CLEAN; - task_profile.description.clean.start_waypoint = - waypoint_name != nullptr ? *waypoint_name : ""; - } - task->task_profile(task_profile); - - _queue.push_back(task); - } - - else if (std::dynamic_pointer_cast( - request->description()) != nullptr) - { - const auto task = tasks::make_charge_battery( - request, - _context, - start, - a.deployment_time(), - a.state()); - - // Populate task_profile for auto-generated ChargeBattery request - if (auto_request) - { - task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; - } - task->task_profile(task_profile); - - _queue.push_back(task); - } - - else if (std::dynamic_pointer_cast( - request->description()) != nullptr) - { - const auto task = tasks::make_delivery( - request, - _context, - start, - a.deployment_time(), - a.state(), - task_profile.description.delivery); - - // Populate task_profile for auto-generated Delivery request - if (auto_request) - { - std::shared_ptr - description = std::dynamic_pointer_cast< - const Delivery::Description>(request->description()); - const auto& graph = _context->navigation_graph(); - const auto pickup_waypoint = description->pickup_waypoint(); - const auto pickup_name = - graph.get_waypoint(pickup_waypoint).name(); - const auto dropoff_waypoint = description->dropoff_waypoint(); - const auto dropoff_name = - graph.get_waypoint(dropoff_waypoint).name(); - task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; - task_profile.description.delivery.pickup_place_name = - pickup_name != nullptr ? *pickup_name : ""; - task_profile.description.delivery.dropoff_place_name = - dropoff_name != nullptr ? *dropoff_name : ""; - } - task->task_profile(task_profile); + const auto top = event_queue.back(); + event_queue.pop_back(); - _queue.push_back(task); - } + auto& event_state = event_states[std::to_string(top->id())]; + event_state["id"] = top->id(); + event_state["status"] = status_to_string(top->status()); - else if (std::dynamic_pointer_cast(request-> - description()) != nullptr) - { - const auto task = tasks::make_loop( - request, - _context, - start, - a.deployment_time(), - a.state()); - - // Populate task_profile for auto-generated Loop request - if (auto_request) - { - std::shared_ptr - description = std::dynamic_pointer_cast< - const Loop::Description>(request->description()); - const auto& graph = _context->navigation_graph(); - const auto start_waypoint = description->start_waypoint(); - const auto start_name = - graph.get_waypoint(start_waypoint).name(); - const auto finish_waypoint = description->finish_waypoint(); - const auto finish_name = - graph.get_waypoint(finish_waypoint).name(); - task_profile.description.loop.num_loops = description->num_loops(); - task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_LOOP; - task_profile.description.loop.start_name = - start_name != nullptr ? *start_name : ""; - task_profile.description.loop.finish_name = - finish_name != nullptr ? *finish_name : ""; - } - task->task_profile(task_profile); + // TODO(MXG): Keep a VersionedString Reader to know when to actually update + // this string + event_state["name"] = + *rmf_task::VersionedString::Reader().read(top->name()); - _queue.push_back(task); - } + event_state["detail"] = + *rmf_task::VersionedString::Reader().read(top->detail()); - else - { - RCLCPP_WARN( - _context->node()->get_logger(), - "[TaskManager] Un-supported request type in assignment list. " - "Please update the implementation of TaskManager::set_queue() to " - "support request with task_id:[%s]", - a.request()->id().c_str()); + std::vector logs; + for (const auto& log : reader.read(top->log())) + logs.push_back(log_to_json(log)); - continue; - } + if (!logs.empty()) + event_logs[std::to_string(top->id())] = std::move(logs); - // publish queued task - TaskSummaryMsg msg; - _populate_task_summary(_queue.back(), msg.STATE_QUEUED, msg); - _context->node()->task_summary()->publish(msg); + std::vector deps; + deps.reserve(top->dependencies().size()); + for (const auto& dep : top->dependencies()) + { + event_queue.push_back(dep); + deps.push_back(dep->id()); } + + event_state["deps"] = std::move(deps); } - _begin_next_task(); + return phase_state; } //============================================================================== -const std::vector TaskManager::requests() const +void copy_phase_data( + nlohmann::json& phases, + const rmf_task::Phase::Pending& pending) { - using namespace rmf_task::requests; - std::vector requests; - requests.reserve(_queue.size()); - for (const auto& task : _queue) - { - if (task->request()->automatic()) - { - continue; - } - - requests.push_back(task->request()); - } - return requests; + const auto id = pending.tag()->id(); + auto& phase = phases[std::to_string(id)]; + phase["id"] = id; + + const auto& header = pending.tag()->header(); + phase["category"] = header.category(); + phase["detail"] = header.detail(); + phase["estimate_millis"] = + std::max(0l, to_millis(header.original_duration_estimate()).count()); } //============================================================================== -TaskManager::RobotModeMsg TaskManager::robot_mode() const +void copy_booking_data( + nlohmann::json& booking_json, + const rmf_task::Task::Booking& booking) { - const auto mode = rmf_fleet_msgs::build() - .mode(_active_task == nullptr ? - RobotModeMsg::MODE_IDLE : - _context->current_mode()) - .mode_request_id(0); + booking_json["id"] = booking.id(); + booking_json["unix_millis_earliest_start_time"] = + to_millis(booking.earliest_start_time().time_since_epoch()).count(); + // TODO(MXG): Add priority and labels +} - return mode; +//============================================================================== +void copy_assignment( + nlohmann::json& assigned_to_json, + const agv::RobotContext& context) +{ + assigned_to_json["group"] = context.group(); + assigned_to_json["name"] = context.name(); } //============================================================================== -void TaskManager::_begin_next_task() +nlohmann::json make_simple_success_response() { - if (_active_task) - return; + nlohmann::json response; + response["success"] = true; + return response; +} - std::lock_guard guard(_mutex); +} // anonymous namespace - if (_queue.empty()) +//============================================================================== +void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr) +{ + auto task_state_update = mgr._task_state_update_json; + + const auto& booking = *_task->tag()->booking(); + copy_booking_data(_state_msg["booking"], booking); + const auto& header = _task->tag()->header(); + _state_msg["category"] = header.category(); + _state_msg["detail"] = header.detail(); + // TODO(MXG): Add unix_millis_start_time and unix_millis_finish_time + _state_msg["original_estimate_millis"] = + std::max(0l, to_millis(header.original_duration_estimate()).count()); + _state_msg["estimate_millis"] = + std::max(0l, to_millis(_task->estimate_remaining_time()).count()); + copy_assignment(_state_msg["assigned_to"], *mgr._context); + _state_msg["status"] = + status_to_string(_task->status_overview()); + auto& phases = _state_msg["phases"]; + + nlohmann::json task_logs; + task_logs["task_id"] = booking.id(); + auto& phase_logs = task_logs["phases"]; + + std::vector completed_ids; + completed_ids.reserve(_task->completed_phases().size()); + for (const auto& completed : _task->completed_phases()) { - if (!_waiting) - _begin_waiting(); + const auto& snapshot = completed->snapshot(); + auto& phase = copy_phase_data( + phases, *snapshot, mgr._log_reader, phase_logs); + phase["unix_millis_start_time"] = + completed->start_time().time_since_epoch().count(); - return; - } + phase["unix_millis_finish_time"] = + completed->finish_time().time_since_epoch().count(); - if (_waiting) - { - _waiting->cancel(); - return; + completed_ids.push_back(snapshot->tag()->id()); } + _state_msg["completed"] = std::move(completed_ids); - const rmf_traffic::Time now = rmf_traffic_ros2::convert( - _context->node()->now()); - const auto next_task = _queue.front(); - // We take the minimum of the two to deal with cases where the deployment_time - // as computed by the task planner is greater than the earliest_start_time - // which is greater than now. This can happen for example if the previous task - // completed earlier than estimated. - // TODO: Reactively replan task assignments across agents in a fleet every - // time as task is completed. - const auto deployment_time = std::min( - next_task->deployment_time(), - next_task->request()->earliest_start_time()); + const auto active_phase = _task->active_phase(); + if (active_phase == nullptr) + return; + copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs); + _state_msg["active"] = active_phase->tag()->id(); - if (now >= deployment_time) + std::vector pending_ids; + pending_ids.reserve(_task->pending_phases().size()); + for (const auto& pending : _task->pending_phases()) { - // Update state in RobotContext and Assign active task - _context->current_task_end_state(_queue.front()->finish_state()); - _active_task = std::move(_queue.front()); - _queue.erase(_queue.begin()); + copy_phase_data(phases, pending); + pending_ids.push_back(pending.tag()->id()); + } + _state_msg["pending"] = std::move(pending_ids); - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning new task [%s] for [%s]. Remaining queue size: %ld", - _active_task->id().c_str(), - _context->requester_id().c_str(), - _queue.size()); + if (!_active_interruptions.empty() || !_removed_interruptions.empty()) + { + auto& interruptions_json = _state_msg["interruptions"]; + for (const auto& i : {&_active_interruptions, &_removed_interruptions}) + { + for (const auto& [token, msg] : *i) + interruptions_json[token] = msg; + } + } - _task_sub = _active_task->observe() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [me = weak_from_this(), active_task = _active_task](Task::StatusMsg msg) - { - const auto self = me.lock(); - if (!self) - return; + if (_cancellation.has_value()) + _state_msg["cancellation"] = *_cancellation; - self->_populate_task_summary(active_task, msg.state, msg); - self->_context->node()->task_summary()->publish(msg); - }, - [me = weak_from_this(), active_task = _active_task](std::exception_ptr e) - { - const auto self = me.lock(); - if (!self) - return; + if (_killed.has_value()) + _state_msg["killed"] = *_killed; - TaskSummaryMsg msg; + for (const auto& [phase, skip_info] : _skip_info_map) + { + auto& skip_requests = phases[phase]["skip_requests"]; + for (const auto& s : {&skip_info.active_skips, &skip_info.removed_skips}) + { + for (const auto& [token, msg] : *s) + skip_requests[token] = msg; + } + } - try - { - std::rethrow_exception(e); - } - catch (const std::exception& e) - { - msg.status = e.what(); - } + task_state_update["data"] = _state_msg; - self->_populate_task_summary(active_task, msg.STATE_FAILED, msg); - self->_context->node()->task_summary()->publish(msg); - }, - [me = weak_from_this(), active_task = _active_task]() - { - const auto self = me.lock(); - if (!self) - return; + static const auto task_update_validator = + mgr._make_validator(rmf_api_msgs::schemas::task_state_update); + mgr._validate_and_publish_websocket(task_state_update, task_update_validator); - TaskSummaryMsg msg; - self->_populate_task_summary( - active_task, msg.STATE_COMPLETED, msg); - self->_context->node()->task_summary()->publish(msg); + auto task_log_update = nlohmann::json(); + task_log_update["type"] = "task_log_update"; + task_log_update["data"] = task_logs; - self->_active_task = nullptr; - }); + static const auto log_update_validator = + mgr._make_validator(rmf_api_msgs::schemas::task_log_update); + mgr._validate_and_publish_websocket(task_log_update, log_update_validator); - _active_task->begin(); - _register_executed_task(_active_task->id()); - } - else + // Backup task state and logs + // TOOO(YV): Validate first + auto fleet_handle = mgr._fleet_handle.lock(); + if (!fleet_handle) + return; + const auto impl = agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + const std::string& robot = mgr._context->name(); + if (impl.db) { - if (!_waiting) - _begin_waiting(); + impl.db->backup_active_task(robot, _state_msg); + impl.db->backup_task_logs(booking.id(), task_logs); } + } //============================================================================== -void TaskManager::_begin_waiting() +std::string TaskManager::ActiveTask::add_interruption( + std::vector labels, + rmf_traffic::Time time, + std::function task_is_interrupted) { - // Determine the waypoint closest to the robot - std::size_t waiting_point = _context->location().front().waypoint(); - double min_dist = std::numeric_limits::max(); - const auto& robot_position = _context->position(); - for (const auto& start : _context->location()) + std::string token = std::to_string(_next_token++); + + nlohmann::json interruption_json; + interruption_json["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + interruption_json["labels"] = std::move(labels); + + _active_interruptions[token] = std::move(interruption_json); + + if (_resume_task.has_value()) { - const auto waypoint = start.waypoint(); - const auto& waypoint_location = - _context->navigation_graph().get_waypoint(waypoint).get_location(); - const auto dist = (robot_position.block<2, 1>(0, 0) - - waypoint_location).norm(); - if (dist < min_dist) + std::lock_guard lock(_interruption_handler->mutex); + if (_interruption_handler->is_interrupted) { - min_dist = dist; - waiting_point = waypoint; + task_is_interrupted(); + } + else + { + _interruption_handler->interruption_listeners + .push_back(std::move(task_is_interrupted)); } - } - _waiting = phases::ResponsiveWait::make_indefinite( - _context, waiting_point)->begin(); + return token; + } - _task_sub = _waiting->observe() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [me = weak_from_this()](Task::StatusMsg msg) + _resume_task = _task->interrupt( + [w = _interruption_handler->weak_from_this()]() { - const auto self = me.lock(); - if (!self) + const auto handler = w.lock(); + if (!handler) return; - self->_populate_task_summary(nullptr, msg.state, msg); - self->_context->node()->task_summary()->publish(msg); - }, - [me = weak_from_this()](std::exception_ptr e) - { - const auto self = me.lock(); - if (!self) - return; + std::lock_guard lock(handler->mutex); + handler->is_interrupted = true; + for (const auto& listener : handler->interruption_listeners) + listener(); + }); - rmf_task_msgs::msg::TaskSummary msg; + return token; +} - try - { - std::rethrow_exception(e); - } - catch (const std::exception& e) +//============================================================================== +std::vector TaskManager::ActiveTask::remove_interruption( + std::vector for_tokens, + std::vector labels, + rmf_traffic::Time time) +{ + nlohmann::json resume_json; + resume_json["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + resume_json["labels"] = std::move(labels); + + std::vector missing_tokens; + for (const auto& token : for_tokens) + { + const auto it = _active_interruptions.find(token); + if (it == _active_interruptions.end()) + { + if (_removed_interruptions.count(token) == 0) { - msg.status = e.what(); + missing_tokens.push_back(token); + continue; } + } - self->_populate_task_summary(nullptr, msg.STATE_FAILED, msg); - self->_context->node()->task_summary()->publish(msg); - - RCLCPP_WARN( - self->_context->node()->get_logger(), - "Robot [%s] encountered an error while doing a ResponsiveWait: %s", - self->_context->requester_id().c_str(), msg.status.c_str()); + auto interruption_json = it->second; + interruption_json["resumed_by"] = resume_json; + _removed_interruptions[token] = interruption_json; + _active_interruptions.erase(it); + } - // Go back to waiting if an error has occurred - self->_begin_waiting(); - }, - [me = weak_from_this()]() + if (_active_interruptions.empty()) + { + if (_resume_task.has_value()) { - const auto self = me.lock(); - if (!self) - return; + std::lock_guard lock(_interruption_handler->mutex); + _interruption_handler->is_interrupted = false; + _interruption_handler->interruption_listeners.clear(); - self->_waiting = nullptr; - self->_begin_next_task(); - }); + (*_resume_task)(); + _resume_task = std::nullopt; + } + } + + return missing_tokens; } //============================================================================== -void TaskManager::retreat_to_charger() +void TaskManager::ActiveTask::cancel( + std::vector labels, + rmf_traffic::Time time) { - { - std::lock_guard guard(_mutex); - if (_active_task || !_queue.empty()) - return; - } - - const auto task_planner = _context->task_planner(); - if (!task_planner) + if (_cancellation.has_value()) return; - if (!task_planner->configuration().constraints().drain_battery()) - return; + nlohmann::json cancellation; + cancellation["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); - const auto current_state = expected_finish_state(); - if (current_state.waypoint() == current_state.charging_waypoint()) - return; + cancellation["labels"] = std::move(labels); - const auto& constraints = task_planner->configuration().constraints(); - const double threshold_soc = constraints.threshold_soc(); - const double retreat_threshold = 1.2 * threshold_soc; // safety factor - const double current_battery_soc = _context->current_battery_soc(); + _cancellation = std::move(cancellation); + _task->cancel(); +} + +//============================================================================== +void TaskManager::ActiveTask::kill( + std::vector labels, + rmf_traffic::Time time) +{ + if (_killed.has_value()) + return; + + nlohmann::json killed; + killed["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + killed["labels"] = std::move(labels); + + _killed = std::move(killed); + _task->kill(); +} + +//============================================================================== +void TaskManager::ActiveTask::rewind(uint64_t phase_id) +{ + _task->rewind(phase_id); +} + +//============================================================================== +std::string TaskManager::ActiveTask::skip( + uint64_t phase_id, + std::vector labels, + rmf_traffic::Time time) +{ + std::string token = std::to_string(_next_token++); + + auto& skip_info = _skip_info_map[phase_id]; + + nlohmann::json skip_json; + skip_json["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + skip_json["labels"] = std::move(labels); + + skip_info.active_skips[token] = std::move(skip_json); + + _task->skip(phase_id, true); + + return token; +} + +//============================================================================== +std::vector TaskManager::ActiveTask::remove_skips( + const std::vector& for_tokens, + std::vector labels, + rmf_traffic::Time time) +{ + nlohmann::json undo_json; + undo_json["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + undo_json["labels"] = std::move(labels); + + std::vector missing_tokens; + for (const auto& token : for_tokens) + { + bool found_token = false; + for (auto& [phase, skip_info] : _skip_info_map) + { + const auto it = skip_info.active_skips.find(token); + if (it == skip_info.active_skips.end()) + continue; + + auto skip_json = it->second; + skip_json["undo"] = undo_json; + skip_info.removed_skips[token] = std::move(skip_json); + skip_info.active_skips.erase(it); + + if (skip_info.active_skips.empty()) + _task->skip(phase, false); + + found_token = true; + break; + } + + if (!found_token) + missing_tokens.push_back(token); + } + + return missing_tokens; +} + +//============================================================================== +std::optional TaskManager::current_task_id() const +{ + if (_active_task) + return _active_task.id(); + + return std::nullopt; +} + +//============================================================================== +auto TaskManager::get_queue() const -> const std::vector& +{ + return _queue; +} + +//============================================================================== +bool TaskManager::cancel_task_if_present(const std::string& task_id) +{ + if (_active_task && _active_task.id() == task_id) + { + _active_task.cancel({"DispatchRequest"}, _context->now()); + return true; + } + + std::lock_guard lock(_mutex); + for (auto it = _queue.begin(); it != _queue.end(); ++it) + { + if (it->request()->booking()->id() == task_id) + { + _queue.erase(it); + return true; + } + } + + return false; +} + +//============================================================================== +std::string TaskManager::robot_status() const +{ + if (!_active_task) + return "idle"; + + // TODO(MXG): Identify if the robot is charging and report that status here + return "working"; +} + +//============================================================================== +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()) + { + return _direct_queue.rbegin()->assignment.finish_state(); + } + + if (_active_task) + return _context->current_task_end_state(); + + return current_state; +} + +//============================================================================== +const agv::RobotContextPtr& TaskManager::context() +{ + return _context; +} + +//============================================================================== +agv::ConstRobotContextPtr TaskManager::context() const +{ + return _context; +} + +//============================================================================== +std::optional> TaskManager::broadcast_client() +const +{ + return _broadcast_client; +} + +//============================================================================== +void TaskManager::set_queue( + const std::vector& assignments) +{ + // We indent this block as _mutex is also locked in the _begin_next_task() + // function that is called at the end of this function. + { + std::lock_guard guard(_mutex); + // Do not remove automatic task if assignments is empty. See Issue #138 + if (assignments.empty() && + _queue.size() == 1 && + _queue.front().request()->booking()->automatic()) + { + return; + } + _queue = assignments; + _publish_task_queue(); + } + + // Backup queue + auto fleet_handle = _fleet_handle.lock(); + if (fleet_handle) + { + const auto& impl = + agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + impl.db->backup_task_queues(*this); + } + + _begin_next_task(); +} + +//============================================================================== +const std::vector TaskManager::requests() const +{ + using namespace rmf_task::requests; + std::vector requests; + requests.reserve(_queue.size()); + std::lock_guard lock(_mutex); + for (const auto& task : _queue) + { + if (task.request()->booking()->automatic()) + { + continue; + } + + requests.push_back(task.request()); + } + return requests; +} + +//============================================================================== +TaskManager::RobotModeMsg TaskManager::robot_mode() const +{ + const auto mode = rmf_fleet_msgs::build() + .mode(_active_task ? + RobotModeMsg::MODE_IDLE : + _context->current_mode()) + .mode_request_id(0); + + return mode; +} + +//============================================================================== +std::vector TaskManager::task_log_updates() const +{ + std::vector logs; + for (const auto& it : _task_logs) + { + nlohmann::json update_msg = _task_log_update_msg; + update_msg["data"] = it.second; + std::string error = ""; + if (_validate_json( + update_msg, rmf_api_msgs::schemas::task_log_update, error)) + { + logs.push_back(update_msg); + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "%s", error.c_str()); + } + } + return logs; +} + +//============================================================================== +void TaskManager::_begin_next_task() +{ + if (_active_task) + return; + + std::lock_guard guard(_mutex); + + if (_queue.empty() && _direct_queue.empty()) + { + if (!_waiting) + _begin_waiting(); + + return; + } + + if (_waiting) + { + _waiting->cancel(); + 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 ? + _direct_queue.begin()->assignment : + _queue.front(); + + // We take the minimum of the two to deal with cases where the deployment_time + // as computed by the task planner is greater than the earliest_start_time + // which is greater than now. This can happen for example if the previous task + // completed earlier than estimated. + // TODO: Reactively replan task assignments across agents in a fleet every + // time as task is completed. + const auto deployment_time = std::min( + assignment.request()->booking()->earliest_start_time(), + assignment.deployment_time()); + + const rmf_traffic::Time now = rmf_traffic_ros2::convert( + _context->node()->now()); + + if (now >= deployment_time) + { + // Update state in RobotContext and Assign active task + const auto& id = assignment.request()->booking()->id(); + _context->current_task_end_state(assignment.finish_state()); + _context->current_task_id(id); + _active_task = _context->task_activator()->activate( + _context->make_get_state(), + _context->task_parameters(), + *assignment.request(), + _update_cb(), + _checkpoint_cb(), + _phase_finished_cb(), + _task_finished(id)); + + if (is_next_task_direct) + _direct_queue.erase(_direct_queue.begin()); + else + _queue.erase(_queue.begin()); + + // Backup queue + auto fleet_handle = _fleet_handle.lock(); + if (fleet_handle) + { + const auto& impl = + agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + impl.db->backup_task_queues(*this); + } + + if (!_active_task) + { + const auto info = assignment.request()->description()->generate_info( + _context->make_get_state()(), *_context->task_parameters()); + RCLCPP_ERROR( + _context->node()->get_logger(), + "[rmf_fleet_adapter::TaskManager::_begin_next_task] Failed to " + "instantiate task type [%s] for ID [%s]. This indicates that the " + "fleet adapter is incorrectly configured.", + info.category.c_str(), + assignment.request()->booking()->id().c_str()); + + _context->worker().schedule( + [w = weak_from_this()](const auto&) + { + if (const auto self = w.lock()) + self->_begin_next_task(); + }); + + return; + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning new task [%s] for [%s]. Remaining queue size: %ld", + _active_task.id().c_str(), + _context->requester_id().c_str(), + _queue.size()); + + _register_executed_task(_active_task.id()); + } + else + { + if (!_waiting) + _begin_waiting(); + } +} + +//============================================================================== +void TaskManager::_begin_waiting() +{ + // Determine the waypoint closest to the robot + std::size_t waiting_point = _context->location().front().waypoint(); + double min_dist = std::numeric_limits::max(); + const auto& robot_position = _context->position(); + for (const auto& start : _context->location()) + { + const auto waypoint = start.waypoint(); + const auto& waypoint_location = + _context->navigation_graph().get_waypoint(waypoint).get_location(); + const auto dist = (robot_position.block<2, 1>(0, 0) - + waypoint_location).norm(); + if (dist < min_dist) + { + min_dist = dist; + waiting_point = waypoint; + } + } + + _waiting = phases::ResponsiveWait::make_indefinite( + _context, waiting_point)->begin(); + + _task_sub = _waiting->observe() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [me = weak_from_this()](LegacyTask::StatusMsg msg) + { + const auto self = me.lock(); + if (!self) + return; + + self->_populate_task_summary(nullptr, msg.state, msg); + self->_context->node()->task_summary()->publish(msg); + }, + [me = weak_from_this()](std::exception_ptr e) + { + const auto self = me.lock(); + if (!self) + return; + + rmf_task_msgs::msg::TaskSummary msg; + + try + { + std::rethrow_exception(e); + } + catch (const std::exception& e) + { + msg.status = e.what(); + } + + self->_populate_task_summary(nullptr, msg.STATE_FAILED, msg); + self->_context->node()->task_summary()->publish(msg); + + RCLCPP_WARN( + self->_context->node()->get_logger(), + "Robot [%s] encountered an error while doing a ResponsiveWait: %s", + self->_context->requester_id().c_str(), msg.status.c_str()); + + // Go back to waiting if an error has occurred + self->_begin_waiting(); + }, + [me = weak_from_this()]() + { + const auto self = me.lock(); + if (!self) + return; + + self->_waiting = nullptr; + self->_begin_next_task(); + }); +} + +//============================================================================== +std::function TaskManager::_make_resume_from_emergency() +{ + return [w = weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_resume_from_emergency(); + }; +} + +//============================================================================== +void TaskManager::_resume_from_emergency() +{ + _context->worker().schedule( + [w = weak_from_this()](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + + if (self->_emergency_active) + return; + + if (!self->_emergency_pullover_interrupt_token.has_value()) + return; + + self->_emergency_pullover = nullptr; + if (self->_active_task) + { + self->_active_task.remove_interruption( + {*self->_emergency_pullover_interrupt_token}, + {"emergency finished"}, + self->_context->now()); + self->_emergency_pullover_interrupt_token = std::nullopt; + } + else + { + self->_begin_next_task(); + } + }); +} + +//============================================================================== +void TaskManager::retreat_to_charger() +{ + { + std::lock_guard guard(_mutex); + if (_active_task || !_queue.empty()) + return; + } + + const auto task_planner = _context->task_planner(); + if (!task_planner) + return; + + if (!task_planner->configuration().constraints().drain_battery()) + return; + + const auto current_state = expected_finish_state(); + const auto charging_waypoint = + current_state.dedicated_charging_waypoint().value(); + if (current_state.waypoint() == charging_waypoint) + return; + + const auto& constraints = task_planner->configuration().constraints(); + const double threshold_soc = constraints.threshold_soc(); + const double retreat_threshold = 1.2 * threshold_soc; // safety factor + const double current_battery_soc = _context->current_battery_soc(); const auto& parameters = task_planner->configuration().parameters(); - auto& estimate_cache = *(task_planner->estimate_cache()); + // TODO(YV): Expose the TravelEstimator in the TaskPlanner to benefit from + // caching + const rmf_traffic::agv::Planner::Goal retreat_goal{charging_waypoint}; + const auto result = _travel_estimator->estimate( + current_state.extract_plan_start().value(), retreat_goal); + if (!result.has_value()) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Unable to compute estimate of journey back to charger for robot [%s]", + _context->name().c_str()); + return; + } + + const double battery_soc_after_retreat = + current_battery_soc - result->change_in_charge(); + + if ((battery_soc_after_retreat < retreat_threshold) && + (battery_soc_after_retreat > threshold_soc)) + { + // Add a new charging task to the task queue + const auto charging_request = rmf_task::requests::ChargeBattery::make( + current_state.time().value()); + const auto model = charging_request->description()->make_model( + current_state.time().value(), + parameters); + + const auto finish = model->estimate_finish( + current_state, + constraints, + *_travel_estimator); + + if (!finish) + return; + + rmf_task::TaskPlanner::Assignment charging_assignment( + charging_request, + finish.value().finish_state(), + current_state.time().value()); + + const DirectAssignment assignment = DirectAssignment{ + _next_sequence_number, + charging_assignment}; + ++_next_sequence_number; + { + std::lock_guard lock(_mutex); + _direct_queue.insert(assignment); + } + + auto fleet_handle = _fleet_handle.lock(); + if (fleet_handle) + { + const auto& impl = + agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + impl.db->backup_task_queues(*this); + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Initiating automatic retreat to charger for robot [%s]", + _context->name().c_str()); + } + + if ((battery_soc_after_retreat < retreat_threshold) && + (battery_soc_after_retreat < threshold_soc)) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Robot [%s] needs to be charged but has insufficient battery remaining " + "to retreat to its designated charger.", + _context->name().c_str()); + } +} + +//============================================================================== +const std::vector& TaskManager::get_executed_tasks() const +{ + return _executed_task_registry; +} + +//============================================================================== +void TaskManager::_register_executed_task(const std::string& id) +{ + // Currently the choice of storing 100 executed tasks is arbitrary. + // TODO: Save a time stamp for when tasks are completed and cull entries after + // a certain time window instead. + if (_executed_task_registry.size() >= 100) + _executed_task_registry.erase(_executed_task_registry.begin()); + + _executed_task_registry.push_back(id); +} + +//============================================================================== +void TaskManager::_populate_task_summary( + std::shared_ptr task, + uint32_t task_summary_state, + TaskManager::TaskSummaryMsg& msg) +{ + if (task == nullptr) // ResponsiveWait + { + msg.task_id = _context->requester_id() + ":waiting"; + + msg.start_time = _context->node()->now(); + msg.end_time = _queue.empty() ? msg.start_time : rmf_traffic_ros2::convert( + _queue.front().deployment_time()); + // Make the task type explicit + msg.task_profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_STATION; + } + + else + { + msg.task_id = task->id(); + msg.start_time = rmf_traffic_ros2::convert( + task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + task->finish_state().time().value()); + msg.task_profile = task->task_profile(); + } + + msg.fleet_name = _context->description().owner(); + msg.robot_name = _context->name(); + + msg.state = task_summary_state; +} + +//============================================================================== +void TaskManager::_schema_loader( + const nlohmann::json_uri& id, nlohmann::json& value) const +{ + const auto it = _schema_dictionary.find(id.url()); + if (it == _schema_dictionary.end()) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "[TaskManager] url: %s not found in schema dictionary", id.url().c_str()); + return; + } + + value = it->second; +} + +//============================================================================== +void TaskManager::_validate_and_publish_websocket( + const nlohmann::json& msg, + const nlohmann::json_schema::json_validator& validator) const +{ + std::string error = ""; + if (!_validate_json(msg, validator, error)) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Failed to validate message [%s]: [%s]", + msg.dump().c_str(), + error.c_str()); + return; + } + + if (!_broadcast_client.has_value()) + return; + + const auto client = _broadcast_client->lock(); + if (!client) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Unable to lock BroadcastClient within TaskManager of robot [%s]", + _context->name().c_str()); + return; + } + client->publish(msg); +} + +//============================================================================== +void TaskManager::_validate_and_publish_api_response( + const nlohmann::json& response, + const nlohmann::json_schema::json_validator& validator, + const std::string& request_id) +{ + std::string error; + if (!_validate_json(response, validator, error)) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Error in response to [%s]: %s", + request_id.c_str(), + error.c_str()); + return; + } + + _context->node()->task_api_response()->publish( + rmf_task_msgs::build() + .type(rmf_task_msgs::msg::ApiResponse::TYPE_RESPONDING) + .json_msg(response.dump()) + .request_id(request_id)); +} + +//============================================================================== +rmf_task::State TaskManager::_get_state() const +{ + return _context->current_task_end_state(); +} + +//============================================================================== +void TaskManager::_consider_publishing_updates() +{ + const auto now = std::chrono::steady_clock::now(); + const auto time_elapsed = now - _last_update_time; + // TODO(MXG): Make max elapsed time configurable + const auto max_time_elapsed = std::chrono::seconds(1); + if (_task_state_update_available || time_elapsed > max_time_elapsed) + { + _task_state_update_available = false; + _last_update_time = now; + _publish_task_state(); + } +} + +//============================================================================== +void TaskManager::_publish_task_state() +{ + if (!_active_task) + return; + + _active_task.publish_task_state(*this); +} + +//============================================================================== +void TaskManager::_publish_task_queue() +{ + rmf_task::State expected_state = _context->current_task_end_state(); + const auto& parameters = *_context->task_parameters(); + static const auto validator = + _make_validator(rmf_api_msgs::schemas::task_state_update); + + for (const auto& pending : _queue) + { + const auto info = pending.request()->description()->generate_info( + expected_state, parameters); + + nlohmann::json pending_json; + const auto& booking = *pending.request()->booking(); + copy_booking_data(pending_json["booking"], booking); + + pending_json["category"] = info.category; + pending_json["detail"] = info.detail; + + const auto estimate = + pending.finish_state().time().value() - pending.deployment_time(); + pending_json["original_estimate_millis"] = + std::max(0l, to_millis(estimate).count()); + copy_assignment(pending_json["assigned_to"], *_context); + pending_json["status"] = "queued"; + + auto task_state_update = _task_state_update_json; + task_state_update["data"] = pending_json; + + _validate_and_publish_websocket(task_state_update, validator); + + expected_state = pending.finish_state(); + } +} + +//============================================================================== +bool TaskManager::_validate_json( + const nlohmann::json& json, + const nlohmann::json_schema::json_validator& validator, + std::string& error) const +{ + try + { + validator.validate(json); + } + catch (const std::exception& e) + { + error = e.what(); + return false; + } + + return true; +} + +//============================================================================== +bool TaskManager::_validate_request_message( + const nlohmann::json& request_json, + const nlohmann::json_schema::json_validator& request_validator, + const std::string& request_id) +{ + std::string error; + if (_validate_json(request_json, request_validator, error)) + return true; - double retreat_battery_drain = 0.0; - const auto endpoints = std::make_pair(current_state.waypoint(), - current_state.charging_waypoint()); - const auto& cache_result = estimate_cache.get(endpoints); + _send_simple_error_response( + request_id, 5, "Invalid request format", std::move(error)); + return false; +} + +//============================================================================== +void TaskManager::_send_simple_success_response(const std::string& request_id) +{ + static const auto response = make_simple_success_response(); + + static const auto simple_response_validator = + _make_validator(rmf_api_msgs::schemas::simple_response); + + _validate_and_publish_api_response( + response, simple_response_validator, request_id); +} + +//============================================================================== +void TaskManager::_send_token_success_response( + std::string token, + const std::string& request_id) +{ + nlohmann::json response; + response["success"] = true; + response["token"] = std::move(token); + + static const auto token_response_validator = + _make_validator(rmf_api_msgs::schemas::token_response); + + _validate_and_publish_api_response( + response, token_response_validator, request_id); +} + +//============================================================================== +nlohmann::json_schema::json_validator TaskManager::_make_validator( + const nlohmann::json& schema) const +{ + return nlohmann::json_schema::json_validator( + schema, + [w = weak_from_this()](const nlohmann::json_uri& id, + nlohmann::json& value) + { + const auto self = w.lock(); + if (!self) + return; + self->_schema_loader(id, value); + }); +} + +//============================================================================== +void TaskManager::_send_simple_error_response( + const std::string& request_id, + uint64_t code, + std::string category, + std::string detail) +{ + static const auto error_validator = + _make_validator(rmf_api_msgs::schemas::simple_response); - if (cache_result) + _validate_and_publish_api_response( + _make_error_response(code, std::move(category), std::move(detail)), + error_validator, + request_id); +} + +//============================================================================== +void TaskManager::_send_simple_error_if_queued( + const std::string& task_id, + const std::string& request_id, + const std::string& type) +{ + // TODO(YV): We could cache the task_ids of direct and dispatched tasks in + // unordered_sets and perform a lookup to see which queue to iterate. + std::lock_guard lock(_mutex); + for (const auto& a : _queue) { - retreat_battery_drain = cache_result->dsoc; + if (a.request()->booking()->id() == task_id) + { + return _send_simple_error_response( + request_id, 6, "Invalid Circumstances", + type + " a task that is queued (not yet active) " + "is not currently supported"); + } } - else + + for (const auto& a : _direct_queue) { - const rmf_traffic::agv::Planner::Goal retreat_goal{ - current_state.charging_waypoint()}; - const auto result_to_charger = parameters.planner()->plan( - current_state.location(), retreat_goal); - - // We assume we can always compute a plan - double dSOC_motion = 0.0; - double dSOC_device = 0.0; - rmf_traffic::Duration retreat_duration = rmf_traffic::Duration{0}; - rmf_traffic::Time itinerary_start_time = current_state.finish_time(); - - for (const auto& itinerary : result_to_charger->get_itinerary()) - { - const auto& trajectory = itinerary.trajectory(); - const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration itinerary_duration = - finish_time - itinerary_start_time; - - dSOC_motion = - parameters.motion_sink()->compute_change_in_charge( - trajectory); - dSOC_device = - parameters.ambient_sink()->compute_change_in_charge( - rmf_traffic::time::to_seconds(itinerary_duration)); - retreat_battery_drain += dSOC_motion + dSOC_device; - retreat_duration += itinerary_duration; - itinerary_start_time = finish_time; + if (a.assignment.request()->booking()->id() == task_id) + { + return _send_simple_error_response( + request_id, 6, "Invalid Circumstances", + type + " a task that is queued (not yet active) " + "is not currently supported"); } - estimate_cache.set(endpoints, retreat_duration, - retreat_battery_drain); } +} - const double battery_soc_after_retreat = - current_battery_soc - retreat_battery_drain; +//============================================================================== +nlohmann::json TaskManager::_make_error_response( + uint64_t code, + std::string category, + std::string detail) +{ + nlohmann::json response; + response["success"] = false; - if ((battery_soc_after_retreat < retreat_threshold) && - (battery_soc_after_retreat > threshold_soc)) - { - // Add a new charging task to the task queue - const auto charging_request = rmf_task::requests::ChargeBattery::make( - current_state.finish_time()); - const auto model = charging_request->description()->make_model( - current_state.finish_time(), - parameters); + nlohmann::json error; + error["code"] = code; + error["category"] = std::move(category); + error["detail"] = std::move(detail); - const auto finish = model->estimate_finish( - current_state, - constraints, - estimate_cache); + response["errors"] = std::vector({std::move(error)}); - if (!finish) + return response; +} + +//============================================================================== +std::function +TaskManager::_update_cb() +{ + return [w = weak_from_this()](rmf_task::Phase::ConstSnapshotPtr) + { + const auto self = w.lock(); + if (!self) + return; + + self->_task_state_update_available = true; + // TODO(MXG): Use this callback to make the state updates more efficient + }; +} + +//============================================================================== +std::function +TaskManager::_checkpoint_cb() +{ + return [w = weak_from_this()](rmf_task::Task::Active::Backup backup) + { + const auto self = w.lock(); + if (!self) return; - rmf_task::agv::TaskPlanner::Assignment charging_assignment( - charging_request, - finish.value().finish_state(), - current_state.finish_time()); + // TODO(MXG): Save the backup + }; +} - set_queue({charging_assignment}); +//============================================================================== +std::function +TaskManager::_phase_finished_cb() +{ + return [w = weak_from_this()](rmf_task::Phase::ConstCompletedPtr) + { + const auto self = w.lock(); + if (!self) + return; - RCLCPP_INFO( + self->_task_state_update_available = true; + // TODO(MXG): Use this callback to make the state updates more efficient + }; +} + +//============================================================================== +std::function TaskManager::_task_finished(std::string id) +{ + return [w = weak_from_this(), id]() + { + const auto self = w.lock(); + if (!self) + return; + + // Publish the final state of the task before destructing it + self->_publish_task_state(); + self->_active_task = ActiveTask(); + + self->_context->worker().schedule( + [w = self->weak_from_this()](const auto&) + { + if (const auto self = w.lock()) + self->_begin_next_task(); + }); + }; +} + +//============================================================================== +void TaskManager::_handle_request( + const std::string& request_msg, + const std::string& request_id) +{ + nlohmann::json request_json; + try + { + request_json = nlohmann::json::parse(request_msg); + } + catch(const std::exception& e) + { + RCLCPP_ERROR( _context->node()->get_logger(), - "Initiating automatic retreat to charger for robot [%s]", - _context->name().c_str()); + "Error parsing json_msg: %s", + e.what()); + return; } - if ((battery_soc_after_retreat < retreat_threshold) && - (battery_soc_after_retreat < threshold_soc)) + const auto type_it = request_json.find("type"); + if (type_it == request_json.end()) + return; + + const auto& type = type_it.value(); + if (!type.is_string()) + return; + + try + { + const auto& type_str = type.get(); + if (type_str == "cancel_task_request") + _handle_cancel_request(request_json, request_id); + else if (type_str == "kill_task_request") + _handle_kill_request(request_json, request_id); + else if (type_str == "interrupt_task_request") + _handle_interrupt_request(request_json, request_id); + else if (type_str == "resume_task_request") + _handle_resume_request(request_json, request_id); + else if (type_str == "rewind_task_request") + _handle_rewind_request(request_json, request_id); + else if (type_str == "skip_phase_request") + _handle_skip_phase_request(request_json, request_id); + else if (type_str == "undo_phase_skip_request") + _handle_undo_skip_phase_request(request_json, request_id); + else if (type_str == "robot_task_request") + _handle_direct_request(request_json, request_id); + else + return; + } + catch (const std::exception& e) { RCLCPP_WARN( _context->node()->get_logger(), - "Robot [%s] needs to be charged but has insufficient battery remaining " - "to retreat to its designated charger.", - _context->name().c_str()); + "Encountered exception while handling a request: %s", e.what()); } } +namespace { //============================================================================== -const std::vector& TaskManager::get_executed_tasks() const +std::vector get_labels(const nlohmann::json& request) { - return _executed_task_registry; + const auto labels_it = request.find("labels"); + if (labels_it != request.end()) + return labels_it->get>(); + + return {}; } //============================================================================== -void TaskManager::_register_executed_task(const std::string& id) +bool remove_task_from_queue( + const std::string& task_id, + std::vector& queue) { - // Currently the choice of storing 100 executed tasks is arbitrary. - // TODO: Save a time stamp for when tasks are completed and cull entries after - // a certain time window instead. - if (_executed_task_registry.size() >= 100) - _executed_task_registry.erase(_executed_task_registry.begin()); + // If the task is queued, then we should make sure to remove it from the + // queue, just in case it reaches an active state before the dispatcher + // issues its cancellation request. + // + // TODO(MXG): We should do a much better of job of coordinating these + // different moving parts in the system. E.g. who is ultimately responsible + // for issuing the response to the request or updating the task state? + for (auto it = queue.begin(); it != queue.end(); ++it) + { + if (it->request()->booking()->id() == task_id) + { + queue.erase(it); + return true; + } + } + return false; +} - _executed_task_registry.push_back(id); +//============================================================================== +bool remove_task_from_queue( + const std::string& task_id, + TaskManager::DirectQueue& queue) +{ + // If the task is queued, then we should make sure to remove it from the + // queue, just in case it reaches an active state before the dispatcher + // issues its cancellation request. + // + // TODO(MXG): We should do a much better of job of coordinating these + // different moving parts in the system. E.g. who is ultimately responsible + // for issuing the response to the request or updating the task state? + for (auto it = queue.begin(); it != queue.end(); ++it) + { + if (it->assignment.request()->booking()->id() == task_id) + { + queue.erase(it); + return true; + } + } + return false; } +} // namespace anonymous //============================================================================== -void TaskManager::_populate_task_summary( - std::shared_ptr task, - uint32_t task_summary_state, - TaskManager::TaskSummaryMsg& msg) +void TaskManager::_handle_direct_request( + const nlohmann::json& request_json, + const std::string& request_id) { - if (task == nullptr) // ResponsiveWait + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::robot_task_request); + + static const auto response_validator = + _make_validator(rmf_api_msgs::schemas::robot_task_response); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& robot = request_json["robot"].get(); + if (robot.empty() || robot != _context->name()) + return; + + const auto& fleet = request_json["fleet"].get(); + if (fleet.empty() || fleet != _context->group()) + return; + + const nlohmann::json& request = request_json["request"]; + auto fleet_handle = _fleet_handle.lock(); + if (!fleet_handle) + return; + auto& impl = + agv::FleetUpdateHandle::Implementation::get(*fleet_handle); + std::vector errors; + const auto new_request = impl.convert(request_id, request, errors); + if (!new_request) { - msg.task_id = _context->requester_id() + ":waiting"; + RCLCPP_ERROR( + _context->node()->get_logger(), + "Unable to generate a valid request for direct task [%s]", + request_id.c_str()); - msg.start_time = _context->node()->now(); - msg.end_time = _queue.empty() ? msg.start_time : rmf_traffic_ros2::convert( - _queue.front()->deployment_time()); - // Make the task type explicit - msg.task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; + nlohmann::json response_json; + response_json["success"] = false; + std::vector json_errors = {}; + for (const auto& e : errors) + json_errors.push_back(nlohmann::json::parse(e)); + response_json["errors"] = std::move(json_errors); + + _validate_and_publish_api_response( + response_json, + response_validator, + request_id); + + return; + } + + // Cache for backups + impl.task_request_jsons.insert({request_id, request}); + + // Generate Assignment for the request + const auto task_planner = _context->task_planner(); + if (!task_planner) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "Fleet [%s] is not configured with parameters for task planning." + "Use FleetUpdateHandle::set_task_planner_params(~) to set the " + "parameters required.", fleet.c_str()); + return; } + const auto current_state = expected_finish_state(); + const auto& constraints = task_planner->configuration().constraints(); + const auto& parameters = task_planner->configuration().parameters(); + const auto model = new_request->description()->make_model( + new_request->booking()->earliest_start_time(), + parameters); + const auto estimate = model->estimate_finish( + current_state, + constraints, + *_travel_estimator); + + rmf_task::State finish_state; + rmf_traffic::Time deployment_time; + + if (!estimate.has_value()) + { + RCLCPP_WARN( + _context->node()->get_logger(), + "Unable to estimate final state for direct task request [%s]. This may " + "be due to insufficient resources to perform the task. The task will be " + "still be added to the queue.", + request_id.c_str()); + finish_state = current_state; + deployment_time = new_request->booking()->earliest_start_time(); + } else { - msg.task_id = task->id(); - msg.start_time = rmf_traffic_ros2::convert( - task->deployment_time()); - msg.end_time = rmf_traffic_ros2::convert( - task->finish_state().finish_time()); - msg.task_profile = task->task_profile(); + finish_state = estimate.value().finish_state(); + deployment_time = estimate.value().wait_until(); } - msg.fleet_name = _context->description().owner(); - msg.robot_name = _context->name(); + const DirectAssignment assignment = DirectAssignment{ + _next_sequence_number, + Assignment( + new_request, + finish_state, + deployment_time) + }; + ++_next_sequence_number; + { + std::lock_guard lock(_mutex); + _direct_queue.insert(assignment); + } - msg.state = task_summary_state; + // Backup direct queue + impl.db->backup_task_queues(*this); + + RCLCPP_INFO( + _context->node()->get_logger(), + "Direct request [%s] successfully queued for robot [%s]", + request_id.c_str(), + robot.c_str()); + + // Publish api response + nlohmann::json response_json; + response_json["success"] = true; + nlohmann::json task_state;; + copy_booking_data(task_state["booking"], *new_request->booking()); + // task_state["category"] = request["category"].get(); + task_state["detail"] = request["description"]; + task_state["status"] = "queued"; + auto& dispatch = task_state["dispatch"]; + dispatch["status"] = "queued"; + auto& assign = task_state["assigned_to"]; + assign["group"] = fleet; + assign["name"] = robot; + response_json["state"] = task_state; + + _validate_and_publish_api_response( + response_json, + response_validator, + request_id); +} + +//============================================================================== +void TaskManager::_handle_cancel_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::cancel_task_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["task_id"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _active_task.cancel(get_labels(request_json), _context->now()); + _task_state_update_available = true; + return _send_simple_success_response(request_id); + } + // TODO(YV): We could cache the task_ids of direct and dispatched tasks in + // unordered_sets and perform a lookup to see which function to call. + std::lock_guard lock(_mutex); + if (!remove_task_from_queue(task_id, _queue)) + remove_task_from_queue(task_id, _direct_queue); +} + +//============================================================================== +void TaskManager::_handle_kill_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::kill_task_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["task_id"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + _active_task.kill(get_labels(request_json), _context->now()); + return _send_simple_success_response(request_id); + } + + // TODO(YV): We could cache the task_ids of direct and dispatched tasks in + // unordered_sets and perform a lookup to see which function to call. + std::lock_guard lock(_mutex); + if (!remove_task_from_queue(task_id, _queue)) + remove_task_from_queue(task_id, _direct_queue); } +//============================================================================== +void TaskManager::_handle_interrupt_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::interrupt_task_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["task_id"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + return _send_token_success_response( + _active_task.add_interruption( + get_labels(request_json), _context->now(), [](){}), + request_id); + } + + _send_simple_error_if_queued(task_id, request_id, "Interrupting"); +} + +//============================================================================== +void TaskManager::_handle_resume_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::resume_task_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["for_tokens"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + auto unknown_tokens = _active_task.remove_interruption( + request_json["for_tokens"].get>(), + get_labels(request_json), + _context->now()); + + if (unknown_tokens.empty()) + return _send_simple_success_response(request_id); + + std::string detail = "["; + for (std::size_t i=0; i < unknown_tokens.size(); ++i) + { + detail += unknown_tokens[i]; + if (i < unknown_tokens.size()-1) + detail += ", "; + } + detail += "]"; + + return _send_simple_error_response( + request_id, 7, "Unknown Tokens", std::move(detail)); + } + + _send_simple_error_if_queued(task_id, request_id, "Resuming"); +} + +//============================================================================== +void TaskManager::_handle_rewind_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::rewind_task_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["task_id"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + _active_task.rewind(request_json["phase_id"].get()); + return _send_simple_success_response(request_id); + } + + _send_simple_error_if_queued(task_id, request_id, "Rewinding"); +} + +//============================================================================== +void TaskManager::_handle_skip_phase_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::skip_phase_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["task_id"].get(); + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + return _send_token_success_response( + _active_task.skip( + request_json["phase_id"].get(), + get_labels(request_json), + _context->now()), + request_id); + } + + _send_simple_error_if_queued(task_id, request_id, "Skipping a phase in "); +} + +//============================================================================== +void TaskManager::_handle_undo_skip_phase_request( + const nlohmann::json& request_json, + const std::string& request_id) +{ + static const auto request_validator = + _make_validator(rmf_api_msgs::schemas::undo_skip_phase_request); + + if (!_validate_request_message(request_json, request_validator, request_id)) + return; + + const auto& task_id = request_json["for_task"]; + + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + auto unknown_tokens = _active_task.remove_skips( + request_json["for_tokens"].get>(), + get_labels(request_json), + _context->now()); + + if (unknown_tokens.empty()) + return _send_simple_success_response(request_id); + + std::string detail = "["; + for (std::size_t i=0; i < unknown_tokens.size(); ++i) + { + detail += unknown_tokens[i]; + if (i < unknown_tokens.size()-1) + detail += ", "; + } + detail += "]"; + + return _send_simple_error_response( + request_id, 7, "Unknown Tokens", std::move(detail)); + } + + _send_simple_error_if_queued(task_id, request_id, "Undoing a phase skip in "); +} } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index f6c764dc5..6b92af73e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -18,17 +18,25 @@ #ifndef SRC__RMF_FLEET_ADAPTER__TASKMANAGER_HPP #define SRC__RMF_FLEET_ADAPTER__TASKMANAGER_HPP -#include "Task.hpp" +#include "LegacyTask.hpp" #include "agv/RobotContext.hpp" +#include "BroadcastClient.hpp" #include -#include +#include +#include + +#include #include #include +#include +#include + #include +#include namespace rmf_fleet_adapter { @@ -42,37 +50,84 @@ class TaskManager : public std::enable_shared_from_this { public: - static std::shared_ptr make(agv::RobotContextPtr context); + static std::shared_ptr make( + agv::RobotContextPtr context, + std::optional> broadcast_client, + std::weak_ptr fleet_handle); using Start = rmf_traffic::agv::Plan::Start; using StartSet = rmf_traffic::agv::Plan::StartSet; - using Assignment = rmf_task::agv::TaskPlanner::Assignment; - using State = rmf_task::agv::State; + using Assignment = rmf_task::TaskPlanner::Assignment; + using State = rmf_task::State; using RobotModeMsg = rmf_fleet_msgs::msg::RobotMode; using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile; using TaskProfiles = std::unordered_map; using TaskSummaryMsg = rmf_task_msgs::msg::TaskSummary; - /// The location where we expect this robot to be at the end of its current - /// task queue. - StartSet expected_finish_location() const; + struct DirectAssignment + { + std::size_t sequence_number; + Assignment assignment; + + DirectAssignment( + std::size_t sequence_number_, + Assignment assignment_) + : sequence_number(sequence_number_), + assignment(std::move(assignment_)) + { + // Do nothing + } + + }; + + struct DirectQueuePriority + { + bool operator()(const DirectAssignment& a, const DirectAssignment& b) const + { + // Sort by start time and then sequence_number if tie + const auto start_time_b = + b.assignment.request()->booking()->earliest_start_time(); + const auto start_time_a = + a.assignment.request()->booking()->earliest_start_time(); + + if (start_time_b == start_time_a) + { + return b.sequence_number < a.sequence_number; + } + + return start_time_b < start_time_a; + } + }; + + using DirectQueue = std::set< + DirectAssignment, + DirectQueuePriority>; const agv::RobotContextPtr& context(); agv::ConstRobotContextPtr context() const; - const Task* current_task() const; + std::optional> broadcast_client() const; /// Set the queue for this task manager with assignments generated from the /// task planner - void set_queue( - const std::vector& assignments, - const TaskProfiles& task_profiles = {}); + void set_queue(const std::vector& assignments); /// Get the non-charging requests among pending tasks const std::vector requests() const; - /// The state of the robot. + std::optional current_task_id() const; + + const std::vector& get_queue() const; + + bool cancel_task_if_present(const std::string& task_id); + + std::string robot_status() const; + + /// The state of the robot. If the direct assignment queue is not empty, + /// This will always return the state of the robot after completing all + /// the tasks in the direct assignment queue. Otherwise, the current state + /// is returned State expected_finish_state() const; /// Callback for the retreat timer. Appends a charging task to the task queue @@ -85,13 +140,111 @@ class TaskManager : public std::enable_shared_from_this RobotModeMsg robot_mode() const; + /// Get a vector of task logs that are validated against the schema + std::vector task_log_updates() const; + private: - TaskManager(agv::RobotContextPtr context); + TaskManager( + agv::RobotContextPtr context, + std::optional> broadcast_client, + std::weak_ptr); + + class ActiveTask + { + public: + ActiveTask(rmf_task::Task::ActivePtr task = nullptr); + + const std::string& id() const; + + void publish_task_state(TaskManager& mgr); + + operator bool() const + { + return static_cast(_task); + } + + /// Adds an interruption + std::string add_interruption( + std::vector labels, + rmf_traffic::Time time, + std::function task_is_interrupted); + + // Any unknown tokens that were included will be returned + std::vector remove_interruption( + std::vector for_tokens, + std::vector labels, + rmf_traffic::Time time); + + void cancel( + std::vector labels, + rmf_traffic::Time time); + + void kill( + std::vector labels, + rmf_traffic::Time time); + + void rewind(uint64_t phase_id); + + std::string skip( + uint64_t phase_id, + std::vector labels, + rmf_traffic::Time time); + + std::vector remove_skips( + const std::vector& for_tokens, + std::vector labels, + rmf_traffic::Time time); + + private: + rmf_task::Task::ActivePtr _task; + nlohmann::json _state_msg; + + std::unordered_map _active_interruptions; + std::unordered_map _removed_interruptions; + std::optional _resume_task; + + struct InterruptionHandler + : public std::enable_shared_from_this + { + std::mutex mutex; + std::vector> interruption_listeners; + bool is_interrupted = false; + }; + + std::shared_ptr _interruption_handler; + + std::optional _cancellation; + std::optional _killed; + + struct SkipInfo + { + std::unordered_map active_skips; + std::unordered_map removed_skips; + }; + + std::unordered_map _skip_info_map; + + uint64_t _next_token = 0; + }; + + friend class ActiveTask; + friend class DatabaseLogger; agv::RobotContextPtr _context; - std::shared_ptr _active_task; - std::vector> _queue; + std::optional> _broadcast_client; + std::weak_ptr _fleet_handle; + rmf_task::ConstActivatorPtr _task_activator; + ActiveTask _active_task; + bool _emergency_active = false; + std::optional _emergency_pullover_interrupt_token; + rmf_task_sequence::Event::ActivePtr _emergency_pullover; + // Queue for dispatched tasks + std::vector _queue; + // An ID to keep track of the FIFO order of direct tasks + std::size_t _next_sequence_number; + // Queue for directly assigned tasks + DirectQueue _direct_queue; rmf_utils::optional _expected_finish_location; rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; @@ -99,34 +252,194 @@ class TaskManager : public std::enable_shared_from_this /// This phase will kick in automatically when no task is being executed. It /// will ensure that the agent continues to respond to traffic negotiations so /// it does not become a blocker for other traffic participants. - std::shared_ptr _waiting; + std::shared_ptr _waiting; // TODO: Eliminate the need for a mutex by redesigning the use of the task // manager so that modifications of shared data only happen on designated // rxcpp worker - std::mutex _mutex; + mutable std::mutex _mutex; rclcpp::TimerBase::SharedPtr _task_timer; rclcpp::TimerBase::SharedPtr _retreat_timer; + rclcpp::TimerBase::SharedPtr _update_timer; + bool _task_state_update_available = true; + std::chrono::steady_clock::time_point _last_update_time; // Container to keep track of tasks that have been started by this TaskManager // Use the _register_executed_task() to populate this container. std::vector _executed_task_registry; + // TravelEstimator for caching travel estimates for automatic charging + // retreat. TODO(YV): Expose the TaskPlanner's TravelEstimator. + std::shared_ptr _travel_estimator; + + // Map schema url to schema for validator. + // TODO: Get this and loader from FleetUpdateHandle + std::unordered_map _schema_dictionary = {}; + + rmf_rxcpp::subscription_guard _task_request_api_sub; + + // Constant jsons with validated schemas for internal use + // TODO(YV): Replace these with codegen tools + const nlohmann::json _task_log_update_msg = + {{"type", "task_log_update"}, {"data", {}}}; + const nlohmann::json _task_log_json = + {{"task_id", {}}, {"log", {}}, {"phases", {{"log", {}}, {"events, {}"}}}}; + const nlohmann::json _task_state_update_json = + {{"type", "task_state_update"}, {"data", {}}}; + const nlohmann::json _task_state_json = + {{"booking", {}}, {"category", {}}, {"detail", {}}, + {"unix_millis_start_time", {}}, {"unix_millis_finish_time", {}}, + {"estimate_millis", {}}, {"phases", {}}, {"completed", {}}, {"active", {}}, + {"pending", {}}, {"interruptions", {}}, {"cancellation", {}}, + {"killed", {}}}; + + rmf_task::Log::Reader _log_reader; + + // Map task_id to task_log.json for all tasks managed by this TaskManager + std::unordered_map _task_logs = {}; + /// Callback for task timer which begins next task if its deployment time has passed void _begin_next_task(); /// Begin responsively waiting for the next task void _begin_waiting(); + /// Make the callback for resuming + std::function _make_resume_from_emergency(); + + /// Resume whatever the task manager should be doing + void _resume_from_emergency(); + + /// Get the current state of the robot + rmf_task::State _get_state() const; + + /// Check whether publishing should happen + void _consider_publishing_updates(); + + /// Publish the current task state + void _publish_task_state(); + + /// Publish the current pending task list + void _publish_task_queue(); + + /// Schema loader for validating jsons + void _schema_loader( + const nlohmann::json_uri& id, nlohmann::json& value) const; + + /// Returns true if json is valid. + // TODO: Move this into a utils? + bool _validate_json( + const nlohmann::json& json, + const nlohmann::json_schema::json_validator& validator, + std::string& error) const; + + /// If the request message is valid this will return true. If it is not valid, + /// this will publish an error message for this request and return false. The + /// caller should not attempt to process this request or respond to it any + /// further. + bool _validate_request_message( + const nlohmann::json& request, + const nlohmann::json_schema::json_validator& validator, + const std::string& request_id); + + void _send_simple_success_response( + const std::string& request_id); + + void _send_token_success_response( + std::string token, + const std::string& request_id); + + /// Make a validator for the given schema + nlohmann::json_schema::json_validator _make_validator( + const nlohmann::json& schema) const; + + void _send_simple_error_response( + const std::string& request_id, + uint64_t code, + std::string category, + std::string detail); + + void _send_simple_error_if_queued( + const std::string& task_id, + const std::string& request_id, + const std::string& type); + + /// Make an error message to return + static nlohmann::json _make_error_response( + uint64_t code, + std::string category, + std::string detail); + + /// Validate and publish a json. This can be used for task + /// state and log updates + void _validate_and_publish_websocket( + const nlohmann::json& msg, + const nlohmann::json_schema::json_validator& validator) const; + + /// Validate and publish a response message over the ROS2 API response topic + void _validate_and_publish_api_response( + const nlohmann::json& msg, + const nlohmann::json_schema::json_validator& validator, + const std::string& request_id); + + /// Callback for when the task has a significat update + std::function _update_cb(); + + /// Callback for when the task reaches a checkpoint + std::function _checkpoint_cb(); + + /// Callback for when a phase within a task has finished + std::function _phase_finished_cb(); + + /// Callback for when the task has finished + std::function _task_finished(std::string id); + /// Function to register the task id of a task that has begun execution /// The input task id will be inserted into the registry such that the max /// size of the registry is 100. void _register_executed_task(const std::string& id); void _populate_task_summary( - std::shared_ptr task, + std::shared_ptr task, uint32_t task_summary_state, TaskSummaryMsg& msg); + + void _handle_request( + const std::string& request_msg, + const std::string& request_id); + + void _handle_direct_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_cancel_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_kill_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_interrupt_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_resume_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_rewind_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_skip_phase_request( + const nlohmann::json& request_json, + const std::string& request_id); + + void _handle_undo_skip_phase_request( + const nlohmann::json& request_json, + const std::string& request_id); + }; using TaskManagerPtr = std::shared_ptr; 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 8fd019665..140d4d9e3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -31,6 +31,8 @@ #include "../load_param.hpp" +#include "../DatabaseLogger.hpp" + namespace rmf_fleet_adapter { namespace agv { @@ -208,7 +210,9 @@ std::shared_ptr Adapter::make( std::shared_ptr Adapter::add_fleet( const std::string& fleet_name, rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph) + rmf_traffic::agv::Graph navigation_graph, + std::optional server_uri, + std::optional backup_file_path) { auto planner = std::make_shared>( @@ -218,10 +222,13 @@ std::shared_ptr Adapter::add_fleet( std::move(traits)), rmf_traffic::agv::Planner::Options(nullptr))); + auto backup_db = backup_file_path.has_value() ? + DatabaseLogger::make(backup_file_path.value()) : nullptr; + auto fleet = FleetUpdateHandle::Implementation::make( fleet_name, std::move(planner), _pimpl->node, _pimpl->worker, _pimpl->schedule_writer, _pimpl->mirror_manager.snapshot_handle(), - _pimpl->negotiation); + _pimpl->negotiation, server_uri, std::move(backup_db)); _pimpl->fleets.push_back(fleet); return fleet; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp index e7ab8f5ea..e5520ae2b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp @@ -321,7 +321,7 @@ auto EasyTrafficLight::Implementation::moving_from( RCLCPP_WARN( node->get_logger(), "[EasyTrafficLight::moving_from] [%s] owned by [%s] is moving from an " - "invalid checkpoint [%u]. This robot currently does not have a path.", + "invalid checkpoint [%lu]. This robot currently does not have a path.", name.c_str(), owner.c_str(), checkpoint); } else @@ -329,8 +329,8 @@ auto EasyTrafficLight::Implementation::moving_from( RCLCPP_WARN( node->get_logger(), "[EasyTrafficLight::moving_from] [%s] owned by [%s] is moving from " - "an invalid checkpoint [%u]. The highest checkpoint value that you " - "can move from is [%u].", + "an invalid checkpoint [%lu]. The highest checkpoint value that you " + "can move from is [%lu].", name.c_str(), owner.c_str(), checkpoint, current_checkpoints.size()-1); } 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 acd37c4a8..95d5df67d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -27,22 +27,28 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" - -#include -#include +#include "../tasks/Clean.hpp" +#include "../tasks/ChargeBattery.hpp" +#include "../tasks/Compose.hpp" +#include "../events/GoToPlace.hpp" +#include "../events/PerformAction.hpp" + +#include +#include #include #include #include -#include -#include -#include +#include #include #include #include #include +#include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -81,6 +87,52 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator }; } // anonymous namespace +//============================================================================== +void TaskDeserialization::add_schema(const nlohmann::json& schema) +{ + _schema_dictionary->insert_or_assign( + nlohmann::json_uri(schema["$id"]).url(), schema); +} + +//============================================================================== +nlohmann::json_schema::json_validator TaskDeserialization::make_validator( + nlohmann::json schema) const +{ + return nlohmann::json_schema::json_validator(std::move(schema), _loader); +} + +//============================================================================== +std::shared_ptr +TaskDeserialization::make_validator_shared(nlohmann::json schema) const +{ + return std::make_shared( + make_validator(std::move(schema))); +} + +//============================================================================== +TaskDeserialization::TaskDeserialization() +{ + task = std::make_shared>(); + phase = std::make_shared>(); + event = std::make_shared>(); + consider_actions = + std::make_shared>(); + + _schema_dictionary = std::make_shared(); + _loader = [dict = _schema_dictionary]( + const nlohmann::json_uri& id, + nlohmann::json& value) + { + const auto it = dict->find(id.url()); + if (it == dict->end()) + return; + + value = it->second; + }; +} + + //============================================================================== void FleetUpdateHandle::Implementation::dock_summary_cb( const DockSummary::SharedPtr& msg) @@ -89,9 +141,9 @@ void FleetUpdateHandle::Implementation::dock_summary_cb( { if (dock.fleet_name == name) { - dock_param_map.clear(); + dock_param_map->clear(); for (const auto& param : dock.params) - dock_param_map.insert({param.start, param}); + dock_param_map->insert({param.start, param}); break; } } @@ -100,358 +152,204 @@ void FleetUpdateHandle::Implementation::dock_summary_cb( } //============================================================================== -void FleetUpdateHandle::Implementation::bid_notice_cb( - const BidNotice::SharedPtr msg) +std::string FleetUpdateHandle::Implementation::make_error_str( + uint64_t code, std::string category, std::string detail) const { - if (task_managers.empty()) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have any robots to accept task [%s]. Use " - "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ", - name.c_str(), msg->task_profile.task_id.c_str()); - return; - } + nlohmann::json error; + error["code"] = code; + error["category"] = std::move(category); + error["detail"] = std::move(detail); + + return error.dump(); +} - if (msg->task_profile.task_id.empty()) +//============================================================================== +std::shared_ptr FleetUpdateHandle::Implementation::convert( + const std::string& task_id, + const nlohmann::json& request_msg, + std::vector& errors) const +{ + const auto& category = request_msg["category"].get(); + + const auto task_deser_it = deserialization.task->handlers.find(category); + if (task_deser_it == deserialization.task->handlers.end()) { - RCLCPP_WARN( - node->get_logger(), - "Received BidNotice for a task with invalid task_id. Request will be " - "ignored."); - return; + errors.push_back(make_error_str( + 4, "Unsupported type", + "Fleet [" + name + "] does not support task category [" + + category + "]")); + return nullptr; } - // TODO remove this block when we support task revival - if (bid_notice_assignments.find(msg->task_profile.task_id) - != bid_notice_assignments.end()) - return; + const auto& description_msg = request_msg["description"]; + const auto& task_deser_handler = task_deser_it->second; - if (!accept_task) + try { - RCLCPP_WARN( - node->get_logger(), - "Fleet [%s] is not configured to accept any task requests. Use " - "FleetUpdateHadndle::accept_task_requests(~) to define a callback " - "for accepting requests", name.c_str()); - - return; + task_deser_handler.validator->validate(description_msg); } - - if (!accept_task(msg->task_profile)) + catch (const std::exception& e) { - RCLCPP_INFO( + RCLCPP_ERROR( node->get_logger(), - "Fleet [%s] is configured to not accept task [%s]", - name.c_str(), - msg->task_profile.task_id.c_str()); - - return; + "Received a request description for [%s] with an invalid format. " + "Error: %s\nRequest:\n%s", + category.c_str(), + e.what(), + description_msg.dump(2, ' ').c_str()); + + errors.push_back(make_error_str(5, "Invalid request format", e.what())); + return nullptr; } - if (!task_planner) - { - RCLCPP_WARN( - node->get_logger(), - "Fleet [%s] is not configured with parameters for task planning." - "Use FleetUpdateHandle::set_task_planner_params(~) to set the " - "parameters required.", name.c_str()); + const auto deserialized_task = + task_deser_handler.deserializer(description_msg); - return; + if (!deserialized_task.description) + { + errors = deserialized_task.errors; + return nullptr; } - // Determine task type and convert to request pointer - rmf_task::ConstRequestPtr new_request = nullptr; - const auto& task_profile = msg->task_profile; - const auto& task_type = task_profile.description.task_type; - const rmf_traffic::Time start_time = - rmf_traffic_ros2::convert(task_profile.description.start_time); - // TODO (YV) get rid of ID field in RequestPtr - std::string id = msg->task_profile.task_id; - const auto& graph = (*planner)->get_configuration().graph(); - - // Generate the priority of the request. The current implementation supports - // binary priority - auto priority = - task_profile.description.priority.value > 0 ? - rmf_task::BinaryPriorityScheme::make_high_priority() : - rmf_task::BinaryPriorityScheme::make_low_priority(); - - // Process Cleaning task - if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) + rmf_traffic::Time earliest_start_time = rmf_traffic_ros2::convert( + node->get_clock()->now()); + const auto t_it = request_msg.find("unix_millis_earliest_start_time"); + if (t_it != request_msg.end()) { - if (task_profile.description.clean.start_waypoint.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [clean.start_waypoint] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } + earliest_start_time = + rmf_traffic::Time(std::chrono::milliseconds(t_it->get())); + } - // Check for valid start waypoint - const std::string start_wp_name = - task_profile.description.clean.start_waypoint; - const auto start_wp = graph.find_waypoint(start_wp_name); - if (!start_wp) + rmf_task::ConstPriorityPtr priority; + const auto p_it = request_msg.find("priority"); + if (p_it != request_msg.end()) + { + // TODO(YV): Validate with priority_description_Binary.json + if (p_it->contains("type") && p_it->contains("value")) { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), start_wp_name.c_str(), id.c_str()); + const auto& p_type = (*p_it)["type"]; + if (p_type.is_string() && p_type.get() == "binary") + { + const auto& p_value = (*p_it)["value"]; + if (p_value.is_number_integer()) + { + if (p_value.is_number_integer() && p_value.get() > 0) + priority = rmf_task::BinaryPriorityScheme::make_high_priority(); + } - return; + priority = rmf_task::BinaryPriorityScheme::make_low_priority(); + } } - // Get dock parameters - const auto clean_param_it = dock_param_map.find(start_wp_name); - if (clean_param_it == dock_param_map.end()) - { - RCLCPP_INFO( - node->get_logger(), - "Dock param for dock_name:[%s] unavailable. Rejecting BidNotice with " - "task_id:[%s]", start_wp_name.c_str(), id.c_str()); - - return; - } - const auto& clean_param = clean_param_it->second; - - // Check for valid finish waypoint - // This is the waypoint on the map where the robot will end up at the end - // of its cleaning process. RMF will not move the robot to this waypoint. - // This information is used to estimate the state of the robot at the end - // of its cleaning process which is relevant for task allocation planning. - const std::string& finish_wp_name = clean_param.finish; - const auto finish_wp = graph.find_waypoint(finish_wp_name); - if (!finish_wp) + if (!priority) { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), finish_wp_name.c_str(), id.c_str()); - - return; + errors.push_back( + make_error_str( + 4, "Unsupported type", + "Fleet [" + name + "] does not support priority request: " + p_it->dump() + + "\nDefaulting to low binary priority.")); } + } - // Interpolate docking waypoint into trajectory - std::vector positions; - for (const auto& location: clean_param.path) - positions.push_back({location.x, location.y, location.yaw}); - rmf_traffic::Trajectory cleaning_trajectory = - rmf_traffic::agv::Interpolate::positions( - (*planner)->get_configuration().vehicle_traits(), - start_time, - positions); - - if (cleaning_trajectory.size() == 0) - { - RCLCPP_INFO( - node->get_logger(), - "Unable to generate cleaning trajectory from positions specified " - " in DockSummary msg for [%s]", start_wp_name.c_str()); + if (!priority) + priority = rmf_task::BinaryPriorityScheme::make_low_priority(); - return; - } + const auto new_request = + std::make_shared( + task_id, + earliest_start_time, + priority, + deserialized_task.description); - new_request = rmf_task::requests::Clean::make( - start_wp->index(), - finish_wp->index(), - cleaning_trajectory, - id, - start_time, - priority); + return new_request; +} +//============================================================================== +void FleetUpdateHandle::Implementation::bid_notice_cb( + const BidNoticeMsg& bid_notice, + rmf_task_ros2::bidding::AsyncBidder::Respond respond) +{ + // TODO(YV): Consider moving these checks into convert() + const auto& task_id = bid_notice.task_id; + if (task_managers.empty()) + { RCLCPP_INFO( node->get_logger(), - "Generated Clean request for task_id:[%s]", id.c_str()); + "Fleet [%s] does not have any robots to accept task [%s]. Use " + "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ", + name.c_str(), task_id.c_str()); + return; } - else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) + if (task_id.empty()) { - const auto& delivery = task_profile.description.delivery; - if (delivery.pickup_place_name.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.pickup_place_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - if (delivery.pickup_dispenser.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.pickup_dispenser] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - if (delivery.dropoff_place_name.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.dropoff_place_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - if (delivery.dropoff_place_name.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.dropoff_place_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - if (delivery.dropoff_ingestor.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [delivery.dropoff_ingestor] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name); - if (!pickup_wp) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), delivery.pickup_place_name.c_str(), id.c_str()); - - return; - } - - const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name); - if (!dropoff_wp) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), delivery.dropoff_place_name.c_str(), id.c_str()); - - return; - } - - // TODO: We set the waiting duration at the pickup and dropoff locations to - // 0s as the cycle time of the dispensers and ingestors are not available. - // We should implement a means to lookup these values for each system. - new_request = rmf_task::requests::Delivery::make( - pickup_wp->index(), - rmf_traffic::time::from_seconds(0), - dropoff_wp->index(), - rmf_traffic::time::from_seconds(0), - id, - start_time, - priority); - - RCLCPP_INFO( + RCLCPP_WARN( node->get_logger(), - "Generated Delivery request for task_id:[%s]", id.c_str()); - + "Received BidNotice for a task with empty task_id. Request will be " + "ignored."); + return; } - else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) - { - const auto& loop = task_profile.description.loop; - if (loop.start_name.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [loop.start_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - if (loop.finish_name.empty()) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [loop.finish_name] missing in TaskProfile." - "Rejecting BidNotice with task_id:[%s]", id.c_str()); - - return; - } - - if (loop.num_loops < 1) - { - RCLCPP_ERROR( - node->get_logger(), - "Required param [loop.num_loops: %d] in TaskProfile is invalid." - "Rejecting BidNotice with task_id:[%s]", loop.num_loops, id.c_str()); - - return; - } - - const auto start_wp = graph.find_waypoint(loop.start_name); - if (!start_wp) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), loop.start_name.c_str(), id.c_str()); - - return; - } + // TODO remove this block when we support task revival + if (bid_notice_assignments.find(task_id) != bid_notice_assignments.end()) + return; - const auto finish_wp = graph.find_waypoint(loop.finish_name); - if (!finish_wp) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] does not have a named waypoint [%s] configured in its " - "nav graph. Rejecting BidNotice with task_id:[%s]", - name.c_str(), loop.finish_name.c_str(), id.c_str()); + if (!task_planner) + { + RCLCPP_WARN( + node->get_logger(), + "Fleet [%s] is not configured with parameters for task planning." + "Use FleetUpdateHandle::set_task_planner_params(~) to set the " + "parameters required.", name.c_str()); - return; - } + return; + } - new_request = rmf_task::requests::Loop::make( - start_wp->index(), - finish_wp->index(), - loop.num_loops, - id, - start_time, - priority); + const auto request_msg = nlohmann::json::parse(bid_notice.request); + static const auto request_validator = + nlohmann::json_schema::json_validator(rmf_api_msgs::schemas::task_request); - RCLCPP_INFO( - node->get_logger(), - "Generated Loop request for task_id:[%s]", id.c_str()); + try + { + request_validator.validate(request_msg); } - else + catch (const std::exception& e) { RCLCPP_ERROR( node->get_logger(), - "Invalid TaskType [%d] in TaskProfile. Rejecting BidNotice with " - "task_id:[%s]", - task_type.type, id.c_str()); + "Received a request with an invalid format. Error: %s\nRequest:\n%s", + e.what(), + request_msg.dump(2, ' ').c_str()); - return; + return respond( + { + std::nullopt, + {make_error_str(5, "Invalid request format", e.what())} + }); } + std::vector errors = {}; + const auto new_request = convert(task_id, request_msg, errors); if (!new_request) - return; - generated_requests.insert({id, new_request}); - task_profile_map.insert({id, task_profile}); + { + return respond( + { + std::nullopt, + errors + }); + } - const auto allocation_result = allocate_tasks(new_request); + // Cache request_msg + task_request_jsons.insert({task_id, request_msg}); + // TODO(MXG): Make the task planning asynchronous. The worker should schedule + // a job to perform the planning which should then spawn a job to save the + // plan result and respond. I started to refactor allocate_tasks(~) to make it + // async, but I will save the remaining effort for later, when there is more + // time to spare. + auto allocation_result = allocate_tasks(new_request, &errors); if (!allocation_result.has_value()) - return; + return respond({std::nullopt, std::move(errors)}); const auto& assignments = allocation_result.value(); @@ -465,16 +363,18 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( debug_stream << "--Agent: " << i << std::endl; for (const auto& a : assignments[i]) { - const auto& s = a.state(); + const auto& s = a.finish_state(); const double request_seconds = - a.request()->earliest_start_time().time_since_epoch().count()/1e9; + a.request()->booking()->earliest_start_time().time_since_epoch().count() + /1e9; const double start_seconds = a.deployment_time().time_since_epoch().count()/1e9; - const rmf_traffic::Time finish_time = s.finish_time(); + const rmf_traffic::Time finish_time = s.time().value(); const double finish_seconds = finish_time.time_since_epoch().count()/1e9; - debug_stream << " <" << a.request()->id() << ": " << request_seconds + debug_stream << " <" << a.request()->booking()->id() << ": " << + request_seconds << ", " << start_seconds - << ", "<< finish_seconds << ", " << 100* s.battery_soc() + << ", "<< finish_seconds << ", " << s.battery_soc().value() << "%>" << std::endl; } } @@ -482,13 +382,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_DEBUG(node->get_logger(), "%s", debug_stream.str().c_str()); - // Publish BidProposal - rmf_task_msgs::msg::BidProposal bid_proposal; - bid_proposal.fleet_name = name; - bid_proposal.task_profile = task_profile; - bid_proposal.prev_cost = current_assignment_cost; - bid_proposal.new_cost = cost; - // Map robot index to name to populate robot_name in BidProposal std::unordered_map robot_name_map; std::size_t index = 0; @@ -498,75 +391,143 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( ++index; } + std::optional robot_name; + std::optional finish_time; index = 0; for (const auto& agent : assignments) { for (const auto& assignment : agent) { - if (assignment.request()->id() == id) + if (assignment.request()->booking()->id() == task_id) { - bid_proposal.finish_time = rmf_traffic_ros2::convert( - assignment.state().finish_time()); + finish_time = assignment.finish_state().time().value(); if (robot_name_map.find(index) != robot_name_map.end()) - bid_proposal.robot_name = robot_name_map[index]; + robot_name = robot_name_map[index]; break; } } ++index; } - bid_proposal_pub->publish(bid_proposal); + if (!robot_name.has_value() || !finish_time.has_value()) + { + errors.push_back( + make_error_str( + 13, "Internal bug", + "Failed to find robot_name or finish_time after allocating task. " + "Please report this bug to the RMF developers.")); + + return respond({std::nullopt, std::move(errors)}); + } + + // Publish BidProposal + respond( + { + rmf_task_ros2::bidding::Response::Proposal{ + name, + *robot_name, + current_assignment_cost, + cost, + *finish_time + }, + std::move(errors) + }); + RCLCPP_INFO( node->get_logger(), "Submitted BidProposal to accommodate task [%s] by robot [%s] with new cost [%f]", - id.c_str(), bid_proposal.robot_name.c_str(), cost); + task_id.c_str(), robot_name->c_str(), cost); // Store assignments in internal map - bid_notice_assignments.insert({id, assignments}); - + bid_notice_assignments.insert({task_id, assignments}); } //============================================================================== -void FleetUpdateHandle::Implementation::dispatch_request_cb( - const DispatchRequest::SharedPtr msg) +void FleetUpdateHandle::Implementation::dispatch_command_cb( + const DispatchCmdMsg::SharedPtr msg) { + const auto& task_id = msg->task_id; if (msg->fleet_name != name) + { + // This task is either being awarded or canceled for another fleet. Either + // way, we will delete it from our record of bid notice assignments. + bid_notice_assignments.erase(task_id); return; + } - const std::string id = msg->task_profile.task_id; DispatchAck dispatch_ack; - dispatch_ack.dispatch_request = *msg; dispatch_ack.success = false; - - if (msg->method == DispatchRequest::ADD) + dispatch_ack.dispatch_id = msg->dispatch_id; + if (msg->type == DispatchCmdMsg::TYPE_AWARD) { - const auto task_it = bid_notice_assignments.find(id); + const auto task_it = bid_notice_assignments.find(task_id); if (task_it == bid_notice_assignments.end()) { - RCLCPP_WARN( - node->get_logger(), - "Received DispatchRequest for task_id:[%s] before receiving BidNotice. " - "This request will be ignored.", - id.c_str()); + // We don't have a record of this bid_notice, so let's check if we already + // received the task assignment. + bool already_assigned = false; + for (const auto& tm : task_managers) + { + const auto current = tm.second->current_task_id(); + if (current == task_id) + { + already_assigned = true; + break; + } + + for (const auto& p : tm.second->get_queue()) + { + if (p.request()->booking()->id() == task_id) + { + already_assigned = true; + break; + } + } + + if (already_assigned) + break; + } + + if (already_assigned) + { + dispatch_ack.success = true; + dispatch_ack_pub->publish(dispatch_ack); + return; + } + + std::string error_str = + "Received DispatchRequest award for task_id [" + task_id + + "] before receiving BidNotice. This request will be ignored."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + dispatch_ack.errors.push_back( + make_error_str(14, "Invalid sequence", std::move(error_str))); dispatch_ack_pub->publish(dispatch_ack); return; } RCLCPP_INFO( node->get_logger(), - "Bid for task_id:[%s] awarded to fleet [%s]. Processing request...", - id.c_str(), + "Bid for task_id [%s] awarded to fleet [%s]. Processing request...", + task_id.c_str(), name.c_str()); - auto& assignments = task_it->second; + auto assignments = std::move(task_it->second); + bid_notice_assignments.erase(task_it); if (assignments.size() != task_managers.size()) { - RCLCPP_ERROR( - node->get_logger(), + // FIXME(MXG): This error mode seems like a problem with our + // implementation. If a robot is added during a bid process, we could + // trigger this error even though it shouldn't actually be a problem. + + std::string error_str = "The number of available robots does not match that in the assignments " - "for task_id:[%s]. This request will be ignored.", - id.c_str()); + "for task_id [" + task_id + "]. This request will be ignored."; + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + dispatch_ack.errors.push_back( + make_error_str(13, "Internal bug", std::move(error_str))); + dispatch_ack_pub->publish(dispatch_ack); return; } @@ -574,158 +535,160 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( // Here we make sure none of the tasks in the assignments has already begun // execution. If so, we replan assignments until a valid set is obtained // and only then update the task manager queues - const auto request_it = generated_requests.find(id); - if (request_it == generated_requests.end()) - { - RCLCPP_ERROR( - node->get_logger(), - "Unable to find generated request for task_id:[%s]. This request will " - "be ignored.", - id.c_str()); - dispatch_ack_pub->publish(dispatch_ack); - return; - } - bool valid_assignments = is_valid_assignments(assignments); if (!valid_assignments) { + rmf_task::ConstRequestPtr request; + for (const auto& a : assignments) + { + for (const auto& r : a) + { + if (r.request()->booking()->id() == task_id) + { + request = r.request(); + break; + } + } + + if (request) + break; + } + + if (!request) + { + std::string error_str = + "Could not find task_id [" + task_id + "] in the set of assignments " + "associated with it. This is a critical bug and should be reported " + "to the RMF developers"; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + dispatch_ack.errors.push_back( + make_error_str(13, "Internal bug", std::move(error_str))); + dispatch_ack_pub->publish(dispatch_ack); + return; + } + // TODO: This replanning is blocking the main thread. Instead, the // replanning should run on a separate worker and then deliver the // result back to the main worker. - const auto replan_results = allocate_tasks(request_it->second); + const auto replan_results = allocate_tasks(request, &dispatch_ack.errors); if (!replan_results) { - RCLCPP_WARN( - node->get_logger(), - "Unable to replan assignments when accommodating task_id:[%s]. This " - "request will be ignored.", - id.c_str()); + std::string error_str = + "Unable to replan assignments when accommodating task_id [" + task_id + + "]. This request will be ignored."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + dispatch_ack.errors.push_back( + make_error_str(9, "Not feasible", std::move(error_str))); dispatch_ack_pub->publish(dispatch_ack); return; } + assignments = replan_results.value(); // We do not need to re-check if assignments are valid as this function // is being called by the ROS2 executor and is running on the main - // rxcpp worker. Hence, no new tasks would have started during this replanning. + // rxcpp worker. Hence, no new tasks would have started during this + // replanning. } std::size_t index = 0; for (auto& t : task_managers) { - t.second->set_queue(assignments[index], task_profile_map); + t.second->set_queue(assignments[index]); ++index; } current_assignment_cost = task_planner->compute_cost(assignments); - assigned_requests.insert({id, request_it->second}); dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); RCLCPP_INFO( node->get_logger(), - "Assignments updated for robots in fleet [%s] to accommodate task_id:[%s]", - name.c_str(), id.c_str()); + "Assignments updated for robots in fleet [%s] to accommodate task_id [%s]", + name.c_str(), task_id.c_str()); } - - else if (msg->method == DispatchRequest::CANCEL) + else if (msg->type == DispatchCmdMsg::TYPE_REMOVE) { - // We currently only support cancellation of a queued task. - // TODO: Support cancellation of an active task. - - // When a queued task is to be cancelled, we simply re-plan and re-allocate - // task assignments for the request set containing all the queued tasks - // excluding the task to be cancelled. - if (cancelled_task_ids.find(id) != cancelled_task_ids.end()) - { - RCLCPP_WARN( - node->get_logger(), - "Request with task_id:[%s] has already been cancelled.", - id.c_str()); - - dispatch_ack.success = true; - dispatch_ack_pub->publish(dispatch_ack); - return; - } - - auto request_to_cancel_it = assigned_requests.find(id); - if (request_to_cancel_it == assigned_requests.end()) + const auto bid_it = bid_notice_assignments.find(task_id); + if (bid_it != bid_notice_assignments.end()) { - RCLCPP_WARN( - node->get_logger(), - "Unable to cancel task with task_id:[%s] as it is not assigned to " - "fleet:[%s].", - id.c_str(), name.c_str()); - - dispatch_ack_pub->publish(dispatch_ack); - return; - } - - std::unordered_set executed_tasks; - for (const auto& [context, mgr] : task_managers) - { - const auto& tasks = mgr->get_executed_tasks(); - executed_tasks.insert(tasks.begin(), tasks.end()); - } - - // Check if received request is to cancel an active task - if (executed_tasks.find(id) != executed_tasks.end()) - { - RCLCPP_WARN( - node->get_logger(), - "Unable to cancel active task with task_id:[%s]. Only queued tasks may " - "be cancelled.", - id.c_str()); - - dispatch_ack_pub->publish(dispatch_ack); - return; + // The task was still in the bid notice assignments, so it was never + // actually assigned to a robot. We will just delete it from this map + // and we're done. + bid_notice_assignments.erase(bid_it); } - - // Re-plan assignments while ignoring request for task to be cancelled - const auto replan_results = allocate_tasks( - nullptr, request_to_cancel_it->second); - - if (!replan_results.has_value()) + else { - RCLCPP_WARN( - node->get_logger(), - "Unable to re-plan assignments when cancelling task with task_id:[%s]", - id.c_str()); + bool task_was_found = false; + // Make sure the task isn't running in any of the task managers + for (const auto& [_, tm] : task_managers) + { + task_was_found = tm->cancel_task_if_present(task_id); + if (task_was_found) + break; + } - dispatch_ack_pub->publish(dispatch_ack); - return; - } + if (task_was_found) + { + // Re-plan assignments while ignoring request for task to be cancelled + std::vector errors; + const auto replan_results = allocate_tasks(nullptr, &errors); + if (!replan_results.has_value()) + { + std::stringstream ss; + ss << "Unabled to replan assignments when cancelling task [" + << task_id << "]. "; + if (errors.empty()) + { + ss << "No planner error messages were provided."; + } + else + { + ss << "The following planner errors occurred:"; + for (const auto& e : errors) + { + const auto err = nlohmann::json::parse(e); + ss << "\n -- " << err["detail"].get(); + } + } + ss << "\n"; - const auto& assignments = replan_results.value(); - std::size_t index = 0; - for (auto& t : task_managers) - { - t.second->set_queue(assignments[index], task_profile_map); - ++index; - } + RCLCPP_WARN(node->get_logger(), "%s", ss.str().c_str()); + } + else + { + const auto& assignments = replan_results.value(); + std::size_t index = 0; + for (auto& t : task_managers) + { + t.second->set_queue(assignments[index]); + ++index; + } - current_assignment_cost = task_planner->compute_cost(assignments); + current_assignment_cost = task_planner->compute_cost(assignments); + + RCLCPP_INFO( + node->get_logger(), + "Task with task_id [%s] has successfully been cancelled. Assignments " + "updated for robots in fleet [%s].", + task_id.c_str(), name.c_str()); + } + } + } dispatch_ack.success = true; dispatch_ack_pub->publish(dispatch_ack); - cancelled_task_ids.insert(id); - - RCLCPP_INFO( - node->get_logger(), - "Task with task_id:[%s] has successfully been cancelled. Assignments " - "updated for robots in fleet [%s].", - id.c_str(), name.c_str()); } - else { - RCLCPP_WARN( - node->get_logger(), - "Received DispatchRequest for task_id:[%s] with invalid method. Only " - "ADD and CANCEL methods are supported. This request will be ignored.", - id.c_str()); - return; + std::string error_str = + "Received unknown dispatch request type: " + std::to_string(msg->type); + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + dispatch_ack.errors.push_back( + make_error_str(4, "Unsupported type", std::move(error_str))); + dispatch_ack_pub->publish(dispatch_ack); } - } //============================================================================== @@ -743,7 +706,8 @@ auto FleetUpdateHandle::Implementation::is_valid_assignments( { for (const auto& a : agent) { - if (executed_tasks.find(a.request()->id()) != executed_tasks.end()) + if (executed_tasks.find(a.request()->booking()->id()) != + executed_tasks.end()) return false; } } @@ -776,22 +740,35 @@ get_nearest_charger( return nearest_charger; } +namespace { //============================================================================== -void FleetUpdateHandle::Implementation::fleet_state_publish_period( - std::optional value) +rmf_fleet_msgs::msg::Location convert_location(const agv::RobotContext& context) { - if (value.has_value()) - { - fleet_state_timer = node->create_wall_timer( - std::chrono::seconds(1), [this]() { this->publish_fleet_state(); }); - } - else + if (context.location().empty()) { - fleet_state_timer = nullptr; + // TODO(MXG): We should emit some kind of critical error if this ever + // happens + return rmf_fleet_msgs::msg::Location(); } + + const auto& graph = context.planner()->get_configuration().graph(); + const auto& l = context.location().front(); + const auto& wp = graph.get_waypoint(l.waypoint()); + const Eigen::Vector2d p = l.location().value_or(wp.get_location()); + + return rmf_fleet_msgs::build() + .t(rmf_traffic_ros2::convert(l.time())) + .x(p.x()) + .y(p.y()) + .yaw(l.orientation()) + .obey_approach_speed_limit(false) + .approach_speed_limit(0.0) + .level_name(wp.get_map_name()) + // NOTE(MXG): This field is only used by the fleet drivers. For now, we + // will just fill it with a zero. + .index(0); } -namespace { //============================================================================== rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) { @@ -799,38 +776,10 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) const auto mode = mgr.robot_mode(); - auto location = [&]() -> rmf_fleet_msgs::msg::Location - { - if (context.location().empty()) - { - // TODO(MXG): We should emit some kind of critical error if this ever - // happens - return rmf_fleet_msgs::msg::Location(); - } - - const auto& graph = context.planner()->get_configuration().graph(); - const auto& l = context.location().front(); - const auto& wp = graph.get_waypoint(l.waypoint()); - const Eigen::Vector2d p = l.location().value_or(wp.get_location()); - - return rmf_fleet_msgs::build() - .t(rmf_traffic_ros2::convert(l.time())) - .x(p.x()) - .y(p.y()) - .yaw(l.orientation()) - .obey_approach_speed_limit(false) - .approach_speed_limit(0.0) - .level_name(wp.get_map_name()) - // NOTE(MXG): This field is only used by the fleet drivers. For now, we - // will just fill it with a zero. - .index(0); - } (); - - return rmf_fleet_msgs::build() .name(context.name()) .model(context.description().owner()) - .task_id(mgr.current_task() ? mgr.current_task()->id() : "") + .task_id(mgr.current_task_id().value_or("")) // TODO(MXG): We could keep track of the seq value and increment it once // with each publication. This is not currently an important feature // outside of the fleet driver, so for now we just set it to zero. @@ -838,7 +787,7 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) .mode(std::move(mode)) // We multiply by 100 to convert from the [0.0, 1.0] range to percentage .battery_percent(context.current_battery_soc()*100.0) - .location(std::move(location)) + .location(convert_location(context)) // NOTE(MXG): The path field is only used by the fleet drivers. For now, // we will just fill it with a zero. We could consider filling it in based // on the robot's plan, but that seems redundant with the traffic schedule @@ -848,7 +797,7 @@ rmf_fleet_msgs::msg::RobotState convert_state(const TaskManager& mgr) } // anonymous namespace //============================================================================== -void FleetUpdateHandle::Implementation::publish_fleet_state() const +void FleetUpdateHandle::Implementation::publish_fleet_state_topic() const { std::vector robot_states; for (const auto& [context, mgr] : task_managers) @@ -862,102 +811,305 @@ void FleetUpdateHandle::Implementation::publish_fleet_state() const } //============================================================================== -auto FleetUpdateHandle::Implementation::allocate_tasks( - rmf_task::ConstRequestPtr new_request, - rmf_task::ConstRequestPtr ignore_request) const -> std::optional +void FleetUpdateHandle::Implementation::update_fleet_state() const { - // Collate robot states, constraints and combine new requestptr with - // requestptr of non-charging tasks in task manager queues - std::vector states; - std::vector pending_requests; - std::string id = ""; + // Publish to API server + if (broadcast_client) + { + nlohmann::json fleet_state_msg; + fleet_state_msg["name"] = name; + auto& robots = fleet_state_msg["robots"]; + for (const auto& [context, mgr] : task_managers) + { + const auto& name = context->name(); + nlohmann::json& json = robots[name]; + json["name"] = name; + json["status"] = mgr->robot_status(); + json["task_id"] = mgr->current_task_id().value_or(""); + json["unix_millis_time"] = + std::chrono::duration_cast( + context->now().time_since_epoch()).count(); + json["battery"] = context->current_battery_soc(); + + nlohmann::json& location = json["location"]; + const auto location_msg = convert_location(*context); + location["map"] = location_msg.level_name; + location["x"] = location_msg.x; + location["y"] = location_msg.y; + location["yaw"] = location_msg.yaw; + + // TODO(YV): json["issues"] + } - if (new_request) + const auto fleet_schema = rmf_api_msgs::schemas::fleet_state_update; + const auto loader = + [n = node, s = schema_dictionary](const nlohmann::json_uri& id, + nlohmann::json& value) + { + const auto it = s.find(id.url()); + if (it == s.end()) + { + RCLCPP_ERROR( + n->get_logger(), + "url: %s not found in schema dictionary", id.url().c_str()); + return; + } + + value = it->second; + }; + + nlohmann::json fleet_state_update_msg; + fleet_state_update_msg["type"] = "fleet_state_update"; + fleet_state_update_msg["data"] = fleet_state_msg; + try + { + static const nlohmann::json_schema::json_validator + validator(fleet_schema, loader); + + validator.validate(fleet_state_update_msg); + } + catch (const std::exception& e) + { + RCLCPP_ERROR( + node->get_logger(), + "Unable to publish fleet state json message: %s", + e.what()); + return; + } + + broadcast_client->publish(fleet_state_update_msg); + } +} + +namespace { +PlaceDeserializer make_place_deserializer( + std::shared_ptr> + planner) +{ + return [planner = std::move(planner)](const nlohmann::json& msg) + -> agv::DeserializedPlace + { + std::optional place; + const auto& graph = (*planner)->get_configuration().graph(); + if (msg.is_number() || (msg.is_object() && msg["waypoint"].is_number())) + { + const auto wp_index = msg.is_number()? + msg.get() : msg["waypoint"].get(); + + if (graph.num_waypoints() <= wp_index) + { + return { + std::nullopt, + {"waypoint index value for Place [" + + std::to_string(wp_index) + "] exceeds the limits for the nav " + "graph size [" + std::to_string(graph.num_waypoints()) + "]"} + }; + } + + place = rmf_traffic::agv::Plan::Goal(wp_index); + } + else if (msg.is_string() || (msg.is_object() && msg["waypoint"].is_string())) + { + const auto& wp_name = msg.is_string()? + msg.get() : msg["waypoint"].get(); + + const auto* wp = graph.find_waypoint(wp_name); + if (!wp) + { + return { + std::nullopt, + {"waypoint name for Place [" + wp_name + "] cannot be " + "found in the navigation graph"} + }; + } + + place = rmf_traffic::agv::Plan::Goal(wp->index()); + } + else + { + return { + std::nullopt, + {"invalid data type provided for Place: expected a number " + "or a string or an object but got " + std::string(msg.type_name()) + + " type instead"} + }; + } + + if (msg.is_object()) + { + const auto& ori_it = msg.find("orientation"); + if (ori_it != msg.end()) + place->orientation(ori_it->get()); + } + + return {place, {}}; + }; +} +} // anonymous namespace + +//============================================================================== +void FleetUpdateHandle::Implementation::add_standard_tasks() +{ + activation.task = std::make_shared(); + activation.phase = std::make_shared(); + activation.event = std::make_shared(); + + rmf_task_sequence::Task::add( + *activation.task, activation.phase, node->clock()); + + rmf_task_sequence::phases::SimplePhase::add( + *activation.phase, activation.event); + + events::GoToPlace::add(*activation.event); + events::PerformAction::add(*activation.event); + deserialization.place = make_place_deserializer(planner); + deserialization.add_schema(schemas::Place); + + tasks::add_delivery( + deserialization, + activation, + node->clock()); + + tasks::add_loop( + deserialization, + activation, + node->clock()); + + tasks::add_clean( + dock_param_map, + (*planner)->get_configuration().vehicle_traits(), + deserialization, + activation, + node->clock()); + + tasks::add_charge_battery( + *activation.task, + activation.phase, + *activation.event, + node->clock()); + + tasks::add_compose( + deserialization, + activation, + node->clock()); +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::add_performable_action( + const std::string& category, + ConsiderRequest consider) +{ + if (category.empty()) { - pending_requests.push_back(new_request); - id = new_request->id(); + RCLCPP_ERROR( + _pimpl->node->get_logger(), + "FleetUpdateHandle::add_performable_action(~) called with empty category" + ); + return *this; } + _pimpl->deserialization.consider_actions->insert_or_assign( + category, consider); + + return *this; +} + +//============================================================================== +auto FleetUpdateHandle::Implementation::aggregate_expectations() const +-> Expectations +{ + Expectations expect; for (const auto& t : task_managers) { - states.push_back(t.second->expected_finish_state()); + expect.states.push_back(t.second->expected_finish_state()); const auto requests = t.second->requests(); - pending_requests.insert( - pending_requests.end(), requests.begin(), requests.end()); + expect.pending_requests.insert( + expect.pending_requests.end(), requests.begin(), requests.end()); } - // Remove the request to be ignored if present - if (ignore_request) + return expect; +} + +//============================================================================== +auto FleetUpdateHandle::Implementation::allocate_tasks( + rmf_task::ConstRequestPtr new_request, + std::vector* errors, + std::optional expectations) const -> std::optional +{ + // Collate robot states, constraints and combine new requestptr with + // requestptr of non-charging tasks in task manager queues + auto expect = expectations.has_value() ? expectations.value() : + aggregate_expectations(); + std::string id = ""; + + if (new_request) { - auto ignore_request_it = pending_requests.end(); - for (auto it = pending_requests.begin(); it != pending_requests.end(); ++it) - { - auto pending_request = *it; - if (pending_request->id() == ignore_request->id()) - ignore_request_it = it; - } - if (ignore_request_it != pending_requests.end()) - { - pending_requests.erase(ignore_request_it); - RCLCPP_INFO( - node->get_logger(), - "Request with task_id:[%s] will be ignored during task allocation.", - ignore_request->id().c_str()); - } - else - { - RCLCPP_WARN( - node->get_logger(), - "Request with task_id:[%s] is not present in any of the task queues.", - ignore_request->id().c_str()); - } + expect.pending_requests.push_back(new_request); + id = new_request->booking()->id(); } RCLCPP_INFO( node->get_logger(), "Planning for [%ld] robot(s) and [%ld] request(s)", - states.size(), - pending_requests.size()); + expect.states.size(), + expect.pending_requests.size()); // Generate new task assignments const auto result = task_planner->plan( rmf_traffic_ros2::convert(node->now()), - states, - pending_requests); + expect.states, + expect.pending_requests); auto assignments_ptr = std::get_if< - rmf_task::agv::TaskPlanner::Assignments>(&result); + rmf_task::TaskPlanner::Assignments>(&result); if (!assignments_ptr) { auto error = std::get_if< - rmf_task::agv::TaskPlanner::TaskPlannerError>(&result); + rmf_task::TaskPlanner::TaskPlannerError>(&result); - if (*error == rmf_task::agv::TaskPlanner::TaskPlannerError::low_battery) + if (*error == rmf_task::TaskPlanner::TaskPlannerError::low_battery) { - RCLCPP_ERROR( - node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to" - " insufficient initial battery charge for all robots in this fleet.", - id.c_str()); + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "] due to insufficient initial battery charge for all robots in this " + "fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + if (errors) + { + errors->push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } } else if (*error == - rmf_task::agv::TaskPlanner::TaskPlannerError::limited_capacity) + rmf_task::TaskPlanner::TaskPlannerError::limited_capacity) { - RCLCPP_ERROR( - node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id:[%s] due to" - " insufficient battery capacity to accommodate one or more requests by" - " any of the robots in this fleet.", id.c_str()); + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + + "] due to insufficient battery capacity to accommodate one or more " + "requests by any of the robots in this fleet."; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + if (errors) + { + errors->push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } } else { - RCLCPP_ERROR( - node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id:[%s]", - id.c_str()); + std::string error_str = + "[TaskPlanner] Failed to compute assignments for task_id [" + id + "]"; + + RCLCPP_ERROR(node->get_logger(), "%s", error_str.c_str()); + if (errors) + { + errors->push_back( + make_error_str(9, "Not feasible", std::move(error_str))); + } } return std::nullopt; @@ -969,7 +1121,7 @@ auto FleetUpdateHandle::Implementation::allocate_tasks( { RCLCPP_ERROR( node->get_logger(), - "[TaskPlanner] Failed to compute assignments for task_id:[%s]", + "[TaskPlanner] Failed to compute assignments for task_id [%s]", id.c_str()); return std::nullopt; @@ -986,7 +1138,6 @@ void FleetUpdateHandle::add_robot( rmf_traffic::agv::Plan::StartSet start, std::function)> handle_cb) { - if (start.empty()) { // *INDENT-OFF* @@ -1028,8 +1179,8 @@ void FleetUpdateHandle::add_robot( // *INDENT-ON* } - rmf_task::agv::State state = rmf_task::agv::State{ - start[0], charger_wp.value(), 1.0}; + rmf_task::State state; + state.load_basic(start[0], charger_wp.value(), 1.0); auto context = std::make_shared( RobotContext @@ -1039,6 +1190,8 @@ void FleetUpdateHandle::add_robot( std::move(participant), fleet->_pimpl->snappable, fleet->_pimpl->planner, + fleet->_pimpl->activation.task, + fleet->_pimpl->task_parameters, fleet->_pimpl->node, fleet->_pimpl->worker, fleet->_pimpl->default_maximum_delay, @@ -1082,6 +1235,23 @@ void FleetUpdateHandle::add_robot( { if (const auto c = w.lock()) { + std::stringstream ss; + ss << "Failed negotiation for [" << c->requester_id() + << "] with these starts:"; + for (const auto& l : c->location()) + { + ss << "\n -- t:" << l.time().time_since_epoch().count() + << " | wp:" << l.waypoint() << " | ori:" + << l.orientation(); + if (l.location().has_value()) + { + const auto& p = *l.location(); + ss << " | pos:(" << p.x() << ", " << p.y() << ")"; + } + } + ss << "\n -- Fin --"; + std::cout << ss.str() << std::endl; + auto& last_time = *last_interrupt_time; const auto now = std::chrono::steady_clock::now(); if (last_time.has_value()) @@ -1117,12 +1287,108 @@ void FleetUpdateHandle::add_robot( return; } + std::optional> broadcast_client = + std::nullopt; + if (fleet->_pimpl->broadcast_client) + broadcast_client = fleet->_pimpl->broadcast_client; + fleet->_pimpl->task_managers.insert({context, - TaskManager::make(context)}); + TaskManager::make( + context, + broadcast_client, + std::weak_ptr(fleet))}); }); }); } +//============================================================================== +class FleetUpdateHandle::Confirmation::Implementation +{ +public: + bool is_accepted = false; + std::vector errors; +}; + +//============================================================================== +FleetUpdateHandle::Confirmation::Confirmation() +: _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +auto FleetUpdateHandle::Confirmation::accept() -> Confirmation& +{ + _pimpl->is_accepted = true; + return *this; +} + +//============================================================================== +bool FleetUpdateHandle::Confirmation::is_accepted() const +{ + return _pimpl->is_accepted; +} + +//============================================================================== +auto FleetUpdateHandle::Confirmation::errors( + std::vector error_messages) -> Confirmation& +{ + _pimpl->errors = std::move(error_messages); + return *this; +} + +//============================================================================== +auto FleetUpdateHandle::Confirmation::add_errors( + std::vector error_messages) -> Confirmation& +{ + _pimpl->errors.insert( + _pimpl->errors.end(), + std::make_move_iterator(error_messages.begin()), + std::make_move_iterator(error_messages.end())); + + return *this; +} + +//============================================================================== +const std::vector& FleetUpdateHandle::Confirmation::errors() const +{ + return _pimpl->errors; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::consider_delivery_requests( + ConsiderRequest consider_pickup, + ConsiderRequest consider_dropoff) +{ + *_pimpl->deserialization.consider_pickup = std::move(consider_pickup); + *_pimpl->deserialization.consider_dropoff = std::move(consider_dropoff); + return *this; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::consider_cleaning_requests( + ConsiderRequest consider) +{ + *_pimpl->deserialization.consider_clean = std::move(consider); + return *this; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::consider_patrol_requests( + ConsiderRequest consider) +{ + *_pimpl->deserialization.consider_patrol = std::move(consider); + return *this; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::consider_composed_requests( + ConsiderRequest consider) +{ + *_pimpl->deserialization.consider_composed = std::move(consider); + return *this; +} + //============================================================================== void FleetUpdateHandle::close_lanes(std::vector lane_indices) { @@ -1160,6 +1426,8 @@ void FleetUpdateHandle::close_lanes(std::vector lane_indices) *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); }); } @@ -1200,6 +1468,8 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) *self->_pimpl->planner = std::make_shared( new_config, rmf_traffic::agv::Planner::Options(nullptr)); + + self->_pimpl->task_parameters->planner(*self->_pimpl->planner); }); } @@ -1207,7 +1477,162 @@ void FleetUpdateHandle::open_lanes(std::vector lane_indices) FleetUpdateHandle& FleetUpdateHandle::accept_task_requests( AcceptTaskRequest check) { - _pimpl->accept_task = std::move(check); + const auto legacy_converter = + [check = std::move(check)]( + const rmf_task_msgs::msg::TaskProfile& profile, + Confirmation& confirm) + { + if (check(profile)) + { + confirm.accept(); + return; + } + + confirm.errors({"Task rejected by legacy AcceptTaskRequest callback"}); + }; + + const auto convert_item = [](const nlohmann::json& item) + -> rmf_dispenser_msgs::msg::DispenserRequestItem + { + rmf_dispenser_msgs::msg::DispenserRequestItem output; + output.type_guid = item["sku"]; + output.quantity = item["quantity"]; + const auto compartment_it = item.find("compartment"); + if (compartment_it != item.end()) + output.compartment_name = compartment_it->get(); + + return output; + }; + + const auto convert_items = [convert_item](const nlohmann::json& payload) + -> std::vector + { + std::vector items; + if (payload.is_object()) + { + items.push_back(convert_item(payload)); + } + else if (payload.is_array()) + { + for (const auto& p : payload) + items.push_back(convert_item(p)); + } + else + { + throw std::runtime_error( + "Invalid payload schema for delivery request:" + payload.dump()); + } + + return items; + }; + + auto consider_pickup = [legacy_converter, convert_items]( + const nlohmann::json& msg, Confirmation& confirm) + { + rmf_task_msgs::msg::TaskProfile profile; + profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + + profile.description.delivery = + rmf_task_msgs::build() + .task_id("") + .items(convert_items(msg["payload"])) + .pickup_place_name(msg["place"].get()) + .pickup_dispenser(msg["handler"].get()) + .pickup_behavior(rmf_task_msgs::msg::Behavior{}) + .dropoff_place_name("") + .dropoff_ingestor("") + .dropoff_behavior(rmf_task_msgs::msg::Behavior{}); + + legacy_converter(profile, confirm); + }; + + auto consider_dropoff = [legacy_converter, convert_items]( + const nlohmann::json& msg, Confirmation& confirm) + { + rmf_task_msgs::msg::TaskProfile profile; + profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + + profile.description.delivery = + rmf_task_msgs::build() + .task_id("") + .items(convert_items(msg["payload"])) + .pickup_place_name("") + .pickup_dispenser("") + .pickup_behavior(rmf_task_msgs::msg::Behavior{}) + .dropoff_place_name(msg["place"].get()) + .dropoff_ingestor(msg["handler"].get()) + .dropoff_behavior(rmf_task_msgs::msg::Behavior{}); + + legacy_converter(profile, confirm); + }; + + consider_delivery_requests( + std::move(consider_pickup), + std::move(consider_dropoff)); + + consider_patrol_requests( + [legacy_converter](const nlohmann::json& msg, Confirmation& confirm) + { + rmf_task_msgs::msg::TaskProfile profile; + + // We use loop here because patrol wasn't supported during the legacy + // versions anyway. + profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_LOOP; + + rmf_task_msgs::msg::Loop loop; + const auto& places = msg["places"]; + if (places.size() == 1) + { + const auto& place = places[0]; + if (!place.is_string()) + { + confirm.errors( + {"Legacy AcceptTaskRequest only accepts destination names " + "for patrol requests"}); + return; + } + + loop.start_name = places[0].get(); + loop.finish_name = loop.start_name; + } + else + { + const auto& start_place = places[0]; + const auto& finish_place = places[1]; + if (!start_place.is_string() || !finish_place.is_string()) + { + confirm.errors( + {"Legacy AcceptTaskRequest only accepts destination names " + "for patrol requests"}); + return; + } + + loop.start_name = start_place.get(); + loop.finish_name = finish_place.get(); + } + + loop.num_loops = msg["rounds"].get(); + profile.description.loop = std::move(loop); + + legacy_converter(profile, confirm); + }); + + consider_cleaning_requests( + [legacy_converter](const nlohmann::json& msg, Confirmation& confirm) + { + rmf_task_msgs::msg::TaskProfile profile; + + profile.description.task_type.type = + rmf_task_msgs::msg::TaskType::TYPE_CLEAN; + + profile.description.clean.start_waypoint = msg["zone"].get(); + + legacy_converter(profile, confirm); + }); + return *this; } @@ -1237,20 +1662,51 @@ FleetUpdateHandle::default_maximum_delay() const //============================================================================== FleetUpdateHandle& FleetUpdateHandle::fleet_state_publish_period( std::optional value) +{ + return fleet_state_topic_publish_period(value); +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::fleet_state_topic_publish_period( + std::optional value) { if (value.has_value()) { - _pimpl->fleet_state_timer = _pimpl->node->try_create_wall_timer( + _pimpl->fleet_state_topic_publish_timer = + _pimpl->node->try_create_wall_timer( value.value(), [me = weak_from_this()]() { if (const auto self = me.lock()) - self->_pimpl->publish_fleet_state(); + self->_pimpl->publish_fleet_state_topic(); }); } else { - _pimpl->fleet_state_timer = nullptr; + _pimpl->fleet_state_topic_publish_timer = nullptr; + } + + return *this; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::fleet_state_update_period( + std::optional value) +{ + if (value.has_value()) + { + _pimpl->fleet_state_update_timer = + _pimpl->node->try_create_wall_timer( + value.value(), + [me = weak_from_this()] + { + if (const auto self = me.lock()) + self->_pimpl->update_fleet_state(); + }); + } + else + { + _pimpl->fleet_state_update_timer = nullptr; } return *this; @@ -1274,25 +1730,25 @@ bool FleetUpdateHandle::set_task_planner_params( (recharge_threshold >= 0.0 && recharge_threshold <= 1.0) && (recharge_soc >= 0.0 && recharge_threshold <= 1.0)) { - const rmf_task::agv::Parameters parameters{ + const rmf_task::Parameters parameters{ *_pimpl->planner, *battery_system, motion_sink, ambient_sink, tool_sink}; - const rmf_task::agv::Constraints constraints{ + const rmf_task::Constraints constraints{ recharge_threshold, recharge_soc, account_for_battery_drain}; - const rmf_task::agv::TaskPlanner::Configuration task_config{ + const rmf_task::TaskPlanner::Configuration task_config{ parameters, constraints, _pimpl->cost_calculator}; - const rmf_task::agv::TaskPlanner::Options options{ + const rmf_task::TaskPlanner::Options options{ false, nullptr, finishing_request}; - _pimpl->task_planner = std::make_shared( + _pimpl->task_planner = std::make_shared( std::move(task_config), std::move(options)); // Here we update the task planner in all the RobotContexts. diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index 4dfc66d34..2e4629ac7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -20,6 +20,8 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -91,6 +93,14 @@ std::shared_ptr Node::make( node->create_publisher( FleetStateTopicName, default_qos); + node->_task_api_request_obs = + node->create_observable( + TaskApiRequests, rclcpp::SystemDefaultsQoS().reliable().transient_local()); + + node->_task_api_response_pub = + node->create_publisher( + TaskApiResponses, rclcpp::SystemDefaultsQoS().reliable().transient_local()); + return node; } @@ -104,6 +114,15 @@ Node::Node( // Do nothing } +//============================================================================== +std::function Node::clock() const +{ + return [self = shared_from_this()]() + { + return rmf_traffic_ros2::convert(self->now()); + }; +} + //============================================================================== auto Node::door_state() const -> const DoorStateObs& { @@ -187,5 +206,17 @@ auto Node::fleet_state() const -> const FleetStatePub& return _fleet_state_pub; } +//============================================================================== +auto Node::task_api_request() const -> const ApiRequestObs& +{ + return _task_api_request_obs->observe(); +} + +//============================================================================== +auto Node::task_api_response() const -> const ApiResponsePub& +{ + return _task_api_response_pub; +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp index 30c0c5b28..f395f4bc8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.hpp @@ -36,6 +36,11 @@ #include +#include +#include + +#include + namespace rmf_fleet_adapter { namespace agv { @@ -49,6 +54,8 @@ class Node : public rmf_rxcpp::Transport const std::string& node_name, const rclcpp::NodeOptions& options); + std::function clock() const; + using DoorState = rmf_door_msgs::msg::DoorState; using DoorStateObs = rxcpp::observable; const DoorStateObs& door_state() const; @@ -105,6 +112,14 @@ class Node : public rmf_rxcpp::Transport using FleetStatePub = rclcpp::Publisher::SharedPtr; const FleetStatePub& fleet_state() const; + using ApiRequest = rmf_task_msgs::msg::ApiRequest; + using ApiRequestObs = rxcpp::observable; + const ApiRequestObs& task_api_request() const; + + using ApiResponse = rmf_task_msgs::msg::ApiResponse; + using ApiResponsePub = rclcpp::Publisher::SharedPtr; + const ApiResponsePub& task_api_response() const; + template rclcpp::TimerBase::SharedPtr try_create_wall_timer( std::chrono::duration period, @@ -160,6 +175,8 @@ class Node : public rmf_rxcpp::Transport Bridge _ingestor_result_obs; Bridge _ingestor_state_obs; FleetStatePub _fleet_state_pub; + Bridge _task_api_request_obs; + ApiResponsePub _task_api_response_pub; }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index bc6ed324a..93b3a045d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -68,6 +68,12 @@ rmf_traffic::Time RobotContext::now() const return rmf_traffic_ros2::convert(_node->now()); } +//============================================================================== +std::function RobotContext::clock() const +{ + return [self = shared_from_this()](){ return self->now(); }; +} + //============================================================================== const std::vector& RobotContext::location() const { @@ -111,6 +117,12 @@ const std::string& RobotContext::name() const return _itinerary.description().name(); } +//============================================================================== +const std::string& RobotContext::group() const +{ + return _itinerary.description().owner(); +} + //============================================================================== const std::string& RobotContext::requester_id() const { @@ -170,6 +182,18 @@ auto RobotContext::set_negotiator( shared_from_this(), negotiator); } +//============================================================================== +std::shared_ptr RobotContext::be_stubborn() +{ + return _stubbornness; +} + +//============================================================================== +bool RobotContext::is_stubborn() const +{ + return _stubbornness.use_count() > 1; +} + //============================================================================== const rxcpp::observable& RobotContext::observe_interrupt() const @@ -216,25 +240,70 @@ RobotContext& RobotContext::maximum_delay( } //============================================================================== -const rmf_task::agv::State& RobotContext::current_task_end_state() const +const rmf_task::ConstActivatorPtr& RobotContext::task_activator() const +{ + return _task_activator; +} + +//============================================================================== +const rmf_task::ConstParametersPtr& RobotContext::task_parameters() const +{ + return _task_parameters; +} + +//============================================================================== +const rmf_task::State& RobotContext::current_task_end_state() const { return _current_task_end_state; } //============================================================================== RobotContext& RobotContext::current_task_end_state( - const rmf_task::agv::State& state) + const rmf_task::State& state) { _current_task_end_state = state; return *this; } +//============================================================================== +std::function RobotContext::make_get_state() +{ + return [self = shared_from_this()]() + { + rmf_task::State state; + state.load_basic( + self->_location.front(), + self->_charger_wp, + self->_current_battery_soc); + + state.insert(GetContext{self->shared_from_this()}); + return state; + }; +} + +//============================================================================== +const std::string* RobotContext::current_task_id() const +{ + if (_current_task_id.has_value()) + return &(*_current_task_id); + + return nullptr; +} + +//============================================================================== +RobotContext& RobotContext::current_task_id(std::optional id) +{ + _current_task_id = std::move(id); + return *this; +} + //============================================================================== double RobotContext::current_battery_soc() const { return _current_battery_soc; } +//============================================================================== RobotContext& RobotContext::current_battery_soc(const double battery_soc) { _current_battery_soc = battery_soc; @@ -243,6 +312,12 @@ RobotContext& RobotContext::current_battery_soc(const double battery_soc) return *this; } +//============================================================================== +std::size_t RobotContext::dedicated_charger_wp() const +{ + return _charger_wp; +} + //============================================================================== const rxcpp::observable& RobotContext::observe_battery_soc() const { @@ -250,7 +325,7 @@ const rxcpp::observable& RobotContext::observe_battery_soc() const } //============================================================================== -const std::shared_ptr& +const std::shared_ptr& RobotContext::task_planner() const { return _task_planner; @@ -258,7 +333,7 @@ RobotContext::task_planner() const //============================================================================== auto RobotContext::task_planner( - const std::shared_ptr task_planner) + const std::shared_ptr task_planner) -> RobotContext& { _task_planner = task_planner; @@ -318,6 +393,28 @@ uint32_t RobotContext::current_mode() const return _current_mode; } +//============================================================================== +void RobotContext::action_executor( + RobotUpdateHandle::ActionExecutor action_executor) +{ + if (action_executor == nullptr) + { + RCLCPP_WARN( + _node->get_logger(), + "ActionExecutor set to nullptr for robot [%s]. If this robot needs to " + "perform an action as part of a task, a critical task error will be " + "thrown.", + this->name().c_str()); + } + _action_executor = action_executor; +} + +//============================================================================== +RobotUpdateHandle::ActionExecutor RobotContext::action_executor() const +{ + return _action_executor; +} + //============================================================================== RobotContext::RobotContext( std::shared_ptr command_handle, @@ -325,22 +422,29 @@ RobotContext::RobotContext( rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, std::shared_ptr> planner, + rmf_task::ConstActivatorPtr activator, + rmf_task::ConstParametersPtr parameters, std::shared_ptr node, const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, - rmf_task::agv::State state, - std::shared_ptr task_planner) + rmf_task::State state, + std::shared_ptr task_planner) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), _schedule(std::move(schedule)), _planner(std::move(planner)), + _task_activator(std::move(activator)), + _task_parameters(std::move(parameters)), + _stubbornness(std::make_shared(0)), _node(std::move(node)), _worker(worker), _maximum_delay(maximum_delay), _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), + _charger_wp(state.dedicated_charging_waypoint().value()), _current_task_end_state(state), + _current_task_id(std::nullopt), _task_planner(std::move(task_planner)) { _profile = std::make_shared( @@ -351,6 +455,8 @@ RobotContext::RobotContext( _battery_soc_obs = _battery_soc_publisher.get_observable(); _current_mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE; + + _action_executor = nullptr; } } // namespace agv 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 6f2a003f2..112ed9f2a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -9,9 +9,10 @@ #include #include -#include -#include -#include +#include +#include +#include +#include #include @@ -49,6 +50,9 @@ class RobotContext /// Get the current time rmf_traffic::Time now() const; + /// Get a clock that can be used by task loggers + std::function clock() const; + /// This is the current "location" of the robot, which can be used to initiate /// a planning job const std::vector& location() const; @@ -73,6 +77,9 @@ class RobotContext /// Get the name of this robot const std::string& name() const; + /// Get the group (fleet) of this robot + const std::string& group() const; + /// Get the requester ID to use for this robot when sending requests const std::string& requester_id() const; @@ -90,6 +97,13 @@ class RobotContext std::shared_ptr set_negotiator( rmf_traffic::schedule::Negotiator* negotiator); + /// This function will indicate that GoToPlace should have a stubborn + /// negotiation behavior for as long as the returned handle is alive. + std::shared_ptr be_stubborn(); + + /// If anything is holding onto a be_stubborn handle, this will return true. + bool is_stubborn() const; + struct Empty {}; const rxcpp::observable& observe_interrupt() const; @@ -116,12 +130,28 @@ class RobotContext const TableViewerPtr& table_viewer, const ResponderPtr& responder) final; + /// Get the task activator for this robot + const rmf_task::ConstActivatorPtr& task_activator() const; + + const rmf_task::ConstParametersPtr& task_parameters() const; + /// Set the state of this robot at the end of its current task - RobotContext& current_task_end_state(const rmf_task::agv::State& state); + RobotContext& current_task_end_state(const rmf_task::State& state); /// Get a mutable reference to the state of this robot at the end of its // current task - const rmf_task::agv::State& current_task_end_state() const; + const rmf_task::State& current_task_end_state() const; + + /// Make a way to get the state for this robot + std::function make_get_state(); + + /// Get the current task ID of the robot, or a nullptr if the robot is not + /// performing any task + const std::string* current_task_id() const; + + /// Set the current task ID of the robot, or give a nullopt if a task is not + /// being performed. + RobotContext& current_task_id(std::optional id); /// Get the current battery state of charge double current_battery_soc() const; @@ -130,15 +160,17 @@ class RobotContext /// publishes the battery soc via _battery_soc_publisher. RobotContext& current_battery_soc(const double battery_soc); + std::size_t dedicated_charger_wp() const; + // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; /// Get a mutable reference to the task planner for this robot - const std::shared_ptr& task_planner() const; + const std::shared_ptr& task_planner() const; /// Set the task planner for this robot RobotContext& task_planner( - const std::shared_ptr task_planner); + const std::shared_ptr task_planner); void set_lift_entry_watchdog( RobotUpdateHandle::Unstable::Watchdog watchdog, @@ -155,6 +187,14 @@ class RobotContext /// Return the current mode of the robot uint32_t current_mode() const; + /// Set the action executor for requesting this robot to execute a + /// PerformAction activity + void action_executor(RobotUpdateHandle::ActionExecutor action_executor); + + /// Get the action executor for requesting this robot to execute a + /// PerformAction activity + RobotUpdateHandle::ActionExecutor action_executor() const; + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -165,20 +205,25 @@ class RobotContext rmf_traffic::schedule::Participant itinerary, std::shared_ptr schedule, std::shared_ptr> planner, + rmf_task::ConstActivatorPtr activator, + rmf_task::ConstParametersPtr parameters, std::shared_ptr node, const rxcpp::schedulers::worker& worker, rmf_utils::optional maximum_delay, - rmf_task::agv::State state, - std::shared_ptr task_planner); + rmf_task::State state, + std::shared_ptr task_planner); std::weak_ptr _command_handle; std::vector _location; rmf_traffic::schedule::Participant _itinerary; std::shared_ptr _schedule; std::shared_ptr> _planner; + rmf_task::ConstActivatorPtr _task_activator; + rmf_task::ConstParametersPtr _task_parameters; std::shared_ptr _profile; std::shared_ptr _negotiation_license; + std::shared_ptr _stubbornness; rxcpp::subjects::subject _interrupt_publisher; rxcpp::observable _interrupt_obs; @@ -192,21 +237,31 @@ class RobotContext /// Always call the current_battery_soc() setter to set a new value double _current_battery_soc = 1.0; + std::size_t _charger_wp; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; - rmf_task::agv::State _current_task_end_state; - std::shared_ptr _task_planner; + rmf_task::State _current_task_end_state; + std::optional _current_task_id; + std::shared_ptr _task_planner; RobotUpdateHandle::Unstable::Watchdog _lift_watchdog; rmf_traffic::Duration _lift_rewait_duration = std::chrono::seconds(0); // Mode value for RobotMode message uint32_t _current_mode; + + RobotUpdateHandle::ActionExecutor _action_executor; }; using RobotContextPtr = std::shared_ptr; using ConstRobotContextPtr = std::shared_ptr; +//============================================================================== +struct GetContext +{ + RobotContextPtr value; +}; + } // namespace agv } // namespace rmf_fleet_adapter 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 148c21d44..324194719 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -173,7 +173,7 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( if (const auto context = _pimpl->get_context()) { auto end_state = context->current_task_end_state(); - end_state.charging_waypoint(charger_wp); + end_state.dedicated_charging_waypoint(charger_wp); context->current_task_end_state(end_state); RCLCPP_INFO( context->node()->get_logger(), @@ -201,6 +201,20 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc) } } +//============================================================================== +void RobotUpdateHandle::set_action_executor( + RobotUpdateHandle::ActionExecutor action_executor) +{ + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [context, action_executor](const auto&) + { + context->action_executor(action_executor); + }); + } +} + //============================================================================== RobotUpdateHandle& RobotUpdateHandle::maximum_delay( rmf_utils::optional value) @@ -272,5 +286,43 @@ void RobotUpdateHandle::Unstable::set_lift_entry_watchdog( } } +//============================================================================== +void RobotUpdateHandle::ActionExecution::update_remaining_time( + rmf_traffic::Duration remaining_time_estimate) +{ + _pimpl->data->remaining_time = remaining_time_estimate; +} + +//============================================================================== +void RobotUpdateHandle::ActionExecution::finished() +{ + if (!_pimpl->data->finished) + return; + + _pimpl->data->finished(); + _pimpl->data->finished = nullptr; +} + +//============================================================================== +bool RobotUpdateHandle::ActionExecution::okay() const +{ + return _pimpl->data->okay; +} + +//============================================================================== +RobotUpdateHandle::ActionExecution::ActionExecution() +{ + // Do nothing +} + +//============================================================================== +RobotUpdateHandle::ActionExecution::~ActionExecution() +{ + // Automatically trigger finished when this object dies + if (_pimpl->data->finished) + _pimpl->data->finished(); +} + + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 3c9eb87ba..e10da1a7d 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 @@ -20,16 +20,21 @@ #include -#include -#include -#include +#include + +#include #include -#include +#include #include #include #include +#include +#include +#include +#include + #include #include @@ -38,6 +43,9 @@ #include "Node.hpp" #include "RobotContext.hpp" #include "../TaskManager.hpp" +#include "../BroadcastClient.hpp" +#include "../DeserializeJSON.hpp" +#include "../DatabaseLogger.hpp" #include #include @@ -48,6 +56,15 @@ #include #include +#include +#include +#include +#include +#include +#include + +#include + #include #include #include @@ -55,6 +72,90 @@ namespace rmf_fleet_adapter { namespace agv { +//============================================================================== +struct TaskActivation +{ + rmf_task::ActivatorPtr task; + rmf_task_sequence::Phase::ActivatorPtr phase; + rmf_task_sequence::Event::InitializerPtr event; +}; + +//============================================================================== +template +struct DeserializedDescription +{ + T description; + std::vector errors; +}; + +//============================================================================== +using DeserializedTask = + DeserializedDescription>; + +//============================================================================== +using TaskDescriptionDeserializer = + std::function; + +//============================================================================== +using DeserializedPhase = + DeserializedDescription< + std::shared_ptr + >; + +//============================================================================== +using PhaseDescriptionDeserializer = + std::function; + +//============================================================================== +using DeserializedEvent = + DeserializedDescription< + std::shared_ptr + >; + +//============================================================================== +using EventDescriptionDeserializer = + std::function; + +//============================================================================== +using DeserializedPlace = + DeserializedDescription>; + +//============================================================================== +using PlaceDeserializer = + std::function; + +//============================================================================== +struct TaskDeserialization +{ + DeserializeJSONPtr task; + DeserializeJSONPtr phase; + DeserializeJSONPtr event; + PlaceDeserializer place; + + std::shared_ptr consider_pickup; + std::shared_ptr consider_dropoff; + std::shared_ptr consider_clean; + std::shared_ptr consider_patrol; + std::shared_ptr consider_composed; + // Map category string to its ConsiderRequest for PerformAction events + std::shared_ptr> consider_actions; + + void add_schema(const nlohmann::json& schema); + + nlohmann::json_schema::json_validator make_validator( + nlohmann::json schema) const; + + std::shared_ptr make_validator_shared( + nlohmann::json schema) const; + + TaskDeserialization(); +private: + using SchemaDictionary = std::unordered_map; + std::shared_ptr _schema_dictionary; + std::function _loader; +}; + //============================================================================== /// This abstract interface class allows us to use the same implementation of /// FleetUpdateHandle whether we are running it in a distributed system or in a @@ -130,6 +231,7 @@ class FleetUpdateHandle::Implementation { public: + std::weak_ptr weak_self; std::string name; std::shared_ptr> planner; std::shared_ptr node; @@ -137,11 +239,18 @@ class FleetUpdateHandle::Implementation std::shared_ptr writer; std::shared_ptr snappable; std::shared_ptr negotiation; + std::optional server_uri; + // Database for backup and restore + std::shared_ptr db; + + TaskActivation activation = TaskActivation(); + TaskDeserialization deserialization = TaskDeserialization(); - // Task planner params + // LegacyTask planner params std::shared_ptr cost_calculator = rmf_task::BinaryPriorityScheme::make_cost_calculator(); - std::shared_ptr task_planner = nullptr; + std::shared_ptr task_parameters = nullptr; + std::shared_ptr task_planner = nullptr; rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); @@ -150,45 +259,48 @@ class FleetUpdateHandle::Implementation std::unordered_map> task_managers = {}; + std::shared_ptr broadcast_client = nullptr; + // Map uri to schema for validator loader function + std::unordered_map schema_dictionary = {}; + rclcpp::Publisher::SharedPtr fleet_state_pub = nullptr; - rclcpp::TimerBase::SharedPtr fleet_state_timer = nullptr; + rclcpp::TimerBase::SharedPtr fleet_state_topic_publish_timer = nullptr; + rclcpp::TimerBase::SharedPtr fleet_state_update_timer = nullptr; + + using DockParamMap = + std::unordered_map< + std::string, + rmf_fleet_msgs::msg::DockParameter + >; - // Map task id to pair of - using Assignments = rmf_task::agv::TaskPlanner::Assignments; + using ConstDockParamsPtr = std::shared_ptr; // Map of dock name to dock parameters - std::unordered_map dock_param_map = {}; + std::shared_ptr dock_param_map = + std::make_shared(); // TODO Support for various charging configurations std::unordered_set charging_waypoints = {}; - double current_assignment_cost = 0.0; - // Map to store task id with assignments for BidNotice - std::unordered_map bid_notice_assignments = {}; + // Map task_id to schemas::task_reqest.json + // TODO(YV): How would we insert request jsons for automatic tasks? + // Perhaps we should just deserialize in DatabaseLogger + std::unordered_map task_request_jsons = {}; - std::unordered_map< - std::string, rmf_task::ConstRequestPtr> generated_requests = {}; - std::unordered_map< - std::string, rmf_task::ConstRequestPtr> assigned_requests = {}; - std::unordered_set cancelled_task_ids = {}; - using TaskProfileMsg = rmf_task_msgs::msg::TaskProfile; - std::unordered_map task_profile_map = {}; + std::shared_ptr bidder = nullptr; - AcceptTaskRequest accept_task = nullptr; - - using BidNotice = rmf_task_msgs::msg::BidNotice; - using BidNoticeSub = rclcpp::Subscription::SharedPtr; - BidNoticeSub bid_notice_sub = nullptr; + double current_assignment_cost = 0.0; + // Map to store task id with assignments for BidNotice + using Assignments = rmf_task::TaskPlanner::Assignments; + using BidNoticeAssignments = std::unordered_map; + BidNoticeAssignments bid_notice_assignments = {}; - using BidProposal = rmf_task_msgs::msg::BidProposal; - using BidProposalPub = rclcpp::Publisher::SharedPtr; - BidProposalPub bid_proposal_pub = nullptr; + using BidNoticeMsg = rmf_task_msgs::msg::BidNotice; - using DispatchRequest = rmf_task_msgs::msg::DispatchRequest; - using DispatchRequestSub = rclcpp::Subscription::SharedPtr; - DispatchRequestSub dispatch_request_sub = nullptr; + using DispatchCmdMsg = rmf_task_msgs::msg::DispatchCommand; + using DispatchCommandSub = rclcpp::Subscription::SharedPtr; + DispatchCommandSub dispatch_command_sub = nullptr; using DispatchAck = rmf_task_msgs::msg::DispatchAck; using DispatchAckPub = rclcpp::Publisher::SharedPtr; @@ -203,51 +315,56 @@ class FleetUpdateHandle::Implementation { auto handle = std::shared_ptr(new FleetUpdateHandle); handle->_pimpl = rmf_utils::make_unique_impl( - Implementation{std::forward(args)...}); + Implementation{handle, std::forward(args)...}); + + handle->_pimpl->add_standard_tasks(); + + // TODO(MXG): This is a very crude implementation. We create a dummy set of + // task planner parameters to stand in until the user sets the task planner + // parameters. We'll distribute this shared_ptr to the robot contexts and + // update it with the proper values once they're available. + // + // Note that we also need to manually update the traffic planner when it + // changes. + handle->_pimpl->task_parameters = + std::make_shared( + *handle->_pimpl->planner, + *rmf_battery::agv::BatterySystem::make(1.0, 1.0, 1.0), + nullptr, nullptr, nullptr); handle->_pimpl->fleet_state_pub = handle->_pimpl->node->fleet_state(); - handle->_pimpl->fleet_state_timer = - handle->_pimpl->node->try_create_wall_timer( - std::chrono::seconds(1), [me = handle->weak_from_this()]() - { - if (const auto self = me.lock()) - self->_pimpl->publish_fleet_state(); - }); + handle->fleet_state_topic_publish_period(std::chrono::seconds(1)); + handle->fleet_state_update_period(std::chrono::seconds(1)); // Create subs and pubs for bidding - auto default_qos = rclcpp::SystemDefaultsQoS(); - auto transient_qos = rclcpp::QoS(10); transient_qos.transient_local(); + auto transient_qos = rclcpp::QoS(10).transient_local(); + auto reliable_transient_qos = + rclcpp::ServicesQoS().keep_last(20).transient_local(); - // Publish BidProposal - handle->_pimpl->bid_proposal_pub = - handle->_pimpl->node->create_publisher( - BidProposalTopicName, default_qos); + // Subscribe DispatchCommand + handle->_pimpl->dispatch_command_sub = + handle->_pimpl->node->create_subscription( + DispatchRequestTopicName, + reliable_transient_qos, + [w = handle->weak_from_this()](const DispatchCmdMsg::SharedPtr msg) + { + if (const auto self = w.lock()) + self->_pimpl->dispatch_command_cb(msg); + }); // Publish DispatchAck handle->_pimpl->dispatch_ack_pub = handle->_pimpl->node->create_publisher( - DispatchAckTopicName, default_qos); - - // Subscribe BidNotice - handle->_pimpl->bid_notice_sub = - handle->_pimpl->node->create_subscription( - BidNoticeTopicName, - default_qos, - [w = handle->weak_from_this()](const BidNotice::SharedPtr msg) - { - if (const auto self = w.lock()) - self->_pimpl->bid_notice_cb(msg); - }); + DispatchAckTopicName, reliable_transient_qos); - // Subscribe DispatchRequest - handle->_pimpl->dispatch_request_sub = - handle->_pimpl->node->create_subscription( - DispatchRequestTopicName, - default_qos, - [w = handle->weak_from_this()](const DispatchRequest::SharedPtr msg) + // Make a dispatch bidder + handle->_pimpl->bidder = rmf_task_ros2::bidding::AsyncBidder::make( + handle->_pimpl->node, + [w = handle->weak_from_this()]( + const auto& msg, auto respond) { if (const auto self = w.lock()) - self->_pimpl->dispatch_request_cb(msg); + self->_pimpl->bid_notice_cb(msg, std::move(respond)); }); // Subscribe DockSummary @@ -269,24 +386,133 @@ class FleetUpdateHandle::Implementation handle->_pimpl->charging_waypoints.insert(i); } + // Initialize schema dictionary + auto schema = rmf_api_msgs::schemas::fleet_state_update; + nlohmann::json_uri json_uri = nlohmann::json_uri{schema["$id"]}; + handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema}); + schema = rmf_api_msgs::schemas::fleet_state; + json_uri = nlohmann::json_uri{schema["$id"]}; + handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema}); + schema = rmf_api_msgs::schemas::robot_state; + json_uri = nlohmann::json_uri{schema["$id"]}; + handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema}); + schema = rmf_api_msgs::schemas::location_2D; + json_uri = nlohmann::json_uri{schema["$id"]}; + handle->_pimpl->schema_dictionary.insert({json_uri.url(), schema}); + + // Start the BroadcastClient + if (handle->_pimpl->server_uri.has_value()) + { + handle->_pimpl->broadcast_client = BroadcastClient::make( + handle->_pimpl->server_uri.value(), + handle->weak_from_this()); + } + + // Add PerformAction event to deserialization + auto validator = handle->_pimpl->deserialization.make_validator_shared( + schemas::event_description_PerformAction); + + const auto deserializer = + [validator, + place = handle->_pimpl->deserialization.place, + consider_actions = handle->_pimpl->deserialization.consider_actions]( + const nlohmann::json& msg) -> DeserializedEvent + { + try + { + validator->validate(msg); + const std::string& category = msg["category"].get(); + const auto consider_action_it = consider_actions->find(category); + if (consider_action_it == consider_actions->end()) + { + return {nullptr, {"Fleet not configured to perform this action"}}; + } + Confirmation confirm; + const auto& consider = consider_action_it->second; + consider(msg, confirm); + if (!confirm.is_accepted()) + { + return {nullptr, confirm.errors()}; + } + + const nlohmann::json desc = msg["description"]; + rmf_traffic::Duration duration_estimate = rmf_traffic::Duration(0); + bool use_tool_sink = false; + std::optional finish_location = + std::nullopt; + auto it = msg.find("unix_millis_action_duration_estimate"); + if (it != msg.end()) + { + duration_estimate = + rmf_traffic::Duration( + std::chrono::milliseconds(it->get())); + } + it = msg.find("use_tool_sink"); + if (it != msg.end()) + { + use_tool_sink = it->get(); + } + it = msg.find("expected_finish_location"); + if (it != msg.end()) + { + auto deser_place = + place(msg["expected_finish_location"]); + if (!deser_place.description.has_value()) + { + return {nullptr, deser_place.errors}; + } + finish_location = deser_place.description.value(); + } + + const auto description = + rmf_task_sequence::events::PerformAction::Description::make( + category, + desc, + duration_estimate, + use_tool_sink, + finish_location); + + std::vector errors = {}; + return {description, errors}; + } + catch(const std::exception& e) + { + return {nullptr, {e.what()}}; + } + }; + + handle->_pimpl->deserialization.event->add( + "perform_action", validator, deserializer); + return handle; } void dock_summary_cb(const DockSummary::SharedPtr& msg); - void bid_notice_cb(const BidNotice::SharedPtr msg); + void bid_notice_cb( + const BidNoticeMsg& msg, + rmf_task_ros2::bidding::AsyncBidder::Respond respond); - void dispatch_request_cb(const DispatchRequest::SharedPtr msg); + void dispatch_command_cb(const DispatchCmdMsg::SharedPtr msg); std::optional get_nearest_charger( const rmf_traffic::agv::Planner::Start& start); + struct Expectations + { + std::vector states; + std::vector pending_requests; + }; + + Expectations aggregate_expectations() const; + /// Generate task assignments for a collection of task requests comprising of /// task requests currently in TaskManager queues while optionally including a /// new request and while optionally ignoring a specific request. std::optional allocate_tasks( rmf_task::ConstRequestPtr new_request = nullptr, - rmf_task::ConstRequestPtr ignore_request = nullptr) const; + std::vector* errors = nullptr, + std::optional expectations = std::nullopt) const; /// Helper function to check if assignments are valid. An assignment set is /// invalid if one of the assignments has already begun execution. @@ -302,10 +528,19 @@ class FleetUpdateHandle::Implementation return *fleet._pimpl; } - void fleet_state_publish_period( - std::optional value); + void publish_fleet_state_topic() const; + + void update_fleet_state() const; + + void add_standard_tasks(); + + std::string make_error_str( + uint64_t code, std::string category, std::string detail) const; - void publish_fleet_state() const; + std::shared_ptr convert( + const std::string& task_id, + const nlohmann::json& request_msg, + std::vector& errors) const; }; } // namespace agv 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 2793c27f0..e3a86f5d0 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 @@ -25,10 +25,44 @@ namespace rmf_fleet_adapter { namespace agv { //============================================================================== -class RobotUpdateHandle::Implementation +class RobotUpdateHandle::ActionExecution::Implementation { public: + struct Data + { + + std::function finished; + std::optional remaining_time; + bool okay; + // TODO: Consider adding a mutex to lock read/write + + Data( + std::function finished_, + std::optional remaining_time_ = std::nullopt) + { + finished = std::move(finished_); + remaining_time = remaining_time_; + okay = true; + } + }; + + static ActionExecution make(std::shared_ptr data) + { + ActionExecution execution; + execution._pimpl = rmf_utils::make_impl( + Implementation{std::move(data)}); + + return execution; + } + + std::shared_ptr data; +}; + +//============================================================================== +class RobotUpdateHandle::Implementation +{ +public: std::weak_ptr context; std::string name; RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable(); @@ -59,7 +93,6 @@ class RobotUpdateHandle::Implementation std::shared_ptr get_context(); std::shared_ptr get_context() const; - }; } // namespace agv diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp index d7d8fa419..1950b817e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/test/MockAdapter.cpp @@ -227,7 +227,8 @@ MockAdapter::MockAdapter( std::shared_ptr MockAdapter::add_fleet( const std::string& fleet_name, rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph) + rmf_traffic::agv::Graph navigation_graph, + std::optional server_uri) { auto planner = std::make_shared>( @@ -240,7 +241,7 @@ std::shared_ptr MockAdapter::add_fleet( auto fleet = FleetUpdateHandle::Implementation::make( fleet_name, std::move(planner), _pimpl->node, _pimpl->worker, std::make_shared(_pimpl->schedule), - _pimpl->schedule->snappable(), nullptr); + _pimpl->schedule->snappable(), nullptr, server_uri); _pimpl->fleets.push_back(fleet); return fleet; @@ -306,29 +307,50 @@ void MockAdapter::stop() } //============================================================================== -void MockAdapter::dispatch_task(const rmf_task_msgs::msg::TaskProfile& profile) +void MockAdapter::dispatch_task( + std::string task_id, + const nlohmann::json& request) { - _pimpl->worker.schedule([profile, fleets = _pimpl->fleets](const auto&) + _pimpl->worker.schedule( + [ + request, + task_id = std::move(task_id), + fleets = _pimpl->fleets + ](const auto&) { for (auto& fleet : fleets) { auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); - if (!fimpl.accept_task) - continue; - // NOTE: althought the current adapter supports multiple fleets. The test + // NOTE: although the current adapter supports multiple fleets. The test // here assumses using a single fleet for each adapter - rmf_task_msgs::msg::BidNotice bid; - bid.task_profile = profile; + bool accepted = false; + auto bid = rmf_task_msgs::build() + .request(request) + .task_id(task_id) + .time_window(rclcpp::Duration(2, 0)); + fimpl.bid_notice_cb( - std::make_shared(bid)); - - rmf_task_msgs::msg::DispatchRequest req; - req.task_profile = profile; - req.fleet_name = fimpl.name; - req.method = req.ADD; - fimpl.dispatch_request_cb( - std::make_shared(req)); + bid, + [&accepted](const rmf_task_ros2::bidding::Response& response) + { + accepted = response.proposal.has_value(); + }); + + if (accepted) + { + rmf_task_msgs::msg::DispatchCommand req; + req.task_id = task_id; + req.fleet_name = fimpl.name; + req.type = req.TYPE_AWARD; + fimpl.dispatch_command_cb( + std::make_shared(req)); + } + else + { + std::cout << "Fleet [" << fimpl.name << "] rejected the task request" + << std::endl; + } } }); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp new file mode 100644 index 000000000..121fac64a --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.cpp @@ -0,0 +1,319 @@ +/* + * Copyright (C) 2021 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 "EmergencyPullover.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +auto EmergencyPullover::Standby::make( + const AssignIDPtr& id, + const agv::RobotContextPtr& context, + std::function update) -> std::shared_ptr +{ + auto standby = std::make_shared(); + standby->_assign_id = id; + standby->_context = context; + standby->_update = std::move(update); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + "Emergency pullover", + "", + rmf_task::Event::Status::Standby, + {}, + context->clock()); + + return standby; +} + +//============================================================================== +auto EmergencyPullover::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration EmergencyPullover::Standby::duration_estimate() const +{ + // An emergency pullover will last indefinitely until it gets cancelled, which + // may happen at any time. + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto EmergencyPullover::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + if (!_active) + { + _active = Active::make( + _assign_id, + _context, + _state, + _update, + std::move(finished)); + } + + return _active; +} + +//============================================================================== +auto EmergencyPullover::Active::make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) -> std::shared_ptr +{ + auto active = std::make_shared(); + active->_assign_id = id; + active->_context = std::move(context); + active->_update = std::move(update); + active->_finished = std::move(finished); + active->_state = std::move(state); + active->_negotiator = + Negotiator::make( + active->_context, + [w = active->weak_from_this()]( + const auto& t, const auto& r) -> Negotiator::NegotiatePtr + { + if (const auto self = w.lock()) + return self->_respond(t, r); + + r->forfeit({}); + return nullptr; + }); + + active->_find_plan(); + return active; +} + +//============================================================================== +auto EmergencyPullover::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration EmergencyPullover::Active::remaining_time_estimate() const +{ + // An emergency pullover will last indefinitely until it gets cancelled, which + // may happen at any time. + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto EmergencyPullover::Active::backup() const -> Backup +{ + // EmergencyPullover doesn't need to be backed up + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto EmergencyPullover::Active::interrupt( + std::function task_is_interrupted) -> Resume +{ + _negotiator->clear_license(); + _is_interrupted = true; + _execution = std::nullopt; + + _state->update_status(Status::Standby); + _state->update_log().info("Going into standby for an interruption"); + _state->update_dependencies({}); + + _context->worker().schedule( + [task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + + return Resume::make( + [w = weak_from_this()]() + { + if (const auto self = w.lock()) + { + self->_negotiator->claim_license(); + self->_is_interrupted = false; + self->_find_plan(); + } + }); +} + +//============================================================================== +void EmergencyPullover::Active::cancel() +{ + _execution = std::nullopt; + _state->update_status(Status::Canceled); + _state->update_log().info("Received signal to cancel"); + _finished(); +} + +//============================================================================== +void EmergencyPullover::Active::kill() +{ + _execution = std::nullopt; + _state->update_status(Status::Killed); + _state->update_log().info("Received signal to kill"); + _finished(); +} + +//============================================================================== +void EmergencyPullover::Active::_find_plan() +{ + if (_is_interrupted) + return; + + _state->update_status(Status::Underway); + _state->update_log().info("Searching for an emergency pullover"); + + _find_pullover_service = std::make_shared( + _context->planner(), _context->location(), _context->schedule()->snapshot(), + _context->itinerary().id(), _context->profile()); + + _pullover_subscription = + rmf_rxcpp::make_job( + _find_pullover_service) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [w = weak_from_this()]( + const services::FindEmergencyPullover::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + if (!result) + { + // The planner could not find any pullover + self->_state->update_status(Status::Error); + self->_state->update_log().error("Failed to find a pullover"); + + self->_execution = std::nullopt; + self->_schedule_retry(); + + self->_context->worker().schedule( + [update = self->_update](const auto&){ update(); }); + + return; + } + + self->_state->update_status(Status::Underway); + self->_state->update_log().info("Found an emergency pullover"); + + self->_execute_plan(*std::move(result)); + self->_find_pullover_service = nullptr; + self->_retry_timer = nullptr; + }); + + _find_pullover_timeout = _context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = _find_pullover_service->weak_from_this(), + weak_self = weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + service->interrupt(); + + if (const auto self = weak_self.lock()) + self->_find_pullover_timeout = nullptr; + }); + + _update(); +} + +//============================================================================== +void EmergencyPullover::Active::_schedule_retry() +{ + if (_retry_timer) + return; + + // TODO(MXG): Make the retry timing configurable + _retry_timer = _context->node()->try_create_wall_timer( + std::chrono::seconds(5), + [w = weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_retry_timer = nullptr; + if (self->_execution.has_value()) + return; + + self->_find_plan(); + }); +} + +//============================================================================== +void EmergencyPullover::Active::_execute_plan(rmf_traffic::agv::Plan plan) +{ + if (_is_interrupted) + return; + + _execution = ExecutePlan::make( + _context, std::move(plan), _assign_id, _state, + _update, _finished, std::nullopt); + + if (!_execution.has_value()) + { + _state->update_status(Status::Error); + _state->update_log().error( + "Invalid (empty) plan generated. Will retry soon. " + "Please report this incident to the Open-RMF developers."); + _schedule_retry(); + } +} + +//============================================================================== +Negotiator::NegotiatePtr EmergencyPullover::Active::_respond( + const Negotiator::TableViewerPtr& table_view, + const Negotiator::ResponderPtr& responder) +{ + if (_context->is_stubborn()) + { + rmf_traffic::schedule::StubbornNegotiator(_context->itinerary()) + .respond(table_view, responder); + return nullptr; + } + + auto approval_cb = [w = weak_from_this()]( + const rmf_traffic::agv::Plan& plan) + -> std::optional + { + if (auto self = w.lock()) + { + self->_execute_plan(plan); + return self->_context->itinerary().version(); + } + + return std::nullopt; + }; + + const auto evaluator = Negotiator::make_evaluator(table_view); + return services::Negotiate::emergency_pullover( + _context->planner(), _context->location(), table_view, + responder, std::move(approval_cb), std::move(evaluator)); +} + + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp new file mode 100644 index 000000000..c6ca1db58 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/EmergencyPullover.hpp @@ -0,0 +1,122 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP + +#include "../agv/RobotContext.hpp" +#include "../Negotiator.hpp" + +#include "../services/FindEmergencyPullover.hpp" + +#include "ExecutePlan.hpp" + +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class EmergencyPullover : public rmf_task_sequence::Event +{ +public: + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + const AssignIDPtr& id, + const agv::RobotContextPtr& context, + std::function update); + + ConstStatePtr state() const; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + + static std::shared_ptr make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + + void _schedule_retry(); + + void _find_plan(); + + void _execute_plan(rmf_traffic::agv::Plan plan); + + Negotiator::NegotiatePtr _respond( + const Negotiator::TableViewerPtr& table_view, + const Negotiator::ResponderPtr& responder); + + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + std::function _update; + std::function _finished; + rmf_task::events::SimpleEventStatePtr _state; + std::shared_ptr _negotiator; + std::optional _execution; + std::shared_ptr _find_pullover_service; + rmf_rxcpp::subscription_guard _pullover_subscription; + rclcpp::TimerBase::SharedPtr _find_pullover_timeout; + rclcpp::TimerBase::SharedPtr _retry_timer; + + bool _is_interrupted = false; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__EMERGENCYPULLOVER_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp new file mode 100644 index 000000000..3f95abef3 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.cpp @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2021 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 "Error.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +auto Error::Standby::make( + rmf_task::events::SimpleEventStatePtr state, + Behavior behavior) -> std::shared_ptr +{ + auto standby = std::make_shared(); + standby->_state = std::move(state); + standby->_behavior = behavior; + + // Make sure the status is set to an error + standby->_state->update_status(Status::Error); + + return standby; +} + +//============================================================================== +auto Error::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration Error::Standby::duration_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto Error::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + if (!_active) + { + _active = Active::make(_state, std::move(finished), _behavior); + } + + return _active; +} + +//============================================================================== +auto Error::Active::make( + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Behavior behavior) -> std::shared_ptr +{ + auto active = std::make_shared(); + active->_state = std::move(state); + active->_finished = std::move(finished); + + active->_state->update_status(Status::Error); + + if (behavior == Behavior::Continue) + active->_finished(); + + return active; +} + +//============================================================================== +auto Error::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration Error::Active::remaining_time_estimate() const +{ + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto Error::Active::backup() const -> Backup +{ + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto Error::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + task_is_interrupted(); + return Resume::make([](){}); +} + +//============================================================================== +void Error::Active::cancel() +{ + _finished(); +} + +//============================================================================== +void Error::Active::kill() +{ + _finished(); +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp new file mode 100644 index 000000000..56138e630 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/Error.hpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP + +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class Error : public rmf_task_sequence::Event +{ +public: + + enum class Behavior + { + /// This Error event will block until an operator explicitly cancels, + /// kills, skips, or rewinds the task. This is the recommended behavior + /// because an event error usually means something in the task process went + /// wrong and needs to be retried. + Block = 0, + + /// This Error event will report that the error occurred but then finish as + /// soon as it is activated. + Continue + }; + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + rmf_task::events::SimpleEventStatePtr state, + Behavior behavior = Behavior::Block); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + rmf_task::events::SimpleEventStatePtr _state; + Behavior _behavior; + ActivePtr _active; + }; + + class Active : public rmf_task_sequence::Event::Active + { + public: + + static std::shared_ptr make( + rmf_task::events::SimpleEventStatePtr state, + std::function finished, + Behavior behavior = Behavior::Block); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + rmf_task::events::SimpleEventStatePtr _state; + std::function _finished; + }; + +}; + +} // namespace task +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__ERROR_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp new file mode 100644 index 000000000..73038ce73 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -0,0 +1,520 @@ +/* + * Copyright (C) 2021 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 "ExecutePlan.hpp" +#include "LegacyPhaseShim.hpp" + +#include "../phases/MoveRobot.hpp" +#include "../phases/DoorOpen.hpp" +#include "../phases/DoorClose.hpp" +#include "../phases/RequestLift.hpp" +#include "../phases/DockRobot.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +namespace { +//============================================================================== +using StandbyPtr = rmf_task_sequence::Event::StandbyPtr; +using UpdateFn = std::function; +using MakeStandby = std::function; +using LegacyPhases = std::vector>; + +using DockRobot = phases::DockRobot::PendingPhase; +using DoorOpen = phases::DoorOpen::PendingPhase; +using DoorClose = phases::DoorClose::PendingPhase; +using RequestLift = phases::RequestLift::PendingPhase; +using EndLift = phases::EndLiftSession::Pending; +using Move = phases::MoveRobot::PendingPhase; + +//============================================================================== +class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor +{ +public: + + using Lane = rmf_traffic::agv::Graph::Lane; + + EventPhaseFactory( + agv::RobotContextPtr context, + LegacyPhases& phases, + rmf_traffic::Time event_start_time, + bool& continuous) + : _context(std::move(context)), + _phases(phases), + _event_start_time(event_start_time), + _continuous(continuous) + { + // Do nothing + } + + void execute(const Dock& dock) final + { + assert(!_moving_lift); + _phases.push_back( + std::make_shared( + _context, dock.dock_name())); + _continuous = false; + } + + void execute(const DoorOpen& open) final + { + assert(!_moving_lift); + const auto node = _context->node(); + _phases.push_back( + std::make_shared( + _context, + open.name(), + _context->requester_id(), + _event_start_time + open.duration())); + _continuous = true; + } + + void execute(const DoorClose& close) final + { + assert(!_moving_lift); + + // TODO(MXG): Account for event duration in this phase + const auto node = _context->node(); + _phases.push_back( + std::make_shared( + _context, + close.name(), + _context->requester_id())); + _continuous = true; + } + + void execute(const LiftSessionBegin& open) final + { + assert(!_moving_lift); + const auto node = _context->node(); + _phases.push_back( + std::make_shared( + _context, + open.lift_name(), + open.floor_name(), + _event_start_time, + phases::RequestLift::Located::Outside)); + + _continuous = true; + } + + void execute(const LiftMove& move) final + { + // TODO(MXG): We should probably keep track of what lift is being moved to + // make sure we weren't given a broken nav graph + _lifting_duration += move.duration(); + _moving_lift = true; + + _continuous = true; + } + + void execute(const LiftDoorOpen& open) final + { + const auto node = _context->node(); + + // TODO(MXG): The time calculation here should be considered more carefully. + _phases.push_back( + std::make_shared( + _context, + open.lift_name(), + open.floor_name(), + _event_start_time + open.duration() + _lifting_duration, + phases::RequestLift::Located::Inside)); + _moving_lift = false; + + _continuous = true; + } + + void execute(const LiftSessionEnd& close) final + { + assert(!_moving_lift); + const auto node = _context->node(); + _phases.push_back( + std::make_shared( + _context, + close.lift_name(), + close.floor_name())); + + _continuous = true; + } + + void execute(const Wait&) final + { + // Do nothing + } + + bool moving_lift() const + { + return _moving_lift; + } + +private: + agv::RobotContextPtr _context; + std::vector>& _phases; + rmf_traffic::Time _event_start_time; + bool& _continuous; + bool _moving_lift = false; + rmf_traffic::Duration _lifting_duration = rmf_traffic::Duration(0); +}; + + +//============================================================================== +struct EventGroupInfo +{ + MakeStandby group; + LegacyPhases::const_iterator tail; +}; + +//============================================================================== +std::optional search_for_door_group( + LegacyPhases::const_iterator head, + LegacyPhases::const_iterator end, + const agv::RobotContextPtr& context, + const rmf_task::Event::AssignIDPtr& id) +{ + const auto* door_open = dynamic_cast( + head->get()); + + if (!door_open) + return std::nullopt; + + // Look for a door close event for this same door + auto tail = head; + ++tail; + auto moving_duration = rmf_traffic::Duration(0); + while (tail != end) + { + const auto* tail_event = tail->get(); + if (const auto* door_close = dynamic_cast(tail_event)) + { + if (door_open->door_name() != door_close->door_name()) + { + // A different door is being closed, so we should not lump this all + // together + return std::nullopt; + } + + // We have found the event where the robot is finished using the door. + // Let's lump these events together. + + auto group_state = rmf_task::events::SimpleEventState::make( + id->assign(), "Pass through [door:" + door_open->door_name() + "]", + "", rmf_task::Event::Status::Standby, {}, context->clock()); + + std::vector door_group; + ++tail; + for (auto it = head; it != tail; ++it) + { + door_group.push_back( + [legacy = *it, context, id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, update); + }); + } + + return EventGroupInfo{ + [ + door_group = std::move(door_group), + group_state = std::move(group_state) + ](UpdateFn update) + { + return rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + door_group, group_state, update); + }, + tail + }; + } + else if (const auto* move = dynamic_cast(tail_event)) + { + moving_duration += move->estimate_phase_duration(); + if (std::chrono::minutes(1) < moving_duration) + { + // There is a lot of moving happening here, so we should not lump + // this all together + return std::nullopt; + } + } + else + { + // If any other type of event is happening, we should not lump this + // all together + return std::nullopt; + } + + ++tail; + } + + return std::nullopt; +} + +//============================================================================== +std::optional search_for_lift_group( + LegacyPhases::const_iterator head, + LegacyPhases::const_iterator end, + const agv::RobotContextPtr& context, + const rmf_task::Event::AssignIDPtr& id, + const rmf_task::events::SimpleEventStatePtr& state) +{ + const auto lift_begin = dynamic_cast(head->get()); + + if (!lift_begin) + return std::nullopt; + + const auto& lift_name = lift_begin->lift_name(); + auto tail = head; + ++tail; + while (tail != end) + { + const auto* tail_event = tail->get(); + if (const auto* lift_request = dynamic_cast(tail_event)) + { + if (lift_request->lift_name() != lift_name) + { + // A different lift is being interacted with before the current lift + // interaction has finished. This is weird, so let's report it. + state->update_log().warn( + "Plan involves using [lift:" + lift_request->lift_name() + + "] while the robot is already in a session with [lift:" + + lift_name + "]. This may indicate a broken navigation graph. " + "Please report this to the system integrator."); + return std::nullopt; + } + } + else if (const auto* lift_end = dynamic_cast(tail_event)) + { + if (lift_end->lift_name() != lift_name) + { + // A different lift session is being ended before this one. This is + // weird, so let's report it. + state->update_log().warn( + "Plan involves ending a session with [lift:" + lift_end->lift_name() + + "] while [lift:" + lift_name + "] is in use. This may indicate " + "a broken navigation graph. Please report this to the system " + "integrator."); + return std::nullopt; + } + + auto category = "Take [lift:" + lift_name + + "] to [floor:" + lift_end->destination() + "]"; + + auto group_state = rmf_task::events::SimpleEventState::make( + id->assign(), std::move(category), + "", rmf_task::Event::Status::Standby, {}, context->clock()); + + std::vector lift_group; + ++tail; + for (auto it = head; it != tail; ++it) + { + lift_group.push_back( + [legacy = *it, context, id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, update); + }); + } + + return EventGroupInfo{ + [ + lift_group = std::move(lift_group), + group_state = std::move(group_state) + ](UpdateFn update) + { + return rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + lift_group, group_state, update); + }, + tail + }; + } + + ++tail; + } + + if (tail == end) + { + state->update_log().warn( + "Plan neglects to end a session with [lift:" + lift_name + "]. This may " + "indicate a broken navigation graph. Please report this to the system " + "integrator."); + } + + return std::nullopt; +} + +} // anonymous namespace + +//============================================================================== +std::optional ExecutePlan::make( + agv::RobotContextPtr context, + rmf_traffic::agv::Plan plan, + const rmf_task::Event::AssignIDPtr& id, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished, + std::optional tail_period) +{ + std::optional finish_time_estimate; + for (const auto& r : plan.get_itinerary()) + { + const auto check = r.trajectory().back().time(); + if (!finish_time_estimate.has_value() || *finish_time_estimate < check) + finish_time_estimate = check; + } + + if (!finish_time_estimate.has_value()) + { + // If this is empty then the entire plan is empty... that's not supposed to + // happen... + return std::nullopt; + } + + std::vector waypoints = + plan.get_waypoints(); + + std::vector move_through; + + LegacyPhases legacy_phases; + while (!waypoints.empty()) + { + auto it = waypoints.begin(); + bool event_occurred = false; + for (; it != waypoints.end(); ++it) + { + move_through.push_back(*it); + + if (it->event()) + { + if (move_through.size() > 1) + { + legacy_phases.push_back( + std::make_shared( + context, move_through, tail_period)); + } + + move_through.clear(); + bool continuous = true; + EventPhaseFactory factory( + context, legacy_phases, it->time(), continuous); + it->event()->execute(factory); + while (factory.moving_lift()) + { + const auto last_it = it; + ++it; + if (!it->event()) + { + const double dist = + (it->position().block<2, 1>(0, 0) + - last_it->position().block<2, 1>(0, 0)).norm(); + + if (dist < 0.5) + { + // We'll assume that this is just a misalignment in the maps + continue; + } + + state->update_log().warn( + "Plan involves a translation of [" + std::to_string(dist) + + "m] while inside a lift. This may indicate an error in the " + "navigation graph. Please report this to the system integrator."); + } + + it->event()->execute(factory); + } + + if (continuous) + { + // Have the next sequence of waypoints begin with the event waypoint + // of this sequence. + move_through.push_back(*it); + } + + waypoints.erase(waypoints.begin(), it+1); + event_occurred = true; + break; + } + } + + if (move_through.size() > 1) + { + // If we have more than one waypoint to move through, then create a + // moving phase. + legacy_phases.push_back( + std::make_shared( + context, move_through, tail_period)); + } + + if (!event_occurred) + { + // If no event occurred on this loop, then we have reached the end of the + // waypoint sequence, and we should simply clear it out. + waypoints.clear(); + } + } + + // Convert the legacy phases into task events. + + // We take the extra step of lumping related events into groups when we can + // manage to identify such groups, e.g. passing through a door or taking a + // lift. + std::vector standbys; + auto head = legacy_phases.cbegin(); + const auto end = legacy_phases.cend(); + while (head != end) + { + if (const auto door = search_for_door_group(head, end, context, id)) + { + standbys.push_back(door->group); + head = door->tail; + } + else if (const auto lift = + search_for_lift_group(head, end, context, id, state)) + { + standbys.push_back(lift->group); + head = lift->tail; + } + else + { + standbys.push_back( + [legacy = *head, context, id](UpdateFn update) + { + return LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, update); + }); + + ++head; + } + } + + auto sequence = rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + standbys, state, std::move(update))->begin([](){}, std::move(finished)); + + context->itinerary().set(plan.get_itinerary()); + + return ExecutePlan{ + std::move(plan), + finish_time_estimate.value(), + std::move(sequence) + }; +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp new file mode 100644 index 000000000..429822b1a --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.hpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP + +#include "../agv/RobotContext.hpp" + +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +struct ExecutePlan +{ + static std::optional make( + agv::RobotContextPtr context, + rmf_traffic::agv::Plan plan, + const rmf_task_sequence::Event::AssignIDPtr& id, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished, + std::optional tail_period); + + rmf_traffic::agv::Plan plan; + rmf_traffic::Time finish_time_estimate; + rmf_task_sequence::Event::ActivePtr sequence; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__EXECUTEPLAN_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp new file mode 100644 index 000000000..354eee4db --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -0,0 +1,432 @@ +/* + * Copyright (C) 2021 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 "GoToPlace.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +void GoToPlace::add(rmf_task_sequence::Event::Initializer& initializer) +{ + initializer.add( + []( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + std::function update) -> StandbyPtr + { + return Standby::make( + id, get_state, parameters, description, std::move(update)); + }, + []( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> ActivePtr + { + return Standby::make( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); +} + +//============================================================================== +auto GoToPlace::Standby::make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const rmf_task_sequence::events::GoToPlace::Description& description, + std::function update, + std::optional tail_period) +-> std::shared_ptr +{ + const auto state = get_state(); + const auto context = state.get()->value; + const auto header = description.generate_header(state, *parameters); + + auto standby = std::make_shared(Standby{description.destination()}); + standby->_assign_id = id; + standby->_context = context; + standby->_time_estimate = header.original_duration_estimate(); + standby->_tail_period = tail_period; + standby->_update = std::move(update); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + header.category(), + header.detail(), + rmf_task::Event::Status::Standby, + {}, + context->clock()); + + return standby; +} + +//============================================================================== +GoToPlace::Standby::Standby(rmf_traffic::agv::Plan::Goal goal) +: _goal(std::move(goal)) +{ + // Do nothin +} + +//============================================================================== +auto GoToPlace::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration GoToPlace::Standby::duration_estimate() const +{ + return _time_estimate; +} + +//============================================================================== +auto GoToPlace::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + if (!_active) + { + _active = Active::make( + _assign_id, + _context, + _goal, + _tail_period, + _state, + _update, + std::move(finished)); + } + + return _active; +} + +//============================================================================== +auto GoToPlace::Active::make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + rmf_traffic::agv::Plan::Goal goal, + std::optional tail_period, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) -> std::shared_ptr +{ + auto active = std::make_shared(Active(std::move(goal))); + active->_assign_id = id; + active->_context = std::move(context); + active->_tail_period = tail_period; + active->_update = std::move(update); + active->_finished = std::move(finished); + active->_state = std::move(state); + active->_negotiator = + Negotiator::make( + active->_context, + [w = active->weak_from_this()]( + const auto& t, const auto& r) -> Negotiator::NegotiatePtr + { + if (const auto self = w.lock()) + return self->_respond(t, r); + + r->forfeit({}); + return nullptr; + }); + + active->_find_plan(); + return active; +} + +//============================================================================== +auto GoToPlace::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const +{ + if (_execution.has_value()) + { + const auto finish = _execution->finish_time_estimate; + const auto now = _context->now(); + return finish - now + _context->itinerary().delay(); + } + + const auto& estimate = + _context->planner()->setup(_context->location(), _goal); + + if (estimate.ideal_cost().has_value()) + return rmf_traffic::time::from_seconds(*estimate.ideal_cost()); + + // It would be very suspicious if this happens... probably indicates that the + // task is impossible. + return rmf_traffic::Duration(0); +} + +//============================================================================== +auto GoToPlace::Active::backup() const -> Backup +{ + // GoToPlace doesn't need to be backed up + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto GoToPlace::Active::interrupt(std::function task_is_interrupted) +-> Resume +{ + _negotiator->clear_license(); + _is_interrupted = true; + _execution = std::nullopt; + + _state->update_status(Status::Standby); + _state->update_log().info("Going into standby for an interruption"); + _state->update_dependencies({}); + + _context->worker().schedule( + [task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + + return Resume::make( + [w = weak_from_this()]() + { + if (const auto self = w.lock()) + { + self->_negotiator->claim_license(); + self->_is_interrupted = false; + self->_find_plan(); + } + }); +} + +//============================================================================== +void GoToPlace::Active::cancel() +{ + _execution = std::nullopt; + _state->update_status(Status::Canceled); + _state->update_log().info("Received signal to cancel"); + _finished(); +} + +//============================================================================== +void GoToPlace::Active::kill() +{ + _execution = std::nullopt; + _state->update_status(Status::Killed); + _state->update_log().info("Received signal to kill"); + _finished(); +} + +//============================================================================== +std::string wp_name( + const agv::RobotContext& context, + const rmf_traffic::agv::Plan::Goal& goal) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& wp = g.get_waypoint(goal.waypoint()); + if (wp.name()) + return *wp.name(); + + return "#" + std::to_string(goal.waypoint()); +} + +//============================================================================== +std::string wp_name(const agv::RobotContext& context) +{ + const auto& g = context.planner()->get_configuration().graph(); + const auto& locations = context.location(); + for (const auto& l : locations) + { + const auto& wp = g.get_waypoint(l.waypoint()); + if (wp.name()) + return *wp.name(); + } + + if (locations.empty()) + return ""; + + return "#" + std::to_string(locations.front().waypoint()); +} + +//============================================================================== +void GoToPlace::Active::_find_plan() +{ + if (_is_interrupted) + return; + + _state->update_status(Status::Underway); + const auto start_name = wp_name(*_context); + const auto goal_name = wp_name(*_context, _goal); + _state->update_log().info( + "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); + + _find_path_service = std::make_shared( + _context->planner(), _context->location(), _goal, + _context->schedule()->snapshot(), _context->itinerary().id(), + _context->profile()); + + _plan_subscription = rmf_rxcpp::make_job( + _find_path_service) + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [w = weak_from_this(), start_name, goal_name]( + const services::FindPath::Result& result) + { + const auto self = w.lock(); + if (!self) + return; + + if (!result) + { + // The planner could not find a way to reach the goal + self->_state->update_status(Status::Error); + self->_state->update_log().error( + "Failed to find a plan to move from [" + + start_name + "] to [" + goal_name + "]. Will retry soon."); + + self->_execution = std::nullopt; + self->_schedule_retry(); + + self->_context->worker() + .schedule([update = self->_update](const auto&){ update(); }); + + return; + } + + self->_state->update_status(Status::Underway); + self->_state->update_log().info( + "Found a plan to move from [" + + start_name + "] to [" + goal_name + "]"); + + self->_execute_plan(*std::move(result)); + self->_find_path_service = nullptr; + self->_retry_timer = nullptr; + }); + + _find_path_timeout = _context->node()->try_create_wall_timer( + std::chrono::seconds(10), + [ + weak_service = _find_path_service->weak_from_this(), + weak_self = weak_from_this() + ]() + { + if (const auto service = weak_service.lock()) + service->interrupt(); + + if (const auto self = weak_self.lock()) + self->_find_path_timeout = nullptr; + }); + + _update(); +} + +//============================================================================== +GoToPlace::Active::Active(rmf_traffic::agv::Plan::Goal goal) +: _goal(std::move(goal)) +{ + // Do nothing +} + +//============================================================================== +void GoToPlace::Active::_schedule_retry() +{ + if (_retry_timer) + return; + + // TODO(MXG): Make the retry timing configurable + _retry_timer = _context->node()->try_create_wall_timer( + std::chrono::seconds(5), + [w = weak_from_this()]() + { + const auto self = w.lock(); + if (!self) + return; + + self->_retry_timer = nullptr; + if (self->_execution.has_value()) + return; + + self->_find_plan(); + }); +} + +//============================================================================== +void GoToPlace::Active::_execute_plan(rmf_traffic::agv::Plan plan) +{ + if (_is_interrupted) + return; + + if (plan.get_itinerary().empty()) + { + _finished(); + return; + } + + _execution = ExecutePlan::make( + _context, std::move(plan), _assign_id, _state, + _update, _finished, _tail_period); + + if (!_execution.has_value()) + { + _state->update_status(Status::Error); + _state->update_log().error( + "Invalid (empty) plan generated. Will retry soon. " + "Please report this incident to the Open-RMF developers."); + _schedule_retry(); + } +} + +//============================================================================== +Negotiator::NegotiatePtr GoToPlace::Active::_respond( + const Negotiator::TableViewerPtr& table_view, + const Negotiator::ResponderPtr& responder) +{ + if (_context->is_stubborn()) + { + rmf_traffic::schedule::StubbornNegotiator(_context->itinerary()) + .respond(table_view, responder); + return nullptr; + } + + auto approval_cb = [w = weak_from_this()]( + const rmf_traffic::agv::Plan& plan) + -> std::optional + { + if (auto self = w.lock()) + { + self->_execute_plan(plan); + return self->_context->itinerary().version(); + } + + return std::nullopt; + }; + + const auto evaluator = Negotiator::make_evaluator(table_view); + return services::Negotiate::path( + _context->planner(), _context->location(), _goal, table_view, + responder, std::move(approval_cb), std::move(evaluator)); +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp new file mode 100644 index 000000000..56e1c9bae --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP + +#include "../agv/RobotContext.hpp" +#include "../Negotiator.hpp" + +#include "../services/FindPath.hpp" + +#include "ExecutePlan.hpp" + +#include +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class GoToPlace : public rmf_task_sequence::Event +{ +public: + + using Description = rmf_task_sequence::events::GoToPlace::Description; + + static void add(rmf_task_sequence::Event::Initializer& initializer); + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const rmf_task_sequence::events::GoToPlace::Description& description, + std::function update, + std::optional tail_period = std::nullopt); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + + Standby(rmf_traffic::agv::Plan::Goal goal); + + rmf_traffic::agv::Plan::Goal _goal; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_traffic::Duration _time_estimate; + std::optional _tail_period; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + + static std::shared_ptr make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + rmf_traffic::agv::Plan::Goal goal, + std::optional tail_period, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + + Active(rmf_traffic::agv::Plan::Goal goal); + + void _schedule_retry(); + + void _find_plan(); + + void _execute_plan(rmf_traffic::agv::Plan plan); + + Negotiator::NegotiatePtr _respond( + const Negotiator::TableViewerPtr& table_view, + const Negotiator::ResponderPtr& responder); + + rmf_traffic::agv::Plan::Goal _goal; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + std::optional _tail_period; + std::function _update; + std::function _finished; + rmf_task::events::SimpleEventStatePtr _state; + std::shared_ptr _negotiator; + std::optional _execution; + std::shared_ptr _find_path_service; + rmf_rxcpp::subscription_guard _plan_subscription; + rclcpp::TimerBase::SharedPtr _find_path_timeout; + rclcpp::TimerBase::SharedPtr _retry_timer; + + bool _is_interrupted = false; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__GOTOPLACE_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp new file mode 100644 index 000000000..82b4e8fb4 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (C) 2021 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 "LegacyPhaseShim.hpp" + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +auto LegacyPhaseShim::Standby::make( + std::shared_ptr legacy, + rxcpp::schedulers::worker worker, + std::function clock, + const AssignIDPtr& id, + std::function parent_update, + std::optional name) -> std::shared_ptr +{ + auto standby = std::make_shared(); + standby->_legacy = std::move(legacy); + standby->_worker = std::move(worker); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), "", "", + rmf_task::Event::Status::Standby, {}, std::move(clock)); + + if (name.has_value()) + { + standby->_state->update_name(*name); + standby->_state->update_detail(standby->_legacy->description()); + } + else + { + standby->_state->update_name(standby->_legacy->description()); + } + + standby->_parent_update = std::move(parent_update); + + return standby; +} + +//============================================================================== +auto LegacyPhaseShim::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LegacyPhaseShim::Standby::duration_estimate() const +{ + return _legacy->estimate_phase_duration(); +} + +//============================================================================== +auto LegacyPhaseShim::Standby::begin( + std::function /* none of the legacy phases have checkpoints */, + std::function finished) -> ActivePtr +{ + if (!_legacy) + { + throw std::runtime_error( + "[rmf_fleet_adapter::events::LegacyPhaseShim::begin] " + "Triggering begin twice!"); + } + + return Active::make( + std::move(_legacy), + std::move(_worker), + std::move(_state), + std::move(_parent_update), + std::move(finished)); +} + +//============================================================================== +auto LegacyPhaseShim::Active::make( + std::shared_ptr legacy, + rxcpp::schedulers::worker worker, + rmf_task::events::SimpleEventStatePtr state, + std::function parent_update, + std::function finished) -> std::shared_ptr +{ + using rmf_task_msgs::msg::TaskSummary; + using rmf_task::Event; + + auto active = std::make_shared(); + active->_state = std::move(state); + active->_parent_update = std::move(parent_update); + active->_finished = std::move(finished); + active->_legacy = legacy->begin(); + active->_subscription = active->_legacy->observe() + .observe_on(rxcpp::identity_same_worker(worker)) + .subscribe( + [w = active->weak_from_this()]( + const rmf_task_msgs::msg::TaskSummary& msg) + { + if (const auto self = w.lock()) + { + bool need_update = false; + if (msg.status != self->_last_status_message) + { + need_update = true; + self->_last_status_message = msg.status; + auto& log = self->_state->update_log(); + if (msg.state == TaskSummary::STATE_FAILED) + log.error(msg.status); + else + log.info(msg.status); + } + + if (!self->_last_state_value.has_value() + || *self->_last_state_value != msg.state) + { + need_update = true; + self->_last_state_value = msg.state; + if (msg.state == TaskSummary::STATE_QUEUED + || msg.state == TaskSummary::STATE_PENDING) + self->_state->update_status(Event::Status::Standby); + else if (msg.state == TaskSummary::STATE_ACTIVE) + self->_state->update_status(Event::Status::Underway); + else if (msg.state == TaskSummary::STATE_COMPLETED) + self->_state->update_status(Event::Status::Completed); + else if (msg.state == TaskSummary::STATE_FAILED) + self->_state->update_status(Event::Status::Failed); + else if (msg.state == TaskSummary::STATE_CANCELED) + self->_state->update_status(Event::Status::Canceled); + else + self->_state->update_status(Event::Status::Uninitialized); + } + + if (need_update) + self->_parent_update(); + } + }, + [w = active->weak_from_this()]() + { + if (const auto self = w.lock()) + { + if (self->_finished) + { + // We don't change the event status here because that should have been + // done in the task summary update, and from here we don't know what + // kind of finish the legacy phase may have had (e.g. completed, + // failed, or canceled). + const auto finished = self->_finished; + self->_finished = nullptr; + finished(); + } + } + }); + + return active; +} + +//============================================================================== +auto LegacyPhaseShim::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration LegacyPhaseShim::Active::remaining_time_estimate() const +{ + return _legacy->estimate_remaining_time(); +} + +//============================================================================== +auto LegacyPhaseShim::Active::backup() const -> Backup +{ + // Legacy phases don't have backups + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto LegacyPhaseShim::Active::interrupt(std::function) -> Resume +{ + // Legacy phases cannot be interrupted. We need to let the phase just run to + // completion. We will never be able to safely trigger the task_is_interrupted + // callback. + return Resume::make([](){ /* do nothing */}); +} + +//============================================================================== +void LegacyPhaseShim::Active::cancel() +{ + _legacy->cancel(); +} + +//============================================================================== +void LegacyPhaseShim::Active::kill() +{ + // If we get a kill command then we'll just give up on whatever we're supposed + // to do without concern for consequences. When an operator kills a task, it's + // up to the operator to sort out the consequences. + if (_finished) + { + const auto finished = _finished; + _finished = nullptr; + finished(); + } +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp new file mode 100644 index 000000000..0037c71a4 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/LegacyPhaseShim.hpp @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP + +#include "../LegacyTask.hpp" + +#include +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +/// This class is being used to gradually migrate the legacy task phase system +/// into the new task event system. It provides a shim for legacy "Phase" +/// implementations to be turned into Event objects. +/// +/// We should endeavor to deprecate the use of this class over time as we work +/// on improving the code quality of RMF. +class LegacyPhaseShim : public rmf_task_sequence::Event +{ +public: + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + std::shared_ptr legacy, + rxcpp::schedulers::worker worker, + std::function clock, + const AssignIDPtr& id, + std::function parent_update, + std::optional name = std::nullopt); + + ConstStatePtr state() const final; + + rmf_traffic::Duration duration_estimate() const final; + + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + std::shared_ptr _legacy; + rxcpp::schedulers::worker _worker; + rmf_task::events::SimpleEventStatePtr _state; + std::function _parent_update; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + + static std::shared_ptr make( + std::shared_ptr legacy, + rxcpp::schedulers::worker worker, + rmf_task::events::SimpleEventStatePtr state, + std::function parent_update, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + std::shared_ptr _legacy; + rmf_task::events::SimpleEventStatePtr _state; + std::string _last_status_message; + std::optional _last_state_value; + std::function _parent_update; + std::function _finished; + rmf_rxcpp::subscription_guard _subscription; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__LEGACYPHASESHIM_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp new file mode 100644 index 000000000..3081b60ae --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.cpp @@ -0,0 +1,269 @@ +/* + * Copyright (C) 2022 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 "PerformAction.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +void PerformAction::add(rmf_task_sequence::Event::Initializer& initializer) +{ + initializer.add( + []( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + std::function update) -> StandbyPtr + { + return Standby::make( + id, get_state, parameters, description, std::move(update)); + }, + []( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const Description& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> ActivePtr + { + return Standby::make( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); +} + +//============================================================================== +auto PerformAction::Standby::make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const rmf_task_sequence::events::PerformAction::Description& description, + std::function update) -> std::shared_ptr +{ + const auto state = get_state(); + const auto context = state.get()->value; + const auto header = description.generate_header(state, *parameters); + + auto standby = std::make_shared(Standby{description}); + standby->_assign_id = id; + standby->_context = context; + standby->_time_estimate = header.original_duration_estimate(); + standby->_update = std::move(update); + standby->_state = rmf_task::events::SimpleEventState::make( + id->assign(), + header.category(), + header.detail(), + rmf_task::Event::Status::Standby, + {}, + context->clock()); + + return standby; +} + +//============================================================================== +PerformAction::Standby::Standby( + rmf_task_sequence::events::PerformAction::Description description) +: _description(std::move(description)) +{ + // Do nothing +} + +//============================================================================== +auto PerformAction::Standby::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration PerformAction::Standby::duration_estimate() const +{ + return _time_estimate; +} + +//============================================================================== +auto PerformAction::Standby::begin( + std::function, + std::function finished) -> ActivePtr +{ + if (!_active) + { + _active = Active::make( + _assign_id, + _context, + _description.category(), + _description.description(), + _time_estimate, + _state, + _update, + std::move(finished)); + } + + return _active; +} + +//============================================================================== +auto PerformAction::Active::make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + const std::string& category, + nlohmann::json desc, + rmf_traffic::Duration time_estimate, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished) -> std::shared_ptr +{ + auto active = std::make_shared( + Active(std::move(category), std::move(desc), time_estimate)); + active->_assign_id = id; + active->_context = std::move(context); + active->_update = std::move(update); + active->_finished = std::move(finished); + active->_state = std::move(state); + active->_execute_action(); + active->_expected_finish_time = + active->_context->now() + time_estimate; + return active; +} + +//============================================================================== +PerformAction::Active::Active( + const std::string& category, + nlohmann::json desc, + rmf_traffic::Duration time_estimate) +: _action_category(std::move(category)), + _action_description(std::move(desc)), + _time_estimate(std::move(time_estimate)) +{ + // Do nothing +} + +//============================================================================== +auto PerformAction::Active::state() const -> ConstStatePtr +{ + return _state; +} + +//============================================================================== +rmf_traffic::Duration PerformAction::Active::remaining_time_estimate() const +{ + auto execution = _execution_data.lock(); + if (!execution) + return rmf_traffic::Duration(0); + + if (execution->remaining_time.has_value()) + return execution->remaining_time.value(); + + // If an estimate is not provided we compute one based on the expected finish + // time + const auto estimate = + std::max(rmf_traffic::Duration(0), + _expected_finish_time - _context->now()); + return estimate; +} + +//============================================================================== +auto PerformAction::Active::backup() const -> Backup +{ + // TODO: Consider adding a function to ActionExecution that allows the + // system integrator to periodically send in backup messages. For now, we + // do not generate backup messages. + return Backup::make(0, nlohmann::json()); +} + +//============================================================================== +auto PerformAction::Active::interrupt( + std::function task_is_interrupted) -> Resume +{ + _state->update_status(Status::Standby); + _state->update_log().info("Going into standby for an interruption"); + _state->update_dependencies({}); + + _context->worker().schedule( + [task_is_interrupted](const auto&) + { + task_is_interrupted(); + }); + + // TODO(YV): Currently we will ask the robot to perform the action again when + // resumed. Future work can be to receive a callback from RobotUpdateHandle + // which can be used to ask the robot to resume from its interrupted state. + // This callback can be accepted by RobotUpdateHandle::ActionExecution + return Resume::make( + [w = weak_from_this()]() + { + if (const auto self = w.lock()) + { + self->_execute_action(); + } + }); +} + +//============================================================================== +void PerformAction::Active::cancel() +{ + _state->update_status(Status::Canceled); + _state->update_log().info("Received signal to cancel"); + _finished(); + if (auto data = _execution_data.lock()) + data->okay = false; +} + +//============================================================================== +void PerformAction::Active::kill() +{ + _state->update_status(Status::Killed); + _state->update_log().info("Received signal to kill"); + _finished(); + if (auto data = _execution_data.lock()) + data->okay = false; +} + +//============================================================================== +void PerformAction::Active::_execute_action() +{ + auto action_executor = _context->action_executor(); + if (action_executor == nullptr) + { + // The action_executor has not been set by the user + _state->update_status(Status::Error); + const std::string msg = "ActionExecutor not set via RobotUpdateHandle. " + "Unable to perform the requested action."; + _state->update_log().error(msg); + _finished(); + return; + } + + auto data = std::make_shared(_finished, std::nullopt); + _execution_data = data; + + auto action_execution = + agv::RobotUpdateHandle::ActionExecution::Implementation::make(data); + + action_executor( + _action_category, + _action_description, + std::move(action_execution)); +} + +} // namespace events +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp new file mode 100644 index 000000000..b8f275c2e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/PerformAction.hpp @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP +#define SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP + +#include "../agv/internal_RobotUpdateHandle.hpp" +#include "../agv/RobotContext.hpp" + +#include +#include +#include + +#include + +namespace rmf_fleet_adapter { +namespace events { + +//============================================================================== +class PerformAction : public rmf_task_sequence::Event +{ +public: + + using Description = rmf_task_sequence::events::PerformAction::Description; + + static void add(rmf_task_sequence::Event::Initializer& initializer); + + class Standby : public rmf_task_sequence::Event::Standby + { + public: + + static std::shared_ptr make( + const AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const rmf_task_sequence::events::PerformAction::Description& description, + std::function update); + + /// Documentation inherited + ConstStatePtr state() const final; + + /// Documentation inherited + rmf_traffic::Duration duration_estimate() const final; + + /// Documentation inherited + ActivePtr begin( + std::function checkpoint, + std::function finished) final; + + private: + + Standby(rmf_task_sequence::events::PerformAction::Description description); + + rmf_task_sequence::events::PerformAction::Description _description; + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + rmf_traffic::Duration _time_estimate; + std::function _update; + rmf_task::events::SimpleEventStatePtr _state; + ActivePtr _active = nullptr; + }; + + class Active + : public rmf_task_sequence::Event::Active, + public std::enable_shared_from_this + { + public: + + using ExecutionData = + agv::RobotUpdateHandle::ActionExecution::Implementation::Data; + + static std::shared_ptr make( + const AssignIDPtr& id, + agv::RobotContextPtr context, + const std::string& action_category, + nlohmann::json action_description, + rmf_traffic::Duration _time_estimate, + rmf_task::events::SimpleEventStatePtr state, + std::function update, + std::function finished); + + ConstStatePtr state() const final; + + rmf_traffic::Duration remaining_time_estimate() const final; + + Backup backup() const final; + + Resume interrupt(std::function task_is_interrupted) final; + + void cancel() final; + + void kill() final; + + private: + + Active( + const std::string& action_category, + nlohmann::json action_description, + rmf_traffic::Duration time_estimate); + + void _execute_action(); + + AssignIDPtr _assign_id; + agv::RobotContextPtr _context; + std::string _action_category; + nlohmann::json _action_description; + rmf_traffic::Duration _time_estimate; + std::function _update; + std::function _finished; + rmf_task::events::SimpleEventStatePtr _state; + rmf_traffic::Time _expected_finish_time; + std::shared_ptr _be_stubborn; + std::weak_ptr _execution_data; + }; +}; + +} // namespace events +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__EVENTS__PERFORMACTION_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp index cf22582cd..8cebd1aa5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp @@ -42,7 +42,7 @@ std::shared_ptr DispenseItem::ActivePhase::make( } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& DispenseItem::ActivePhase::observe() const { return _obs; @@ -87,7 +87,7 @@ DispenseItem::ActivePhase::ActivePhase( _items(std::move(items)) { std::ostringstream oss; - oss << "Dispense items ("; + oss << "Receive items ("; for (size_t i = 0; i < _items.size(); i++) { oss << _items[i].type_guid; @@ -135,11 +135,11 @@ void DispenseItem::ActivePhase::_init_obs() { auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); return me->_get_status(std::get<0>(v), std::get<1>(v)); }) - .lift(grab_while_active()) + .lift(grab_while_active()) .finally([weak = weak_from_this()]() { auto me = weak.lock(); @@ -152,12 +152,12 @@ void DispenseItem::ActivePhase::_init_obs() } //============================================================================== -Task::StatusMsg DispenseItem::ActivePhase::_get_status( +LegacyTask::StatusMsg DispenseItem::ActivePhase::_get_status( const rmf_dispenser_msgs::msg::DispenserResult::SharedPtr& dispenser_result, const rmf_dispenser_msgs::msg::DispenserState::SharedPtr& dispenser_state) { - Task::StatusMsg status{}; - status.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg status{}; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; using rmf_dispenser_msgs::msg::DispenserResult; if (dispenser_result @@ -171,10 +171,10 @@ Task::StatusMsg DispenseItem::ActivePhase::_get_status( _request_acknowledged = true; break; case DispenserResult::SUCCESS: - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; break; case DispenserResult::FAILED: - status.state = Task::StatusMsg::STATE_FAILED; + status.state = LegacyTask::StatusMsg::STATE_FAILED; break; } } @@ -200,7 +200,7 @@ Task::StatusMsg DispenseItem::ActivePhase::_get_status( { // The request has been received, so if it's no longer in the queue, // then we'll assume it's finished. - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; } } @@ -245,7 +245,7 @@ DispenseItem::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr DispenseItem::PendingPhase::begin() +std::shared_ptr DispenseItem::PendingPhase::begin() { return DispenseItem::ActivePhase::make( _context, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp index 946d6d257..7b2d9757d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__PHASES__DISPENSEITEM_HPP #include "RxOperators.hpp" -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include "rmf_fleet_adapter/StandardNames.hpp" @@ -33,7 +33,7 @@ namespace phases { struct DispenseItem { - class ActivePhase : public Task::ActivePhase, + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -45,7 +45,7 @@ struct DispenseItem std::string transporter_type, std::vector items); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -63,7 +63,7 @@ struct DispenseItem std::string _transporter_type; std::vector _items; std::string _description; - rxcpp::observable _obs; + rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; bool _request_acknowledged = false; builtin_interfaces::msg::Time _last_msg; @@ -77,14 +77,14 @@ struct DispenseItem void _init_obs(); - Task::StatusMsg _get_status( + LegacyTask::StatusMsg _get_status( const rmf_dispenser_msgs::msg::DispenserResult::SharedPtr& dispenser_result, const rmf_dispenser_msgs::msg::DispenserState::SharedPtr& dispenser_state); void _do_publish(); }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -95,7 +95,7 @@ struct DispenseItem std::string transporter_type, std::vector items); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp index e93fcccbb..6f97efb97 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.cpp @@ -25,20 +25,21 @@ DockRobot::ActivePhase::ActivePhase( agv::RobotContextPtr context, std::string dock_name) : _context{std::move(context)}, - _dock_name{std::move(dock_name)} + _dock_name{std::move(dock_name)}, + _be_stubborn(_context->be_stubborn()) { std::ostringstream oss; oss << "Docking robot to " << _dock_name; _description = oss.str(); _action = std::make_shared(this); - _obs = rmf_rxcpp::make_job(_action); + _obs = rmf_rxcpp::make_job(_action); _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_DOCKING); } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& DockRobot::ActivePhase::observe() const { return _obs; @@ -82,7 +83,7 @@ DockRobot::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr DockRobot::PendingPhase::begin() +std::shared_ptr DockRobot::PendingPhase::begin() { return std::make_shared(_context, _dock_name); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp index e584a5cf5..f76a9d90b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DockRobot.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__DOCKROBOT_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__DOCKROBOT_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" namespace rmf_fleet_adapter { @@ -29,7 +29,7 @@ struct DockRobot { class Action; - class ActivePhase : public Task::ActivePhase + class ActivePhase : public LegacyTask::ActivePhase { public: @@ -37,7 +37,7 @@ struct DockRobot agv::RobotContextPtr context, std::string dock_name); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -54,10 +54,11 @@ struct DockRobot std::string _dock_name; std::string _description; std::shared_ptr _action; - rxcpp::observable _obs; + rxcpp::observable _obs; + std::shared_ptr _be_stubborn; }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -65,7 +66,7 @@ struct DockRobot agv::RobotContextPtr context, std::string dock_name); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; @@ -96,8 +97,8 @@ struct DockRobot template void DockRobot::Action::operator()(const Subscriber& s) { - Task::StatusMsg status; - status.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg status; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; status.status = "Docking [" + _phase->_context->requester_id() + "] into dock [" + _phase->_dock_name + "]"; @@ -107,10 +108,10 @@ void DockRobot::Action::operator()(const Subscriber& s) _phase->_dock_name, [s, dock_name = _phase->_dock_name, context = _phase->_context]() { - Task::StatusMsg status; + LegacyTask::StatusMsg status; status.status = "Finished docking [" + context->requester_id() + "] into dock [" + dock_name + "]"; - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; s.on_next(status); s.on_completed(); }); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp index 2d9aec95d..7f12f6841 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.cpp @@ -39,7 +39,7 @@ std::shared_ptr DoorClose::ActivePhase::make( } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& DoorClose::ActivePhase::observe() const { return _obs; @@ -84,7 +84,7 @@ void DoorClose::ActivePhase::_init_obs() if (!me) return; - me->_status.state = Task::StatusMsg::STATE_ACTIVE; + me->_status.state = LegacyTask::StatusMsg::STATE_ACTIVE; me->_publish_close_door(); me->_timer = me->_context->node()->try_create_wall_timer( std::chrono::milliseconds(1000), @@ -101,12 +101,12 @@ void DoorClose::ActivePhase::_init_obs() { auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); me->_update_status(heartbeat); return me->_status; }) - .lift(grab_while_active()) + .lift(grab_while_active()) .finally([weak = weak_from_this()]() { auto me = weak.lock(); @@ -135,7 +135,7 @@ void DoorClose::ActivePhase::_update_status( if (!supervisor_has_session(*heartbeat, _request_id, _door_name)) { _status.status = "success"; - _status.state = Task::StatusMsg::STATE_COMPLETED; + _status.state = LegacyTask::StatusMsg::STATE_COMPLETED; } else { @@ -169,7 +169,7 @@ DoorClose::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr DoorClose::PendingPhase::begin() +std::shared_ptr DoorClose::PendingPhase::begin() { return DoorClose::ActivePhase::make( _context, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp index 43dafc073..df9faf763 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorClose.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__DOORCLOSE_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include @@ -38,7 +38,7 @@ struct DoorClose * 3. It is completed when the supervisor state does NOT contains the requester_id, regardless of the door state * 4. Cancellation requests are ignored */ - class ActivePhase : public Task::ActivePhase, + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -48,7 +48,7 @@ struct DoorClose std::string door_name, std::string request_id); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -63,10 +63,10 @@ struct DoorClose agv::RobotContextPtr _context; std::string _door_name; std::string _request_id; - rxcpp::observable _obs; + rxcpp::observable _obs; std::string _description; rclcpp::TimerBase::SharedPtr _timer; - Task::StatusMsg _status; + LegacyTask::StatusMsg _status; ActivePhase( agv::RobotContextPtr context, @@ -81,7 +81,7 @@ struct DoorClose const rmf_door_msgs::msg::SupervisorHeartbeat::SharedPtr& heartbeat); }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -90,12 +90,17 @@ struct DoorClose std::string door_name, std::string request_id); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; const std::string& description() const override; + const std::string& door_name() const + { + return _door_name; + } + private: agv::RobotContextPtr _context; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp index 61d13325f..c69a4da95 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.cpp @@ -58,7 +58,7 @@ DoorOpen::ActivePhase::ActivePhase( } //============================================================================== -const rxcpp::observable& DoorOpen::ActivePhase::observe() const +const rxcpp::observable& DoorOpen::ActivePhase::observe() const { return _obs; } @@ -107,7 +107,7 @@ void DoorOpen::ActivePhase::_init_obs() if (!me) return; - me->_status.state = Task::StatusMsg::STATE_ACTIVE; + me->_status.state = LegacyTask::StatusMsg::STATE_ACTIVE; me->_publish_open_door(); me->_timer = transport->try_create_wall_timer(std::chrono::milliseconds(1000), @@ -139,12 +139,12 @@ void DoorOpen::ActivePhase::_init_obs() { auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); me->_update_status(std::get<0>(v), std::get<1>(v)); return me->_status; }) - .lift(grab_while_active()) + .lift(grab_while_active()) .finally([weak = weak_from_this()]() { auto me = weak.lock(); @@ -156,7 +156,7 @@ void DoorOpen::ActivePhase::_init_obs() // When the phase is cancelled, queue a door close phase to make sure that there is no hanging // open doors .take_until(_cancelled.get_observable().filter([](auto b) { return b; })) - .concat(rxcpp::observable<>::create( + .concat(rxcpp::observable<>::create( [weak = weak_from_this()](const auto& s) { auto me = weak.lock(); @@ -201,7 +201,7 @@ void DoorOpen::ActivePhase::_update_status( && supervisor_has_session(*heartbeat, _request_id, _door_name)) { _status.status = "success"; - _status.state = Task::StatusMsg::STATE_COMPLETED; + _status.state = LegacyTask::StatusMsg::STATE_COMPLETED; } else { @@ -225,7 +225,7 @@ DoorOpen::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr DoorOpen::PendingPhase::begin() +std::shared_ptr DoorOpen::PendingPhase::begin() { return ActivePhase::make(_context, _door_name, _request_id, _expected_finish); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp index 1c5ba3488..9e7064fef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DoorOpen.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__PHASES__DOOROPEN_HPP #include "DoorClose.hpp" -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include @@ -39,7 +39,7 @@ struct DoorOpen * 2. It is completed when the supervisor state contains the requester_id and the door has an OPEN mode * 4. If cancelled, should start a door close phase */ - class ActivePhase : public Task::ActivePhase, + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -50,7 +50,7 @@ struct DoorOpen std::string request_id, rmf_traffic::Time expected_finish); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -68,10 +68,10 @@ struct DoorOpen rmf_traffic::Time _expected_finish; rxcpp::subjects::behavior _cancelled = rxcpp::subjects::behavior(false); - rxcpp::observable _obs; + rxcpp::observable _obs; std::string _description; rclcpp::TimerBase::SharedPtr _timer; - Task::StatusMsg _status; + LegacyTask::StatusMsg _status; std::shared_ptr _door_close_phase; ActivePhase( @@ -89,7 +89,7 @@ struct DoorOpen const rmf_door_msgs::msg::SupervisorHeartbeat::SharedPtr& heartbeat); }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -99,12 +99,17 @@ struct DoorOpen std::string request_id, rmf_traffic::Time expected_finish); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; const std::string& description() const override; + const std::string& door_name() const + { + return _door_name; + } + private: agv::RobotContextPtr _context; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp index ae281ed5d..a4224e938 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.cpp @@ -49,7 +49,7 @@ EndLiftSession::Active::Active( } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& EndLiftSession::Active::observe() const { return _obs; @@ -108,10 +108,10 @@ void EndLiftSession::Active::_init_obs() { const auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); - Task::StatusMsg msg; - msg.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; if (state->lift_name != me->_lift_name) return msg; @@ -119,12 +119,12 @@ void EndLiftSession::Active::_init_obs() if (state->session_id != me->_context->requester_id()) { msg.status = "success"; - msg.state = Task::StatusMsg::STATE_COMPLETED; + msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; } return msg; }) - .lift(grab_while_active()) + .lift(grab_while_active()) .finally([weak = weak_from_this()]() { const auto me = weak.lock(); @@ -160,7 +160,7 @@ EndLiftSession::Pending::Pending( } //============================================================================== -std::shared_ptr EndLiftSession::Pending::begin() +std::shared_ptr EndLiftSession::Pending::begin() { return Active::make( std::move(_context), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp index 727f2ed65..fdd2e339b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/EndLiftSession.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__ENDLIFTSESSION_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__ENDLIFTSESSION_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include "rmf_fleet_adapter/StandardNames.hpp" @@ -27,7 +27,7 @@ namespace phases { struct EndLiftSession { - class Active : public Task::ActivePhase, + class Active : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -42,7 +42,7 @@ struct EndLiftSession std::string lift_name, std::string destination); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -58,14 +58,14 @@ struct EndLiftSession std::string _lift_name; std::string _destination; std::string _description; - rxcpp::observable _obs; + rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; void _init_obs(); void _publish_session_end(); }; - class Pending : public Task::PendingPhase + class Pending : public LegacyTask::PendingPhase { public: @@ -74,12 +74,22 @@ struct EndLiftSession std::string lift_name, std::string destination); - std::shared_ptr begin() final; + std::shared_ptr begin() final; rmf_traffic::Duration estimate_phase_duration() const final; const std::string& description() const override; + const std::string& lift_name() const + { + return _lift_name; + } + + const std::string& destination() const + { + return _destination; + } + private: agv::RobotContextPtr _context; std::string _lift_name; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp index 02980fe2b..c2c0bddb5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -337,7 +337,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor EventPhaseFactory( agv::RobotContextPtr context, - Task::PendingPhases& phases, + LegacyTask::PendingPhases& phases, rmf_traffic::Time event_start_time, bool& continuous) : _context(std::move(context)), @@ -451,7 +451,7 @@ class EventPhaseFactory : public rmf_traffic::agv::Graph::Lane::Executor private: agv::RobotContextPtr _context; - Task::PendingPhases& _phases; + LegacyTask::PendingPhases& _phases; rmf_traffic::Time _event_start_time; bool& _continuous; bool _moving_lift = false; @@ -468,7 +468,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) _plan->get_waypoints(); std::vector move_through; - Task::PendingPhases sub_phases; + LegacyTask::PendingPhases sub_phases; while (!waypoints.empty()) { auto it = waypoints.begin(); @@ -553,8 +553,13 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) // TODO: Make distinctions between task and subtasks to avoid passing // dummy parameters for subtasks rmf_traffic::Time dummy_time; - rmf_task::agv::State dummy_state{{dummy_time, 0, 0.0}, 0, 1.0}; - _subtasks = Task::make( + rmf_task::State dummy_state; + dummy_state.waypoint(0) + .orientation(0.0) + .time(dummy_time) + .dedicated_charging_waypoint(0) + .battery_soc(1.0); + _subtasks = LegacyTask::make( _description, std::move(sub_phases), _context->worker(), @@ -597,7 +602,7 @@ void GoToPlace::Active::execute_plan(rmf_traffic::agv::Plan new_plan) } //============================================================================== -std::shared_ptr GoToPlace::Pending::begin() +std::shared_ptr GoToPlace::Pending::begin() { auto active = std::shared_ptr( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp index a969131f7..7930ccf87 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__GOTOPLACE_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__GOTOPLACE_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include "../services/FindPath.hpp" @@ -33,11 +33,11 @@ class GoToPlace { public: - using StatusMsg = Task::StatusMsg; + using StatusMsg = LegacyTask::StatusMsg; class Pending; class Active - : public Task::ActivePhase, + : public LegacyTask::ActivePhase, public rmf_traffic::schedule::Negotiator, public std::enable_shared_from_this { @@ -82,7 +82,7 @@ class GoToPlace double _latest_time_estimate; std::string _description; rmf_utils::optional _plan; - std::shared_ptr _subtasks; + std::shared_ptr _subtasks; bool _emergency_active = false; bool _performing_emergency_task = false; rxcpp::subjects::subject _status_publisher; @@ -110,12 +110,12 @@ class GoToPlace std::shared_ptr _negotiator_license; }; - class Pending : public Task::PendingPhase + class Pending : public LegacyTask::PendingPhase { public: // Documentation inherited - std::shared_ptr begin() final; + std::shared_ptr begin() final; // Documentation inherited rmf_traffic::Duration estimate_phase_duration() const final; @@ -138,7 +138,7 @@ class GoToPlace std::string _description; }; - /// Make a Task Phase for going to a place + /// Make a LegacyTask Phase for going to a place static std::unique_ptr make( agv::RobotContextPtr context, rmf_traffic::agv::Plan::Start start_estimate, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp index 158af77a5..3db97ec62 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp @@ -42,7 +42,7 @@ std::shared_ptr IngestItem::ActivePhase::make( } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& IngestItem::ActivePhase::observe() const { return _obs; @@ -134,11 +134,11 @@ void IngestItem::ActivePhase::_init_obs() { auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); return me->_get_status(std::get<0>(v), std::get<1>(v)); }) - .lift(grab_while_active()) + .lift(grab_while_active()) .finally([weak = weak_from_this()]() { auto me = weak.lock(); @@ -151,12 +151,12 @@ void IngestItem::ActivePhase::_init_obs() } //============================================================================== -Task::StatusMsg IngestItem::ActivePhase::_get_status( +LegacyTask::StatusMsg IngestItem::ActivePhase::_get_status( const rmf_ingestor_msgs::msg::IngestorResult::SharedPtr& ingestor_result, const rmf_ingestor_msgs::msg::IngestorState::SharedPtr& ingestor_state) { - Task::StatusMsg status{}; - status.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg status{}; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; using rmf_ingestor_msgs::msg::IngestorResult; if (ingestor_result @@ -170,10 +170,10 @@ Task::StatusMsg IngestItem::ActivePhase::_get_status( _request_acknowledged = true; break; case IngestorResult::SUCCESS: - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; break; case IngestorResult::FAILED: - status.state = Task::StatusMsg::STATE_FAILED; + status.state = LegacyTask::StatusMsg::STATE_FAILED; break; } } @@ -199,7 +199,7 @@ Task::StatusMsg IngestItem::ActivePhase::_get_status( { // The request has been received, so if it's no longer in the queue, // then we'll assume it's finished. - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; } } @@ -244,7 +244,7 @@ IngestItem::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr IngestItem::PendingPhase::begin() +std::shared_ptr IngestItem::PendingPhase::begin() { return IngestItem::ActivePhase::make( _context, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp index 0e4cd16d2..802a533bb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.hpp @@ -19,7 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__PHASES__INGESTITEM_HPP #include "RxOperators.hpp" -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include "rmf_fleet_adapter/StandardNames.hpp" @@ -33,7 +33,7 @@ namespace phases { struct IngestItem { - class ActivePhase : public Task::ActivePhase, + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -45,7 +45,7 @@ struct IngestItem std::string transporter_type, std::vector items); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -63,7 +63,7 @@ struct IngestItem std::string _transporter_type; std::vector _items; std::string _description; - rxcpp::observable _obs; + rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; bool _request_acknowledged = false; builtin_interfaces::msg::Time _last_msg; @@ -77,14 +77,14 @@ struct IngestItem void _init_obs(); - Task::StatusMsg _get_status( + LegacyTask::StatusMsg _get_status( const rmf_ingestor_msgs::msg::IngestorResult::SharedPtr& ingestor_result, const rmf_ingestor_msgs::msg::IngestorState::SharedPtr& ingestor_state); void _do_publish(); }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -95,7 +95,7 @@ struct IngestItem std::string transporter_type, std::vector items); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp index 4a7224261..a0f197bb2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.cpp @@ -38,17 +38,17 @@ MoveRobot::ActivePhase::ActivePhase( _action = std::make_shared( _context, waypoints, _tail_period); - auto job = rmf_rxcpp::make_job(_action); + auto job = rmf_rxcpp::make_job(_action); _obs = make_cancellable(job, _cancel_subject.get_observable()) - .lift(grab_while_active()) + .lift(grab_while_active()) .observe_on(rxcpp::identity_same_worker(_context->worker())); _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_MOVING); } //============================================================================== -const rxcpp::observable& MoveRobot::ActivePhase::observe() +const rxcpp::observable& MoveRobot::ActivePhase::observe() const { return _obs; @@ -96,7 +96,7 @@ MoveRobot::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr MoveRobot::PendingPhase::begin() +std::shared_ptr MoveRobot::PendingPhase::begin() { return std::make_shared( _context, _waypoints, _tail_period); 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 8fdf0f0b0..08cc8c7e7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/MoveRobot.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__MOVEROBOT_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__MOVEROBOT_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include @@ -30,7 +30,7 @@ struct MoveRobot { class Action; - class ActivePhase : public Task::ActivePhase + class ActivePhase : public LegacyTask::ActivePhase { public: @@ -39,7 +39,7 @@ struct MoveRobot std::vector waypoints, std::optional tail_period); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -54,12 +54,12 @@ struct MoveRobot agv::RobotContextPtr _context; std::string _description; std::shared_ptr _action; - rxcpp::observable _obs; + rxcpp::observable _obs; rxcpp::subjects::subject _cancel_subject; std::optional _tail_period; }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -68,7 +68,7 @@ struct MoveRobot std::vector waypoints, std::optional tail_period); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; @@ -142,8 +142,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) if (path_index != action->_next_path_index) { action->_next_path_index = path_index; - Task::StatusMsg msg; - msg.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_ACTIVE; if (path_index < action->_waypoints.size()) { @@ -217,8 +217,8 @@ void MoveRobot::Action::operator()(const Subscriber& s) }, [s]() { - Task::StatusMsg msg; - msg.state = Task::StatusMsg::STATE_COMPLETED; + LegacyTask::StatusMsg msg; + msg.state = LegacyTask::StatusMsg::STATE_COMPLETED; msg.status = "move robot success"; s.on_next(msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index 437cdd231..bc76dc870 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -43,7 +43,7 @@ std::shared_ptr RequestLift::ActivePhase::make( } //============================================================================== -const rxcpp::observable& +const rxcpp::observable& RequestLift::ActivePhase::observe() const { return _obs; @@ -138,20 +138,20 @@ void RequestLift::ActivePhase::_init_obs() { auto me = weak.lock(); if (!me) - return Task::StatusMsg(); + return LegacyTask::StatusMsg(); return me->_get_status(v); }) - .lift(grab_while([weak = - weak_from_this()](const Task::StatusMsg& status) + .lift(grab_while([weak = + weak_from_this()](const LegacyTask::StatusMsg& status) { auto me = weak.lock(); if (!me) return false; if ( - status.state == Task::StatusMsg::STATE_COMPLETED || - status.state == Task::StatusMsg::STATE_FAILED) + status.state == LegacyTask::StatusMsg::STATE_COMPLETED || + status.state == LegacyTask::StatusMsg::STATE_FAILED) { me->_timer.reset(); return false; @@ -159,7 +159,7 @@ void RequestLift::ActivePhase::_init_obs() return true; })) .take_until(_cancelled.get_observable().filter([](auto b) { return b; })) - .concat(rxcpp::observable<>::create( + .concat(rxcpp::observable<>::create( [weak = weak_from_this()](const auto& s) { auto me = weak.lock(); @@ -185,13 +185,13 @@ void RequestLift::ActivePhase::_init_obs() } //============================================================================== -Task::StatusMsg RequestLift::ActivePhase::_get_status( +LegacyTask::StatusMsg RequestLift::ActivePhase::_get_status( const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state) { using rmf_lift_msgs::msg::LiftState; using rmf_lift_msgs::msg::LiftRequest; - Task::StatusMsg status{}; - status.state = Task::StatusMsg::STATE_ACTIVE; + LegacyTask::StatusMsg status{}; + status.state = LegacyTask::StatusMsg::STATE_ACTIVE; if (!_rewaiting && lift_state->lift_name == _lift_name && lift_state->current_floor == _destination && @@ -273,7 +273,7 @@ Task::StatusMsg RequestLift::ActivePhase::_get_status( if (completed) { - status.state = Task::StatusMsg::STATE_COMPLETED; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; status.status = "success"; _timer.reset(); } @@ -334,7 +334,7 @@ RequestLift::PendingPhase::PendingPhase( } //============================================================================== -std::shared_ptr RequestLift::PendingPhase::begin() +std::shared_ptr RequestLift::PendingPhase::begin() { return ActivePhase::make( _context, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp index 5c2f35057..6197ee306 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__REQUESTLIFT_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__REQUESTLIFT_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include "rmf_fleet_adapter/StandardNames.hpp" #include "EndLiftSession.hpp" @@ -34,7 +34,7 @@ struct RequestLift Outside }; - class ActivePhase : public Task::ActivePhase, + class ActivePhase : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -46,7 +46,7 @@ struct RequestLift rmf_traffic::Time expected_finish, Located located); - const rxcpp::observable& observe() const override; + const rxcpp::observable& observe() const override; rmf_traffic::Duration estimate_remaining_time() const override; @@ -65,7 +65,7 @@ struct RequestLift rxcpp::subjects::behavior _cancelled = rxcpp::subjects::behavior(false); std::string _description; - rxcpp::observable _obs; + rxcpp::observable _obs; rclcpp::TimerBase::SharedPtr _timer; std::shared_ptr _lift_end_phase; Located _located; @@ -90,13 +90,13 @@ struct RequestLift void _init_obs(); - Task::StatusMsg _get_status( + LegacyTask::StatusMsg _get_status( const rmf_lift_msgs::msg::LiftState::SharedPtr& lift_state); void _do_publish(); }; - class PendingPhase : public Task::PendingPhase + class PendingPhase : public LegacyTask::PendingPhase { public: @@ -107,12 +107,17 @@ struct RequestLift rmf_traffic::Time expected_finish, Located located); - std::shared_ptr begin() override; + std::shared_ptr begin() override; rmf_traffic::Duration estimate_phase_duration() const override; const std::string& description() const override; + const std::string& lift_name() const + { + return _lift_name; + } + private: agv::RobotContextPtr _context; std::string _lift_name; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp index 23dc0256e..540bf7633 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.cpp @@ -154,7 +154,7 @@ void ResponsiveWait::Active::_begin_movement() } //============================================================================== -std::shared_ptr ResponsiveWait::Pending::begin() +std::shared_ptr ResponsiveWait::Pending::begin() { auto active = std::shared_ptr(new Active(_info)); active->_begin_movement(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp index 8d2b1ad4d..3ff0dd573 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/ResponsiveWait.hpp @@ -38,11 +38,11 @@ class ResponsiveWait public: - using StatusMsg = Task::StatusMsg; + using StatusMsg = LegacyTask::StatusMsg; class Pending; class Active - : public Task::ActivePhase, + : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -72,15 +72,15 @@ class ResponsiveWait rxcpp::observable _status_obs; rxcpp::subjects::subject _status_publisher; rmf_rxcpp::subscription_guard _movement_subscription; - std::shared_ptr _movement; + std::shared_ptr _movement; }; - class Pending : public Task::PendingPhase + class Pending : public LegacyTask::PendingPhase { public: // Documentation inherited - std::shared_ptr begin() final; + std::shared_ptr begin() final; // Documentation inherited rmf_traffic::Duration estimate_phase_duration() const final; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp index a284a22ac..438aec528 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RxOperators.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__RXOPERATORS_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__RXOPERATORS_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include @@ -53,10 +53,10 @@ auto grab_while(Predicate pred) inline auto grab_while_active() { - return grab_while([](const Task::StatusMsg& status) + return grab_while([](const LegacyTask::StatusMsg& status) { - return !(status.state == Task::StatusMsg::STATE_COMPLETED || - status.state == Task::StatusMsg::STATE_FAILED); + return !(status.state == LegacyTask::StatusMsg::STATE_COMPLETED || + status.state == LegacyTask::StatusMsg::STATE_FAILED); }); } @@ -80,10 +80,10 @@ auto on_subscribe(F f) const std::string status_msg_cancelled = "cancelled"; /** - * Makes an observable cancellable, the observable must be of type `Task::StatusMsg`. + * Makes an observable cancellable, the observable must be of type `LegacyTask::StatusMsg`. * TODO: Add a "CANCELLED" task state? * Use the `cancelled_msg` status text to find if the result of the observable is cancelled. - * @tparam Observable An observable of type `Task::StatusMsg` + * @tparam Observable An observable of type `LegacyTask::StatusMsg` * @tparam CancelObservable An observable of type `bool` * @param obs * @param cancel_obs @@ -96,14 +96,14 @@ auto make_cancellable(const Observable& obs, const CancelObservable& cancel_obs) .filter([](const auto& b) { return b; }) .map([](const auto&) { - Task::StatusMsg status; - status.state = Task::StatusMsg::STATE_COMPLETED; + LegacyTask::StatusMsg status; + status.state = LegacyTask::StatusMsg::STATE_COMPLETED; status.status = status_msg_cancelled; return status; }); return obs .merge(rxcpp::observe_on_event_loop(), cancelled_obs) - .template lift(grab_while_active()); + .template lift(grab_while_active()); } } // namespace phases diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index 2822b206a..b19b4206d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -92,7 +92,7 @@ WaitForCharge::Active::Active( } //============================================================================== -std::shared_ptr WaitForCharge::Pending::begin() +std::shared_ptr WaitForCharge::Pending::begin() { const auto& now = std::chrono::steady_clock::now(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index e6d8801e7..b603fcd92 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -18,7 +18,7 @@ #ifndef SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP #define SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include @@ -33,11 +33,11 @@ class WaitForCharge { public: - using StatusMsg = Task::StatusMsg; + using StatusMsg = LegacyTask::StatusMsg; class Pending; class Active - : public Task::ActivePhase, + : public LegacyTask::ActivePhase, public std::enable_shared_from_this { public: @@ -79,11 +79,11 @@ class WaitForCharge }; - class Pending : public Task::PendingPhase + class Pending : public LegacyTask::PendingPhase { public: // Documentation inherited - std::shared_ptr begin() final; + std::shared_ptr begin() final; // Documentation inherited rmf_traffic::Duration estimate_phase_duration() const final; @@ -116,4 +116,4 @@ class WaitForCharge } // namespace phases } // namespace rmf_fleet_adapter -#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP \ No newline at end of file +#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp index 7c7d5664f..8ed682884 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/services/FindPath.hpp @@ -26,7 +26,7 @@ namespace services { //============================================================================== /// Find a path that gets from the start to the goal. It might or might not /// comply with the given schedule, depending on what is feasible. -class FindPath +class FindPath : public std::enable_shared_from_this { public: diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index dfd37a01a..8f7f11393 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -16,21 +16,29 @@ */ #include "../phases/GoToPlace.hpp" - #include "../phases/WaitForCharge.hpp" +#include "../events/GoToPlace.hpp" +#include "../events/LegacyPhaseShim.hpp" + #include "ChargeBattery.hpp" +#include +#include +#include + +#include + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_charge_battery( +std::shared_ptr make_charge_battery( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state) + const rmf_task::State finish_state) { std::shared_ptr description = std::dynamic_pointer_cast< @@ -39,9 +47,10 @@ std::shared_ptr make_charge_battery( if (description == nullptr) return nullptr; - rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()}; + rmf_traffic::agv::Planner::Goal goal{ + finish_state.dedicated_charging_waypoint().value()}; - Task::PendingPhases phases; + LegacyTask::PendingPhases phases; phases.push_back( phases::GoToPlace::make(context, std::move(start), goal)); phases.push_back( @@ -50,8 +59,8 @@ std::shared_ptr make_charge_battery( context->task_planner()->configuration().parameters().battery_system(), context->task_planner()->configuration().constraints().recharge_soc())); - return Task::make( - request->id(), + return LegacyTask::make( + request->booking()->id(), std::move(phases), context->worker(), deployment_time, @@ -59,5 +68,188 @@ std::shared_ptr make_charge_battery( request); } +//============================================================================== +struct GoToChargerDescription + : public rmf_task_sequence::events::Placeholder::Description +{ + GoToChargerDescription() + : rmf_task_sequence::events::Placeholder::Description( + "Go to charger", "") + { + // Do nothing + } + + static rmf_task_sequence::Event::StandbyPtr standby( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const GoToChargerDescription&, + std::function update) + { + using GoToPlace = rmf_task_sequence::events::GoToPlace::Description; + const auto state = get_state(); + const auto context = state.get()->value; + + return events::GoToPlace::Standby::make( + id, get_state, parameters, + *GoToPlace::make(context->dedicated_charger_wp()), + std::move(update)); + } + + static void add(rmf_task_sequence::Event::Initializer& initializer) + { + initializer.add( + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const GoToChargerDescription& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr + { + return standby( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const GoToChargerDescription& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return standby( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); + } +}; + +//============================================================================== +struct WaitForChargeDescription + : public rmf_task_sequence::events::Placeholder::Description +{ + WaitForChargeDescription() + : rmf_task_sequence::events::Placeholder::Description( + "Wait for charging", "") + { + // Do nothing + } + + static rmf_task_sequence::Event::StandbyPtr standby( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const WaitForChargeDescription&, + std::function update) + { + const auto state = get_state(); + const auto context = state.get()->value; + + auto legacy = phases::WaitForCharge::make( + context, + parameters->battery_system(), + context->task_planner()->configuration().constraints().recharge_soc()); + + return events::LegacyPhaseShim::Standby::make( + std::move(legacy), context->worker(), context->clock(), id, + std::move(update)); + } + + static void add(rmf_task_sequence::Event::Initializer& initializer) + { + initializer.add( + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const WaitForChargeDescription& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr + { + return standby( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const WaitForChargeDescription& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return standby( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); + } +}; + +//============================================================================== +// TODO(MXG): Consider making the ChargeBatteryEvent description public so it +// can be incorporated into other task types +struct ChargeBatteryEventDescription + : public rmf_task_sequence::events::Placeholder::Description +{ + ChargeBatteryEventDescription() + : rmf_task_sequence::events::Placeholder::Description( + "Charge Battery", "") + { + // Do nothing + } +}; + +//============================================================================== +void add_charge_battery( + rmf_task::Activator& task_activator, + const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator, + rmf_task_sequence::Event::Initializer& event_initializer, + std::function clock) +{ + using Bundle = rmf_task_sequence::events::Bundle; + using Phase = rmf_task_sequence::phases::SimplePhase; + using ChargeBattery = rmf_task::requests::ChargeBattery; + + auto private_initializer = + std::make_shared(); + + WaitForChargeDescription::add(*private_initializer); + GoToChargerDescription::add(*private_initializer); + + auto charge_battery_event_unfolder = + [](const ChargeBatteryEventDescription&) + { + return Bundle::Description({ + std::make_shared(), + std::make_shared() + }, Bundle::Sequence, "Charge Battery"); + }; + + Bundle::unfold( + std::move(charge_battery_event_unfolder), + event_initializer, private_initializer); + + auto charge_battery_task_unfolder = + [](const rmf_task::requests::ChargeBattery::Description&) + { + rmf_task_sequence::Task::Builder builder; + builder + .add_phase( + Phase::Description::make( + std::make_shared(), + "Charge Battery", ""), {}); + + return *builder.build("Charge Battery", ""); + }; + + rmf_task_sequence::Task::unfold( + std::move(charge_battery_task_unfolder), + task_activator, + phase_activator, + std::move(clock)); +} + } // namespace task } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index e938190d5..55657bbcf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -18,25 +18,35 @@ #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP #define SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include #include -#include +#include #include +#include +#include + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_charge_battery( +std::shared_ptr make_charge_battery( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state); + const rmf_task::State finish_state); + +//============================================================================== +void add_charge_battery( + rmf_task::Activator& task_activator, + const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator, + rmf_task_sequence::Event::Initializer& event_initializer, + std::function clock); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp index 4a2980969..93a0367b1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -19,16 +19,27 @@ #include "Clean.hpp" +#include +#include +#include +#include + +#include "../events/Error.hpp" +#include "../events/GoToPlace.hpp" + +#include +#include + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_clean( +std::shared_ptr make_clean( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state) + const rmf_task::State finish_state) { std::shared_ptr description = std::dynamic_pointer_cast< @@ -45,13 +56,13 @@ std::shared_ptr make_clean( const auto start_waypoint = description->start_waypoint(); const rmf_traffic::agv::Planner::Goal clean_goal{start_waypoint}; - Task::PendingPhases phases; + LegacyTask::PendingPhases phases; // TODO(YV): If the robot is already at the start_waypoint, the Dock entry event will - // not be triggered and the task will be competed without any cleaning + // not be triggered and the task will be completed without any cleaning // performed. To avoid this, we request the robot to re-enter the lane. // This should be fixed when we define a new phase cleaning and not rely on // docking - if (context->current_task_end_state().location().waypoint() == start_waypoint) + if (context->current_task_end_state().waypoint().value() == start_waypoint) { const auto& graph = context->navigation_graph(); const auto& lane_from_index = graph.lanes_from(start_waypoint)[0]; @@ -66,8 +77,8 @@ std::shared_ptr make_clean( phases.push_back( phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); - return Task::make( - request->id(), + return LegacyTask::make( + request->booking()->id(), std::move(phases), context->worker(), deployment_time, @@ -75,5 +86,304 @@ std::shared_ptr make_clean( request); } +//============================================================================== +// TODO(MXG): This implementation that uses the Dock command is crude and +// should be replaced with an explicit cleaning activation command. +struct CleanEvent : public rmf_task_sequence::events::Placeholder::Description +{ + CleanEvent(const rmf_task::requests::Clean::Description& clean) + : rmf_task_sequence::events::Placeholder::Description("Clean", ""), + start_waypoint(clean.start_waypoint()), + end_waypoint(clean.end_waypoint()) + { + // Do nothing + } + + struct DockChecker : public rmf_traffic::agv::Graph::Lane::Executor + { + void execute(const Dock& dock) final { found_dock = dock.dock_name(); }; + void execute(const Wait&) final {}; + void execute(const DoorOpen&) final {}; + void execute(const LiftMove&) final {}; + void execute(const DoorClose&) final {}; + void execute(const LiftDoorOpen&) final {}; + void execute(const LiftSessionEnd&) final {}; + void execute(const LiftSessionBegin&) final {}; + + std::optional found_dock = std::nullopt; + }; + + static rmf_task_sequence::Event::StandbyPtr standby( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const CleanEvent& description, + std::function update) + { + const auto state = get_state(); + const auto context = state.get()->value; + + // Check if going to the start waypoint from the robot's current location + // will successfully pass through the dock event + const auto result = context->planner()->plan( + context->location(), + description.start_waypoint, {nullptr}); + + // TODO(MXG): Make this name more detailed + const std::string name = "Clean"; + + if (!result.success()) + { + const auto error_state = rmf_task::events::SimpleEventState::make( + id->assign(), name, "", rmf_task::Event::Status::Error, {}, + context->clock()); + + error_state->update_log().error( + "Could not find a path to the cleaning zone from the robot's current " + "location"); + + return events::Error::Standby::make(std::move(error_state)); + } + + bool will_pass_through_dock = false; + const auto& graph = context->planner()->get_configuration().graph(); + for (const auto& wp : result->get_waypoints()) + { + if (!wp.graph_index().has_value()) + continue; + + if (*wp.graph_index() == description.start_waypoint) + { + for (const auto& l : wp.approach_lanes()) + { + DockChecker check; + const auto& lane = graph.get_lane(l); + if (lane.entry().event()) + { + if (lane.entry().event()->execute(check).found_dock.has_value()) + { + will_pass_through_dock = true; + break; + } + } + + if (lane.exit().event()) + { + if (lane.exit().event()->execute(check).found_dock.has_value()) + { + will_pass_through_dock = true; + break; + } + } + } + } + } + + using StandbyPtr = rmf_task_sequence::Event::StandbyPtr; + using UpdateFn = std::function; + using MakeStandby = std::function; + using GoToPlaceDesc = rmf_task_sequence::events::GoToPlace::Description; + + auto go_to_place = [id, get_state, parameters](std::size_t wp) + { + return [id, get_state, parameters, wp](UpdateFn update) + { + return events::GoToPlace::Standby::make( + id, get_state, parameters, + *GoToPlaceDesc::make(wp), std::move(update)); + }; + }; + + std::vector events; + if (!will_pass_through_dock) + { + // If the robot does not pass through the dock on its way to the start + // location then it is likely already sitting on the start location. We + // should order the robot to the exit first and then back to the start. + // + // This strategy still has flaws because we can't rule out that by the + // time the robot is generating its plan to reach the exit it might + // accidentally trigger the docking action. This is one of the reasons we + // should fix the overall implementation. + events.push_back(go_to_place(description.end_waypoint)); + } + + events.push_back(go_to_place(description.start_waypoint)); + events.push_back(go_to_place(description.end_waypoint)); + + auto sequence_state = rmf_task::events::SimpleEventState::make( + id->assign(), name, "", rmf_task::Event::Status::Standby, {}, + context->clock()); + + return rmf_task_sequence::events::Bundle::standby( + rmf_task_sequence::events::Bundle::Type::Sequence, + events, std::move(sequence_state), std::move(update)); + } + + static void add(rmf_task_sequence::Event::Initializer& initializer) + { + initializer.add( + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const CleanEvent& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr + { + return standby( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const CleanEvent& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return standby( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); + } + + std::size_t start_waypoint; + std::size_t end_waypoint; +}; + +//============================================================================== +void add_clean( + const agv::FleetUpdateHandle::Implementation::ConstDockParamsPtr& dock_params, + const rmf_traffic::agv::VehicleTraits& traits, + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock) +{ + using Clean = rmf_task::requests::Clean; + using Phase = rmf_task_sequence::phases::SimplePhase; + + auto validate_clean_event = + deserialization.make_validator_shared( + schemas::event_description_Clean); + deserialization.add_schema(schemas::event_description_Clean); + + auto validate_clean_task = + deserialization.make_validator_shared( + schemas::task_description_Clean); + deserialization.add_schema(schemas::task_description_Clean); + + deserialization.consider_clean = + std::make_shared(); + + auto deserialize_clean = + [ + dock_params, + traits, + place_deser = deserialization.place, + consider = deserialization.consider_clean + ](const nlohmann::json& msg) -> agv::DeserializedTask + { + if (!consider || !(*consider)) + { + return { + nullptr, + {"Not accepting cleaning tasks"} + }; + } + + const auto zone = msg["zone"].get(); + const auto clean_it = dock_params->find(zone); + if (clean_it == dock_params->end()) + { + return { + nullptr, + {"No cleaning zone named [" + zone + "] for this fleet adapter"} + }; + } + + const auto& clean_info = clean_it->second; + auto start_place = place_deser(clean_info.start); + auto exit_place = place_deser(clean_info.finish); + if (!start_place.description.has_value() + || !exit_place.description.has_value()) + { + auto errors = std::move(start_place.errors); + errors.insert( + errors.end(), exit_place.errors.begin(), exit_place.errors.end()); + return {nullptr, std::move(errors)}; + } + + std::vector positions; + for (const auto& p : clean_info.path) + positions.push_back({p.x, p.y, p.yaw}); + + rmf_traffic::Trajectory clean_path = + rmf_traffic::agv::Interpolate::positions( + traits, + rmf_traffic::Time(rmf_traffic::Duration(0)), + positions); + + if (clean_path.size() < 2) + { + return { + nullptr, + {"Invalid cleaning path for zone named [" + zone + + "]: Too few waypoints [" + std::to_string(clean_path.size()) + + "]"} + }; + } + + agv::FleetUpdateHandle::Confirmation confirm; + (*consider)(msg, confirm); + if (!confirm.is_accepted()) + return {nullptr, confirm.errors()}; + + // TODO(MXG): Validate the type of cleaning (vacuum, mopping, etc) + return { + rmf_task::requests::Clean::Description::make( + start_place.description->waypoint(), + exit_place.description->waypoint(), + std::move(clean_path)), + confirm.errors() + }; + }; + deserialization.task->add("clean", validate_clean_task, deserialize_clean); + + auto deserialize_clean_event = + [deserialize_clean](const nlohmann::json& msg) -> agv::DeserializedEvent + { + auto clean_task = deserialize_clean(msg); + if (!clean_task.description) + return {nullptr, std::move(clean_task.errors)}; + + return { + std::make_shared( + static_cast(*clean_task.description)), + std::move(clean_task.errors) + }; + }; + deserialization.event->add( + "clean", validate_clean_event, deserialize_clean_event); + + CleanEvent::add(*activation.event); + + auto clean_unfolder = + [](const Clean::Description& clean) + { + rmf_task_sequence::Task::Builder builder; + builder.add_phase( + Phase::Description::make(std::make_shared(clean)), {}); + + // TODO(MXG): Make the name and detail more detailed + return *builder.build("Clean", ""); + }; + + rmf_task_sequence::Task::unfold( + std::move(clean_unfolder), *activation.task, + activation.phase, std::move(clock)); +} + } // namespace task } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp index 1782c2405..80a47e0bf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -18,25 +18,37 @@ #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP #define SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include #include -#include +#include #include +#include +#include + +#include "../agv/internal_FleetUpdateHandle.hpp" + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_clean( +std::shared_ptr make_clean( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start clean_start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state); + const rmf_task::State finish_state); + +void add_clean( + const agv::FleetUpdateHandle::Implementation::ConstDockParamsPtr& dock_params, + const rmf_traffic::agv::VehicleTraits& traits, + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp new file mode 100644 index 000000000..737d69557 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.cpp @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2022 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 "Compose.hpp" + +#include +#include +#include + +#include +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +void add_compose( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock) +{ + deserialization.add_schema( + rmf_fleet_adapter::schemas::task_description_Compose); + + deserialization.add_schema( + rmf_fleet_adapter::schemas::event_description_Sequence); + + auto validate_compose_task = + deserialization.make_validator_shared(schemas::task_description_Compose); + + auto deserialize_activity = + [ + deser_phase = deserialization.phase, + deser_event = deserialization.event + ](const nlohmann::json& msg) -> agv::DeserializedPhase + { + const auto& category = msg["category"].get(); + const auto& description_json = msg["description"]; + + const auto p_it = deser_phase->handlers.find(category); + if (p_it != deser_phase->handlers.end()) + { + // There is a specialized phase for this category, so we will + // deserialize and use that. + auto phase = p_it->second.deserializer(description_json); + if (!phase.description) + return {nullptr, std::move(phase.errors)}; + + return phase; + } + + // There is no specialized phase for this category, so we will check for + // an event that has the same category name. + const auto e_it = deser_event->handlers.find(category); + if (e_it == deser_event->handlers.end()) + { + // There is no specialized phase or event for this category, so we + // cannot support this task. + return {nullptr, {"No support for [" + category + "] activities"}}; + } + + auto event = e_it->second.deserializer(description_json); + if (!event.description) + return {nullptr, std::move(event.errors)}; + + using rmf_task_sequence::phases::SimplePhase; + return { + SimplePhase::Description::make(event.description), + std::move(event.errors) + }; + }; + + deserialization.consider_composed = + std::make_shared(); + + // Accept composed tasks by default + *deserialization.consider_composed = [](const auto&, auto& confirm) + { + confirm.accept(); + }; + + auto deserialize_compose_task = + [ + deser_activity = deserialize_activity, + consider = deserialization.consider_composed + ](const nlohmann::json& msg) + -> agv::DeserializedTask + { + if (!(*consider)) + return {nullptr, {"Not accepting composed requests"}}; + + rmf_task_sequence::Task::Builder builder; + std::string category = "Composed Task"; + const auto& category_json = msg["category"]; + if (category_json.is_string()) + category = category_json.get(); + + std::string detail = ""; + const auto& detail_json = msg["detail"]; + if (detail_json.is_string()) + detail = detail_json.get(); + + std::vector errors; + const auto& phases_json = msg["phases"]; + for (const auto& phase_json : phases_json) + { + auto phase_activity = deser_activity(phase_json["activity"]); + if (!phase_activity.description) + return {nullptr, std::move(phase_activity.errors)}; + + errors.insert( + errors.end(), + phase_activity.errors.begin(), phase_activity.errors.end()); + + std::vector cancel = {}; + const auto cancel_it = phase_json.find("on_cancel"); + if (cancel_it != phase_json.end()) + { + if (cancel_it.value().is_array()) + { + const auto& on_cancel_json = cancel_it.value(); + cancel.reserve(on_cancel_json.size()); + for (const auto& cancellation_activity_json : on_cancel_json) + { + auto cancellation_activity = + deser_activity(cancellation_activity_json); + + if (!cancellation_activity.description) + return {nullptr, std::move(cancellation_activity.errors)}; + + cancel.push_back(cancellation_activity.description); + errors.insert( + errors.end(), + cancellation_activity.errors.begin(), + cancellation_activity.errors.end()); + } + } + else + { + errors.push_back("The on_cancel property for one of the phases is " + "not an array. The intended activities will not be performed."); + } + } + + builder.add_phase(phase_activity.description, cancel); + } + + agv::FleetUpdateHandle::Confirmation confirm; + (*consider)(msg, confirm); + if (!confirm.is_accepted()) + return {nullptr, confirm.errors()}; + + return { + builder.build(category, detail), + std::move(errors) + }; + }; + + using rmf_task_sequence::events::Bundle; + using DeserializedDependencies = + agv::DeserializedDescription< + std::optional>; + + // Deserialize composed tasks + deserialization.task->add( + "compose", validate_compose_task, deserialize_compose_task); + + // Activate composed tasks + rmf_task_sequence::Task::add(*activation.task, activation.phase, clock); + + auto validate_event_sequence = + deserialization.make_validator_shared(schemas::event_description_Sequence); + + auto deserialize_event_dependencies = + [deser_event = deserialization.event](const nlohmann::json& msg) + -> DeserializedDependencies + { + std::vector errors; + Bundle::Description::Dependencies deps; + deps.reserve(msg.size()); + for (const auto& activity_json : msg) + { + const auto& category = activity_json["category"].get(); + const auto& description_json = activity_json["description"]; + const auto e_it = deser_event->handlers.find(category); + if (e_it == deser_event->handlers.end()) + { + errors.push_back("No support for [" + category + "] activities"); + return {std::nullopt, std::move(errors)}; + } + + auto event = e_it->second.deserializer(description_json); + errors.insert(errors.end(), event.errors.begin(), event.errors.end()); + if (!event.description) + return {std::nullopt, std::move(errors)}; + + deps.push_back(event.description); + } + + return {std::move(deps), std::move(errors)}; + }; + + auto deserialize_event_sequence = + [deserialize_event_dependencies](const nlohmann::json& msg) + -> agv::DeserializedEvent + { + DeserializedDependencies dependencies; + std::optional category; + std::optional detail; + if (msg.is_array()) + { + dependencies = deserialize_event_dependencies(msg); + } + else + { + dependencies = deserialize_event_dependencies(msg["activities"]); + + const auto& category_json = msg["category"]; + if (category_json.is_string()) + category = category_json.get(); + + const auto& detail_json = msg["detail"]; + if (detail_json.is_string()) + detail = detail_json.get(); + } + + if (!dependencies.description.has_value()) + return {nullptr, std::move(dependencies.errors)}; + + return { + std::make_shared( + *dependencies.description, + Bundle::Type::Sequence, + std::move(category), + std::move(detail)), + std::move(dependencies.errors) + }; + }; + + // Deserialize activity sequences + deserialization.event->add( + "sequence", validate_event_sequence, deserialize_event_sequence); + + // Activate activity sequences + rmf_task_sequence::events::Bundle::add(activation.event); +} + +} // namespace tasks +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp new file mode 100644 index 000000000..068c8f58c --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Compose.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP +#define SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP + +#include "../agv/internal_FleetUpdateHandle.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +void add_compose( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock); + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__TASKS__COMPOSE_HPP diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index 0bf55d5d2..4afc4915c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -19,20 +19,37 @@ #include "../phases/IngestItem.hpp" #include "../phases/GoToPlace.hpp" +#include "../events/LegacyPhaseShim.hpp" +#include "../events/Error.hpp" +#include "../events/GoToPlace.hpp" + #include "Delivery.hpp" #include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_delivery( +std::shared_ptr make_delivery( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state, + const rmf_task::State finish_state, const rmf_task_msgs::msg::Delivery delivery_profile) { @@ -46,7 +63,7 @@ std::shared_ptr make_delivery( const auto pickup_waypoint = description->pickup_waypoint(); const auto dropoff_waypoint = description->dropoff_waypoint(); - Task::PendingPhases phases; + LegacyTask::PendingPhases phases; phases.push_back( phases::GoToPlace::make( context, @@ -56,7 +73,7 @@ std::shared_ptr make_delivery( phases.push_back( std::make_unique( context, - request->id(), + request->booking()->id(), delivery_profile.pickup_dispenser, context->itinerary().description().owner(), delivery_profile.items)); @@ -102,13 +119,13 @@ std::shared_ptr make_delivery( phases.push_back( std::make_unique( context, - request->id(), + request->booking()->id(), delivery_profile.dropoff_ingestor, context->itinerary().description().owner(), ingestor_items)); - return Task::make( - request->id(), + return LegacyTask::make( + request->booking()->id(), std::move(phases), context->worker(), deployment_time, @@ -116,5 +133,370 @@ std::shared_ptr make_delivery( request); } +//============================================================================== +struct TransferItems : public rmf_task_sequence::events::Placeholder::Description +{ + enum class Dir + { + Load, + Unload + }; + + TransferItems(const rmf_task_sequence::events::PickUp::Description& pickup) + : rmf_task_sequence::events::Placeholder::Description("Load items", ""), + direction(Dir::Load), + target(pickup.from_dispenser()), + payload(pickup.payload()) + { + // Do nothing + } + + TransferItems(const rmf_task_sequence::events::DropOff::Description& dropoff) + : rmf_task_sequence::events::Placeholder::Description("Unload items", ""), + direction(Dir::Unload), + target(dropoff.into_ingestor()), + payload(dropoff.payload()) + { + // Do nothing + } + + template class Build, typename T> + std::vector collect_items() const + { + std::vector items; + for (const auto& c : payload.components()) + { + items.push_back( + Build() + .type_guid(c.sku()) + .quantity(c.quantity()) + .compartment_name(c.compartment())); + } + } + + Dir direction; + std::string target; + rmf_task::Payload payload; + + static rmf_task_sequence::Event::StandbyPtr standby( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr&, + const TransferItems& description, + std::function update) + { + const auto state = get_state(); + const auto context = state.get()->value; + const auto* task_id = context->current_task_id(); + + std::string name; + if (description.direction == Dir::Unload) + name = "Unload"; + else + name = "Load"; + + if (description.payload.components().size() == 1) + name += "item"; + else + name += "items"; + + if (!task_id) + { + const auto error_state = rmf_task::events::SimpleEventState::make( + id->assign(), name, "", rmf_task::Event::Status::Error, {}, + context->clock()); + + error_state->update_log().error( + "Task ID is null while trying to perform a delivery. This indicates a " + "serious software error. Please try to reset the task, and contact the " + "system integrator to inform them of this issue."); + + return events::Error::Standby::make(std::move(error_state)); + } + + std::shared_ptr legacy; + if (description.direction == Dir::Unload) + { + using IngestorItem = rmf_ingestor_msgs::msg::IngestorRequestItem; + std::vector items; + for (const auto& c : description.payload.components()) + { + items.push_back( + rmf_ingestor_msgs::build() + .type_guid(c.sku()) + .quantity(c.quantity()) + .compartment_name(c.compartment())); + } + + legacy = std::make_shared( + context, *task_id, description.target, + context->itinerary().description().owner(), + std::move(items)); + } + else + { + using DispenserItem = rmf_dispenser_msgs::msg::DispenserRequestItem; + std::vector items; + for (const auto& c : description.payload.components()) + { + items.push_back( + rmf_dispenser_msgs::build() + .type_guid(c.sku()) + .quantity(c.quantity()) + .compartment_name(c.compartment())); + } + + legacy = std::make_shared( + context, *task_id, description.target, + context->itinerary().description().owner(), + std::move(items)); + } + + return events::LegacyPhaseShim::Standby::make( + legacy, context->worker(), context->clock(), id, std::move(update), name); + } + + static void add(rmf_task_sequence::Event::Initializer& initializer) + { + initializer.add( + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const TransferItems& description, + std::function update) -> rmf_task_sequence::Event::StandbyPtr + { + return standby( + id, get_state, parameters, description, std::move(update)); + }, + []( + const rmf_task_sequence::Event::AssignIDPtr& id, + const std::function& get_state, + const rmf_task::ConstParametersPtr& parameters, + const TransferItems& description, + const nlohmann::json&, + std::function update, + std::function checkpoint, + std::function finished) -> rmf_task_sequence::Event::ActivePtr + { + return standby( + id, get_state, parameters, description, std::move(update)) + ->begin(std::move(checkpoint), std::move(finished)); + }); + } +}; + +//============================================================================== +using DeserializedItemTransfer = agv::DeserializedEvent; +template +std::function +make_deserializer( + const agv::PlaceDeserializer& place_deser, + const std::shared_ptr& consider) +{ + auto parse_payload_component = [](const nlohmann::json& msg) + -> rmf_task::Payload::Component + { + std::string compartment = ""; + + const auto compartment_json_it = msg.find("compartment"); + if (compartment_json_it != msg.end()) + compartment = compartment_json_it->get(); + + return rmf_task::Payload::Component( + msg["sku"].get(), + msg["quantity"].get(), + std::move(compartment)); + }; + + return + [ + place_deser, + consider, + parse_payload_component = std::move(parse_payload_component) + ](const nlohmann::json& msg) -> agv::DeserializedEvent + { + if (!consider || !(*consider)) + return {nullptr, {"Not accepting delivery requests"}}; + + auto place = place_deser(msg["place"]); + if (!place.description.has_value()) + { + return {nullptr, std::move(place.errors)}; + } + + std::vector payload_components; + const auto& payload_json = msg["payload"]; + if (payload_json.is_object()) + { + payload_components.push_back(parse_payload_component(payload_json)); + } + else if (payload_json.is_array()) + { + for (const auto& component_json : payload_json) + { + payload_components.push_back(parse_payload_component(component_json)); + } + } + else + { + return { + nullptr, + {"invalid data type provided for 'payload' property: expected an " + "object or an array but got " + std::string(payload_json.type_name()) + + " type instead"} + }; + } + + std::string handler; + const auto& handler_json_it = msg.find("handler"); + if (handler_json_it != msg.end()) + { + handler = handler_json_it->get(); + } + + agv::FleetUpdateHandle::Confirmation confirm; + (*consider)(msg, confirm); + if (!confirm.is_accepted()) + return {nullptr, confirm.errors()}; + + // TODO(MXG): Add a way for system integrators to specify a duration + // estimate for the payload transfer + return { + T::make( + std::move(*place.description), + std::move(handler), + rmf_task::Payload(std::move(payload_components)), + rmf_traffic::Duration(0)), + confirm.errors() + }; + }; +} + +//============================================================================== +void add_delivery( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock) +{ + using Bundle = rmf_task_sequence::events::Bundle; + using PickUp = rmf_task_sequence::events::PickUp; + using DropOff = rmf_task_sequence::events::DropOff; + using Phase = rmf_task_sequence::phases::SimplePhase; + using DeliveryDescription = rmf_task::requests::Delivery::Description; + + deserialization.add_schema(schemas::event_description_PayloadTransfer); + deserialization.add_schema(schemas::task_description_Delivery); + deserialization.add_schema(schemas::event_description_PickUp); + deserialization.add_schema(schemas::event_description_DropOff); + auto validate_payload_transfer = + deserialization.make_validator_shared( + schemas::event_description_PayloadTransfer); + + deserialization.consider_pickup = + std::make_shared(); + auto deserialize_pickup = + make_deserializer( + deserialization.place, deserialization.consider_pickup); + deserialization.event->add( + "pickup", validate_payload_transfer, deserialize_pickup); + + deserialization.consider_dropoff = + std::make_shared(); + auto deserialize_dropoff = + make_deserializer( + deserialization.place, deserialization.consider_dropoff); + deserialization.event->add( + "dropoff", validate_payload_transfer, deserialize_dropoff); + + auto validate_delivery = + deserialization.make_validator_shared(schemas::task_description_Delivery); + + auto deserialize_delivery = + [deserialize_pickup, deserialize_dropoff]( + const nlohmann::json& msg) -> agv::DeserializedTask + { + const auto pickup = deserialize_pickup(msg["pickup"]); + const auto dropoff = deserialize_dropoff(msg["dropoff"]); + std::vector errors; + errors.reserve(pickup.errors.size() + dropoff.errors.size()); + errors.insert( + errors.end(), pickup.errors.begin(), pickup.errors.end()); + errors.insert( + errors.end(), dropoff.errors.begin(), dropoff.errors.end()); + + if (!pickup.description || !dropoff.description) + { + return {nullptr, std::move(errors)}; + } + + rmf_task_sequence::Task::Builder builder; + builder.add_phase(Phase::Description::make(pickup.description), {}); + builder.add_phase(Phase::Description::make(dropoff.description), {}); + // TODO(MXG): Consider making the category and detail more detailed + return {builder.build("Delivery", ""), std::move(errors)}; + }; + + deserialization.task->add("delivery", validate_delivery, deserialize_delivery); + + auto private_initializer = + std::make_shared(); + + TransferItems::add(*private_initializer); + events::GoToPlace::add(*private_initializer); + + auto pickup_unfolder = + [](const PickUp::Description& pickup) + { + return Bundle::Description({ + events::GoToPlace::Description::make(pickup.pickup_location()), + std::make_shared(pickup) + }, Bundle::Type::Sequence, "Pick up"); + }; + + Bundle::unfold( + std::move(pickup_unfolder), *activation.event, private_initializer); + + auto dropoff_unfolder = + [](const DropOff::Description& dropoff) + { + return Bundle::Description({ + events::GoToPlace::Description::make(dropoff.drop_off_location()), + std::make_shared(dropoff) + }, Bundle::Type::Sequence, "Drop Off"); + }; + + Bundle::unfold( + std::move(dropoff_unfolder), *activation.event, private_initializer); + + auto delivery_unfolder = + [](const DeliveryDescription& delivery) + { + rmf_task_sequence::Task::Builder builder; + builder + .add_phase( + Phase::Description::make( + PickUp::Description::make( + delivery.pickup_waypoint(), + delivery.pickup_from_dispenser(), + delivery.payload(), + delivery.pickup_wait())), {}) + .add_phase( + Phase::Description::make( + DropOff::Description::make( + delivery.dropoff_waypoint(), + delivery.dropoff_to_ingestor(), + delivery.payload(), + delivery.dropoff_wait())), {}); + + // TODO(MXG): Consider making the category and detail more detailed + return *builder.build("Delivery", ""); + }; + + rmf_task_sequence::Task::unfold( + std::move(delivery_unfolder), *activation.task, + activation.phase, std::move(clock)); +} + } // namespace task } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp index f7f718b40..41a0ed124 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp @@ -18,25 +18,32 @@ #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__DELIVERY_HPP #define SRC__RMF_FLEET_ADAPTER__TASKS__DELIVERY_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include #include +#include "../agv/internal_FleetUpdateHandle.hpp" + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_delivery( +std::shared_ptr make_delivery( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start pickup_start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state, + const rmf_task::State finish_state, const rmf_task_msgs::msg::Delivery delivery_profile); +void add_delivery( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock); + } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index cdd0b4b48..06a5b916f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -19,16 +19,23 @@ #include "../phases/GoToPlace.hpp" +#include +#include +#include + +#include +#include + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_loop( +std::shared_ptr make_loop( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state) + const rmf_task::State finish_state) { std::shared_ptr description = std::dynamic_pointer_cast< @@ -80,7 +87,7 @@ std::shared_ptr make_loop( orientation}; } (); - Task::PendingPhases phases; + LegacyTask::PendingPhases phases; phases.push_back( phases::GoToPlace::make( context, std::move(start), start_waypoint)); @@ -100,8 +107,8 @@ std::shared_ptr make_loop( context, loop_start, finish_waypoint)); } - return Task::make( - request->id(), + return LegacyTask::make( + request->booking()->id(), std::move(phases), context->worker(), deployment_time, @@ -109,5 +116,128 @@ std::shared_ptr make_loop( request); } +//============================================================================== +void add_loop( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock) +{ + using Loop = rmf_task::requests::Loop; + using Phase = rmf_task_sequence::phases::SimplePhase; + using GoToPlace = rmf_task_sequence::events::GoToPlace; + + deserialization.add_schema(schemas::task_description_Patrol); + deserialization.add_schema(schemas::event_description_GoToPlace); + auto validate_go_to_place = + deserialization.make_validator_shared(schemas::event_description_GoToPlace); + + auto deserialize_go_to_place = + [place_deser = deserialization.place](const nlohmann::json& msg) + -> agv::DeserializedEvent + { + auto place = place_deser(msg); + if (!place.description.has_value()) + return {nullptr, std::move(place.errors)}; + + return { + GoToPlace::Description::make(std::move(*place.description)), + std::move(place.errors) + }; + }; + + deserialization.event->add( + "go_to_place", validate_go_to_place, deserialize_go_to_place); + + auto validate_patrol = + deserialization.make_validator_shared(schemas::task_description_Patrol); + + deserialization.consider_patrol = + std::make_shared(); + + // Accept patrol tasks by default + *deserialization.consider_patrol = [](const auto&, auto& confirm) + { + confirm.accept(); + }; + + auto deserialize_patrol = + [ + place_deser = deserialization.place, + consider = deserialization.consider_patrol + ](const nlohmann::json& msg) -> agv::DeserializedTask + { + if (!(*consider)) + return {nullptr, {"Not accepting patrol requests"}}; + + const auto& places_json = msg["places"]; + std::vector places; + std::vector errors; + bool any_failure = false; + for (const auto& place_json : places_json) + { + const auto place = place_deser(place_json); + if (!place.description.has_value()) + any_failure = true; + else + places.push_back(*place.description); + + errors.insert(errors.begin(), place.errors.begin(), place.errors.end()); + } + + if (any_failure) + return {nullptr, std::move(errors)}; + + agv::FleetUpdateHandle::Confirmation confirm; + (*consider)(msg, confirm); + errors.insert( + errors.end(), confirm.errors().begin(), confirm.errors().end()); + + if (!confirm.is_accepted()) + return {nullptr, errors}; + + std::size_t rounds = 1; + const auto& rounds_json_it = msg.find("rounds"); + if (rounds_json_it != msg.end()) + rounds = rounds_json_it->get(); + + rmf_task_sequence::Task::Builder builder; + for (std::size_t i=0; i < rounds; ++i) + { + for (const auto& place : places) + { + builder.add_phase( + Phase::Description::make(GoToPlace::Description::make(place)), {}); + } + } + + return {builder.build("Patrol", ""), std::move(errors)}; + }; + + deserialization.task->add("patrol", validate_patrol, deserialize_patrol); + + auto loop_unfolder = + [](const Loop::Description& loop) + { + rmf_task_sequence::Task::Builder builder; + for (std::size_t i=0; i < loop.num_loops(); ++i) + { + builder + .add_phase( + Phase::Description::make( + GoToPlace::Description::make(loop.start_waypoint())), {}) + .add_phase( + Phase::Description::make( + GoToPlace::Description::make(loop.finish_waypoint())), {}); + } + + // TODO(MXG): Consider making the category and detail more details + return *builder.build("Loop", ""); + }; + + rmf_task_sequence::Task::unfold( + std::move(loop_unfolder), *activation.task, + activation.phase, std::move(clock)); +} + } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp index 15dd7d306..388bbaf4b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp @@ -18,21 +18,31 @@ #ifndef SRC__RMF_FLEET_ADAPTER__TASKS__LOOP_HPP #define SRC__RMF_FLEET_ADAPTER__TASKS__LOOP_HPP -#include "../Task.hpp" +#include "../LegacyTask.hpp" #include "../agv/RobotContext.hpp" #include +#include + +#include "../agv/internal_FleetUpdateHandle.hpp" + namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_loop( +std::shared_ptr make_loop( const rmf_task::ConstRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, const rmf_traffic::Time deployment_time, - const rmf_task::agv::State finish_state); + const rmf_task::State finish_state); + +//============================================================================== +void add_loop( + agv::TaskDeserialization& deserialization, + agv::TaskActivation& activation, + std::function clock); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp b/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp index 38f7dbfb8..877a7ad90 100644 --- a/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp +++ b/rmf_fleet_adapter/test/phases/test_DispenseItem.cpp @@ -38,7 +38,7 @@ struct TestData std::list received_requests; std::condition_variable status_updates_cv; - std::list status_updates; + std::list status_updates; std::optional last_state_value() const { @@ -141,7 +141,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]") { for (const auto& status : test->status_updates) { - if (status.state == Task::StatusMsg::STATE_COMPLETED) + if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED) return true; } test->status_updates.clear(); @@ -204,7 +204,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]") bool completed = test->status_updates_cv.wait_for( lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -269,7 +269,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]") test->status_updates_cv.wait_for(lk, std::chrono::milliseconds( 10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_FAILED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_FAILED; }); CHECK(failed); } @@ -332,7 +332,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]") bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds( 10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -398,7 +398,7 @@ SCENARIO_METHOD(MockAdapterFixture, "dispense item phase", "[phases]") bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds( 10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(!completed); } diff --git a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp index 053c74007..0b6bf8edf 100644 --- a/rmf_fleet_adapter/test/phases/test_DoorClose.cpp +++ b/rmf_fleet_adapter/test/phases/test_DoorClose.cpp @@ -39,7 +39,7 @@ struct TestData std::list received_requests; std::condition_variable status_updates_cv; - std::list status_updates; + std::list status_updates; std::optional last_state_value() const { @@ -207,7 +207,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -230,7 +230,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") bool completed = test->status_updates_cv.wait_for( lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -254,7 +254,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") bool completed = test->status_updates_cv.wait_for( lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(!completed); } @@ -271,7 +271,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door close phase", "[phases]") { for (const auto& status : test->status_updates) { - if (status.state == Task::StatusMsg::STATE_COMPLETED) + if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED) return true; } test->status_updates.clear(); diff --git a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp index c9e97fd08..20c79f9f9 100644 --- a/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp +++ b/rmf_fleet_adapter/test/phases/test_DoorOpen.cpp @@ -41,7 +41,7 @@ struct TestData std::list received_requests; std::condition_variable status_updates_cv; - std::list status_updates; + std::list status_updates; std::optional last_state_value() const { @@ -226,7 +226,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -250,7 +250,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(!completed); } @@ -274,7 +274,7 @@ SCENARIO_METHOD(MockAdapterFixture, "door open phase", "[phases]") lk, std::chrono::milliseconds(10), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(!completed); } diff --git a/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp b/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp index b55ad55ee..2513618ac 100644 --- a/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp +++ b/rmf_fleet_adapter/test/phases/test_GoToPlace.cpp @@ -188,12 +188,12 @@ SCENARIO_METHOD(MockAdapterFixture, "go to place negotiation", "[phases]") const auto pending_0 = phases::GoToPlace::make(context_0, {start_0}, goal_0); const auto active_0 = info_0.schedule_and_wait( - std::function()>( + std::function()>( [&]() { return pending_0->begin(); })); const auto pending_1 = phases::GoToPlace::make(context_1, {start_1}, goal_1); const auto active_1 = info_1.schedule_and_wait( - std::function()>( + std::function()>( [&]() { return pending_1->begin(); })); const auto negotiators = std::make_shared(); diff --git a/rmf_fleet_adapter/test/phases/test_IngestItem.cpp b/rmf_fleet_adapter/test/phases/test_IngestItem.cpp index 719f14890..41fa5d21d 100644 --- a/rmf_fleet_adapter/test/phases/test_IngestItem.cpp +++ b/rmf_fleet_adapter/test/phases/test_IngestItem.cpp @@ -38,7 +38,7 @@ struct TestData std::list received_requests; std::condition_variable status_updates_cv; - std::list status_updates; + std::list status_updates; std::optional last_state_value() const { @@ -147,7 +147,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]") { for (const auto& status : test->status_updates) { - if (status.state == Task::StatusMsg::STATE_COMPLETED) + if (status.state == LegacyTask::StatusMsg::STATE_COMPLETED) return true; } test->status_updates.clear(); @@ -202,7 +202,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]") bool completed = test->status_updates_cv.wait_for( lk, std::chrono::milliseconds(1000), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -258,7 +258,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]") test->status_updates_cv.wait_for(lk, std::chrono::milliseconds( 1000), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_FAILED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_FAILED; }); CHECK(failed); } @@ -314,7 +314,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]") bool completed = test->status_updates_cv.wait_for(lk, std::chrono::milliseconds( 1000), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } @@ -370,7 +370,7 @@ SCENARIO_METHOD(MockAdapterFixture, "ingest item phase", "[phases]") bool completed = test->status_updates_cv.wait_for( lk, std::chrono::milliseconds(30), [test]() { - return test->last_state_value() == Task::StatusMsg::STATE_COMPLETED; + return test->last_state_value() == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(!completed); } diff --git a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp index b97d7a17e..6e2a76c1f 100644 --- a/rmf_fleet_adapter/test/phases/test_RequestLift.cpp +++ b/rmf_fleet_adapter/test/phases/test_RequestLift.cpp @@ -38,7 +38,7 @@ struct TestData std::string session_id; std::condition_variable status_updates_cv; - std::list status_updates; + std::list status_updates; }; } // anonymous namespace @@ -191,7 +191,7 @@ SCENARIO_METHOD(MockAdapterFixture, "request lift phase", "[phases]") if (test->status_updates.empty()) return false; const auto& state = test->status_updates.back().state; - return state == Task::StatusMsg::STATE_COMPLETED; + return state == LegacyTask::StatusMsg::STATE_COMPLETED; }); CHECK(completed); } diff --git a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp index 3b4b6902f..f459f1a54 100644 --- a/rmf_fleet_adapter/test/tasks/test_Delivery.cpp +++ b/rmf_fleet_adapter/test/tasks/test_Delivery.cpp @@ -447,20 +447,19 @@ SCENARIO("Test Delivery") std::this_thread::sleep_for(1s); // Dispatch Delivery Task - rmf_task_msgs::msg::TaskProfile task_profile; - task_profile.task_id = delivery_id; - task_profile.description.start_time = adapter.node()->now(); - task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; - - rmf_task_msgs::msg::Delivery delivery; - delivery.pickup_place_name = pickup_name; - delivery.pickup_dispenser = quiet_dispenser_name; - delivery.dropoff_place_name = dropoff_name; - delivery.dropoff_ingestor = flaky_ingestor_name; - - task_profile.description.delivery = delivery; - adapter.dispatch_task(task_profile); + nlohmann::json request; + request["category"] = "delivery"; + auto& desc = request["description"]; + auto& pickup = desc["pickup"]; + pickup["place"] = pickup_name; + pickup["handler"] = quiet_dispenser_name; + pickup["payload"] = std::vector(); + auto& dropoff = desc["dropoff"]; + dropoff["place"] = dropoff_name; + dropoff["handler"] = flaky_ingestor_name; + dropoff["payload"] = std::vector(); + + adapter.dispatch_task(delivery_id, request); const auto quiet_status = quiet_future.wait_for(15s); REQUIRE(quiet_status == std::future_status::ready); diff --git a/rmf_fleet_adapter/test/tasks/test_Loop.cpp b/rmf_fleet_adapter/test/tasks/test_Loop.cpp index 41f8b3b7a..f7df03fa4 100644 --- a/rmf_fleet_adapter/test/tasks/test_Loop.cpp +++ b/rmf_fleet_adapter/test/tasks/test_Loop.cpp @@ -302,18 +302,20 @@ SCENARIO("Test loop requests", "[.flaky]") rmf_task_msgs::msg::TaskType::TYPE_LOOP; // Dsipatch Loop 0 Task - task_profile.task_id = loop_0; - task_profile.description.loop.num_loops = n_loops; - task_profile.description.loop.robot_type = fleet_type; - task_profile.description.loop.start_name = south; - task_profile.description.loop.finish_name = east; - adapter.dispatch_task(task_profile); + nlohmann::json request; + request["category"] = "patrol"; + auto& desc = request["description"]; + auto& places = desc["places"]; + places.push_back(south); + places.push_back(east); + desc["rounds"] = n_loops; + adapter.dispatch_task(loop_0, request); // Dispatch Loop 1 Task - task_profile.task_id = loop_1; - task_profile.description.loop.start_name = north; - task_profile.description.loop.finish_name = east; - adapter.dispatch_task(task_profile); + places.clear(); + places.push_back(north); + places.push_back(east); + adapter.dispatch_task(loop_1, request); const auto task_0_completed_status = task_0_completed_future.wait_for(20s); CHECK(task_0_completed_status == std::future_status::ready); diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp index 9fe23155e..069ff22b2 100644 --- a/rmf_fleet_adapter/test/test_Task.cpp +++ b/rmf_fleet_adapter/test/test_Task.cpp @@ -17,7 +17,7 @@ #include -#include +#include #include "thread_cooldown.hpp" @@ -67,7 +67,7 @@ class MockPhase }; - class Active : public rmf_fleet_adapter::Task::ActivePhase + class Active : public rmf_fleet_adapter::LegacyTask::ActivePhase { public: @@ -124,7 +124,7 @@ class MockPhase }; - class Pending : public rmf_fleet_adapter::Task::PendingPhase + class Pending : public rmf_fleet_adapter::LegacyTask::PendingPhase { public: @@ -141,7 +141,7 @@ class MockPhase // Do nothing } - std::shared_ptr begin() final + std::shared_ptr begin() final { return std::make_shared(_desc, _count, _count_length, _period); } @@ -168,23 +168,26 @@ class MockSubtaskPhase { public: - using PendingPhases = rmf_fleet_adapter::Task::PendingPhases; + using PendingPhases = rmf_fleet_adapter::LegacyTask::PendingPhases; class Active - : public rmf_fleet_adapter::Task::ActivePhase, + : public rmf_fleet_adapter::LegacyTask::ActivePhase, public std::enable_shared_from_this { public: Active(PendingPhases phases) - : _subtasks( - rmf_fleet_adapter::Task::make( + { + rmf_task::State state; + state.load_basic( + {std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0); + _subtasks = + rmf_fleet_adapter::LegacyTask::make( "subtasks", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), std::chrono::steady_clock::now(), - {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0}, - nullptr)) - { + state, + nullptr); _desc = "subtasks"; _status_obs = _status_publisher.get_observable(); } @@ -237,7 +240,7 @@ class MockSubtaskPhase private: - std::shared_ptr _subtasks; + std::shared_ptr _subtasks; rxcpp::subscription _subscription; rxcpp::subjects::subject _status_publisher; rxcpp::observable _status_obs; @@ -246,7 +249,7 @@ class MockSubtaskPhase }; - class Pending : public rmf_fleet_adapter::Task::PendingPhase + class Pending : public rmf_fleet_adapter::LegacyTask::PendingPhase { public: @@ -256,7 +259,7 @@ class MockSubtaskPhase _desc = "subtasks"; } - std::shared_ptr begin() final + std::shared_ptr begin() final { auto active = std::make_shared(std::move(_phases)); active->begin(); @@ -288,17 +291,18 @@ SCENARIO("Test simple task") std::shared_ptr count = std::make_shared(0); const auto dt = 10ms; - rmf_fleet_adapter::Task::PendingPhases phases; + rmf_fleet_adapter::LegacyTask::PendingPhases phases; phases.push_back(std::make_unique("A", count, 5, dt)); phases.push_back(std::make_unique("B", count, 15, dt)); phases.push_back(std::make_unique("C", count, 18, dt)); // Dummy parameters rmf_traffic::Time deployment_time; - rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; + rmf_task::State finish_state; + finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0); - std::shared_ptr task = - rmf_fleet_adapter::Task::make( + std::shared_ptr task = + rmf_fleet_adapter::LegacyTask::make( "test_Task", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); @@ -307,7 +311,7 @@ SCENARIO("Test simple task") auto completed_future = completed_promise.get_future(); auto status_sub = task->observe() .subscribe( - [](const rmf_fleet_adapter::Task::StatusMsg& msg) + [](const rmf_fleet_adapter::LegacyTask::StatusMsg& msg) { if (msg.status.find("A") != std::string::npos) { @@ -344,7 +348,7 @@ SCENARIO("Test nested task") { rmf_fleet_adapter_test::thread_cooldown = true; - using PendingPhases = rmf_fleet_adapter::Task::PendingPhases; + using PendingPhases = rmf_fleet_adapter::LegacyTask::PendingPhases; std::shared_ptr count = std::make_shared(0); const auto dt = 10ms; @@ -395,9 +399,10 @@ SCENARIO("Test nested task") // Dummy parameters rmf_traffic::Time deployment_time; - rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; + rmf_task::State finish_state; + finish_state.load_basic({deployment_time, 0, 0.0}, 0, 1.0); - const auto task = rmf_fleet_adapter::Task::make( + const auto task = rmf_fleet_adapter::LegacyTask::make( "test_NestedTask", std::move(phases), rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, finish_state, nullptr); @@ -406,7 +411,7 @@ SCENARIO("Test nested task") auto completed_future = completed_promise.get_future(); auto status_sub = task->observe() .subscribe( - [&count_limits](const rmf_fleet_adapter::Task::StatusMsg& msg) + [&count_limits](const rmf_fleet_adapter::LegacyTask::StatusMsg& msg) { std::pair limits = {0, 0}; for (const auto& element : count_limits) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 929fcbc72..2b99b12f4 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -353,7 +353,8 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("add_fleet", &agv::Adapter::add_fleet, py::arg("fleet_name"), py::arg("traits"), - py::arg("navigation_graph")) + py::arg("navigation_graph"), + py::arg("server_uri") = std::nullopt) .def("add_easy_traffic_light", &agv::Adapter::add_easy_traffic_light, py::arg("handle_callback"), py::arg("fleet_name"), @@ -383,7 +384,8 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("add_fleet", &agv::test::MockAdapter::add_fleet, py::arg("fleet_name"), py::arg("traits"), - py::arg("navigation_graph")) + py::arg("navigation_graph"), + py::arg("server_uri") = std::nullopt) .def_property_readonly("node", py::overload_cast<>( &agv::test::MockAdapter::node)) diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt index 9b04c0641..0cd778244 100644 --- a/rmf_task_ros2/CMakeLists.txt +++ b/rmf_task_ros2/CMakeLists.txt @@ -14,21 +14,27 @@ endif() include(GNUInstallDirs) find_package(ament_cmake REQUIRED) +find_package(rmf_api_msgs REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_traffic_ros2 REQUIRED) find_package(rmf_task_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(rclcpp REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(nlohmann_json_schema_validator_vendor REQUIRED) file(GLOB_RECURSE core_lib_srcs "src/rmf_task_ros2/*.cpp") add_library(rmf_task_ros2 SHARED ${core_lib_srcs}) target_link_libraries(rmf_task_ros2 PUBLIC + rmf_api_msgs::rmf_api_msgs rmf_traffic::rmf_traffic rmf_traffic_ros2::rmf_traffic_ros2 ${rmf_task_msgs_LIBRARIES} ${rclcpp_LIBRARIES} + nlohmann_json::nlohmann_json + nlohmann_json_schema_validator ) target_include_directories(rmf_task_ros2 @@ -41,7 +47,7 @@ target_include_directories(rmf_task_ros2 ) ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) -ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp) +ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp nlohmann_json) #=============================================================================== diff --git a/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp new file mode 100644 index 000000000..151129478 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/DispatchState.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2020 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_TASK_ROS2__DISPATCH_STATE_HPP +#define RMF_TASK_ROS2__DISPATCH_STATE_HPP + +#include +#include + +#include +#include + +#include + +namespace rmf_task_ros2 { + +//============================================================================== +using TaskID = std::string; + +//============================================================================== +/// \note TaskStatus struct is based on TaskSummary.msg +struct DispatchState +{ + using Msg = rmf_task_msgs::msg::DispatchState; + + enum class Status : uint8_t + { + /// This task has not been assigned yet + Queued = Msg::STATUS_QUEUED, + + /// An assignment has been selected, but we have not received acknowledgment + /// from the assignee + Selected = Msg::STATUS_SELECTED, + + /// The task is dispatched and no longer being managed by the dispatcher + Dispatched = Msg::STATUS_DISPATCHED, + + /// There was a failure to assign the task to anyone + FailedToAssign = Msg::STATUS_FAILED_TO_ASSIGN, + + /// The task was canceled before it managed to get dispatched + CanceledInFlight = Msg::STATUS_CANCELED_IN_FLIGHT + }; + + struct Assignment + { + std::string fleet_name; + std::string expected_robot_name; + }; + + /// The Task ID for that this dispatching refers to + std::string task_id; + + /// Submission arrival time + rmf_traffic::Time submission_time; + + /// The status of this dispatching + Status status = Status::Queued; + + /// The assignment that was made for this dispatching + std::optional assignment; + + /// Any errors that have occurred for this dispatching + std::vector errors; + + DispatchState(std::string task_id, rmf_traffic::Time submission_time); +}; + +using DispatchStatePtr = std::shared_ptr; + +//============================================================================== +rmf_task_msgs::msg::Assignment convert( + const std::optional& assignment); + +//============================================================================== +rmf_task_msgs::msg::DispatchState convert(const DispatchState& state); + +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__DISPATCH_STATE_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp index f01ebf656..d13985dd4 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/Dispatcher.hpp @@ -24,8 +24,10 @@ #include #include -#include +#include +// Deprecated message definition +#include namespace rmf_task_ros2 { @@ -35,8 +37,7 @@ namespace rmf_task_ros2 { class Dispatcher : public std::enable_shared_from_this { public: - using DispatchTasks = std::unordered_map; - using TaskDescription = rmf_task_msgs::msg::TaskDescription; + using DispatchStates = std::unordered_map; /// Initialize an rclcpp context and make an dispatcher instance. This will /// instantiate an rclcpp::Node, a task dispatcher node. Dispatcher node will @@ -78,8 +79,9 @@ class Dispatcher : public std::enable_shared_from_this /// /// \return task_id /// self-generated task_id, nullopt is submit task failed + [[deprecated]] std::optional submit_task( - const TaskDescription& task_description); + const rmf_task_msgs::msg::TaskDescription& task_description); /// Cancel an active task which was previously submitted to Dispatcher. This /// will terminate the task with a State of: `Canceled`. If a task is @@ -99,29 +101,30 @@ class Dispatcher : public std::enable_shared_from_this /// task_id obtained from `submit_task()` /// /// \return State of the task, nullopt if task is not available - const rmf_utils::optional get_task_state( + std::optional get_dispatch_state( const TaskID& task_id) const; /// Get a mutable ref of active tasks map list handled by dispatcher - const DispatchTasks& active_tasks() const; + const DispatchStates& active_dispatches() const; /// Get a mutable ref of terminated tasks map list - const DispatchTasks& terminated_tasks() const; + const DispatchStates& finished_dispatches() const; - using StatusCallback = std::function; + using DispatchStateCallback = + std::function; /// Trigger this callback when a task status is changed. This will return the /// Changed task status. /// /// \param [in] callback function - void on_change(StatusCallback on_change_fn); + void on_change(DispatchStateCallback on_change_fn); /// Change the default evaluator to a custom evaluator, which is used by /// bidding auctioneer. Default evaluator is: `LeastFleetDiffCostEvaluator` /// /// \param [in] evaluator /// evaluator used to select the best bid from fleets - void evaluator(std::shared_ptr evaluator); + void evaluator(bidding::Auctioneer::ConstEvaluatorPtr evaluator); /// Get the rclcpp::Node that this dispatcher will be using for communication. std::shared_ptr node(); diff --git a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp index 6d1ac8338..fea8c49eb 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/StandardNames.hpp @@ -24,16 +24,16 @@ namespace rmf_task_ros2 { const std::string Prefix = "rmf_task/"; const std::string BidNoticeTopicName = Prefix + "bid_notice"; -const std::string BidProposalTopicName = Prefix + "bid_proposal"; +const std::string BidResponseTopicName = Prefix + "bid_response"; const std::string SubmitTaskSrvName = "submit_task"; const std::string CancelTaskSrvName = "cancel_task"; -const std::string GetTaskListSrvName = "get_tasks"; +const std::string GetDispatchStatesSrvName = "get_dispatches"; -const std::string TaskRequestTopicName = Prefix + "dispatch_request"; -const std::string TaskAckTopicName = Prefix + "dispatch_ack"; +const std::string DispatchCommandTopicName = Prefix + "dispatch_request"; +const std::string DispatchAckTopicName = Prefix + "dispatch_ack"; +const std::string DispatchStatesTopicName = "dispatch_states"; const std::string TaskStatusTopicName = "task_summaries"; -const std::string ActiveTasksTopicName = "dispatcher_ongoing_tasks"; } // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp b/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp deleted file mode 100644 index a8f96fcd6..000000000 --- a/rmf_task_ros2/include/rmf_task_ros2/TaskStatus.hpp +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2020 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_TASK_ROS2__TASK_STATUS_HPP -#define RMF_TASK_ROS2__TASK_STATUS_HPP - -#include -#include -#include -#include - -namespace rmf_task_ros2 { - -//============================================================================== -using TaskProfile = rmf_task_msgs::msg::TaskProfile; -using StatusMsg = rmf_task_msgs::msg::TaskSummary; -using TaskID = std::string; - -//============================================================================== -/// \note TaskStatus struct is based on TaskSummary.msg -struct TaskStatus -{ - enum class State : uint8_t - { - Queued = StatusMsg::STATE_QUEUED, - Executing = StatusMsg::STATE_ACTIVE, - Completed = StatusMsg::STATE_COMPLETED, - Failed = StatusMsg::STATE_FAILED, - Canceled = StatusMsg::STATE_CANCELED, - Pending = StatusMsg::STATE_PENDING - }; - - std::string fleet_name; - TaskProfile task_profile; - rmf_traffic::Time start_time; - rmf_traffic::Time end_time; - std::string robot_name; - std::string status; // verbose msg - State state = State::Pending; // default - - bool is_terminated() const; -}; - -using TaskStatusPtr = std::shared_ptr; - -// ============================================================================== -TaskStatus convert_status(const StatusMsg& from); - -// ============================================================================== -StatusMsg convert_status(const TaskStatus& from); - -} // namespace rmf_task_ros2 - -#endif // RMF_TASK_ROS2__TASK_STATUS_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp similarity index 65% rename from rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp rename to rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp index ef53db6fc..80bfd12e5 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/MinimalBidder.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/AsyncBidder.hpp @@ -15,34 +15,25 @@ * */ -#ifndef RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP -#define RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP +#ifndef RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP +#define RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP #include #include #include -#include +#include namespace rmf_task_ros2 { namespace bidding { //============================================================================== -class MinimalBidder +class AsyncBidder { public: - /// Type of Task in RMF - using TaskTypeMsg = rmf_task_msgs::msg::TaskType; - enum class TaskType - { - Station = TaskTypeMsg::TYPE_STATION, - Loop = TaskTypeMsg::TYPE_LOOP, - Delivery = TaskTypeMsg::TYPE_DELIVERY, - ChargeBattery = TaskTypeMsg::TYPE_CHARGE_BATTERY, - Clean = TaskTypeMsg::TYPE_CLEAN, - Patrol = TaskTypeMsg::TYPE_PATROL - }; + + using Respond = std::function; /// Callback function when a bid notice is received from the autioneer /// @@ -51,9 +42,8 @@ class MinimalBidder /// /// \return submission /// Estimates of a task. This submission is used by dispatcher for eval - using ParseSubmissionCallback = - std::function; - + using ReceiveNotice = + std::function; /// Create a bidder to bid for incoming task requests from Task Dispatcher /// @@ -68,20 +58,18 @@ class MinimalBidder /// /// \param[in] submission_cb /// fn which is used to provide a bid submission during a call for bid - static std::shared_ptr make( + static std::shared_ptr make( const std::shared_ptr& node, - const std::string& fleet_name, - const std::unordered_set& valid_task_types, - ParseSubmissionCallback submission_cb); + ReceiveNotice notice_cb); class Implementation; private: - MinimalBidder(); + AsyncBidder(); rmf_utils::unique_impl_ptr _pimpl; }; } // namespace bidding } // namespace rmf_task_ros2 -#endif // RMF_TASK_ROS2__BIDDING__MINIMALBIDDER_HPP +#endif // RMF_TASK_ROS2__BIDDING__ASYNCBIDDER_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp index 4b9b5a87d..fb21adf69 100644 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Auctioneer.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include namespace rmf_task_ros2 { namespace bidding { @@ -34,6 +34,7 @@ namespace bidding { class Auctioneer : public std::enable_shared_from_this { public: + /// Callback which will provide the winner when a bid is concluded /// /// \param[in] task_id @@ -42,8 +43,27 @@ class Auctioneer : public std::enable_shared_from_this /// \param[in] winner /// single winner from all submissions. nullopt if non using BiddingResultCallback = - std::function winner)>; + std::function< + void( + const std::string& task_id, + const std::optional winner, + const std::vector& errors)>; + + /// A pure abstract interface class for the auctioneer to choose the best + /// choosing the best submissions. + class Evaluator + { + public: + + /// Given a list of submissions, choose the one that is the "best". It is + /// up to the implementation of the Evaluator to decide how to rank. + virtual std::optional choose( + const Responses& responses) const = 0; + + virtual ~Evaluator() = default; + }; + + using ConstEvaluatorPtr = std::shared_ptr; /// Create an instance of the Auctioneer. This instance will handle all /// the task dispatching bidding mechanism. A default evaluator is used. @@ -57,33 +77,24 @@ class Auctioneer : public std::enable_shared_from_this /// \sa make() static std::shared_ptr make( const std::shared_ptr& node, - BiddingResultCallback result_callback); + BiddingResultCallback result_callback, + ConstEvaluatorPtr evaluator); /// Start a bidding process by provide a bidding task. Note the each /// bidding process is conducted sequentially /// /// \param[in] bid_notice /// bidding task, task which will call for bid - void start_bidding(const BidNotice& bid_notice); - - /// A pure abstract interface class for the auctioneer to choose the best - /// choosing the best submissions. - class Evaluator - { - public: - - /// Given a list of submissions, choose the one that is the "best". It is - /// up to the implementation of the Evaluator to decide how to rank. - virtual std::size_t choose(const Submissions& submissions) const = 0; + void request_bid(const BidNoticeMsg& bid_notice); - virtual ~Evaluator() = default; - }; + /// Call this to tell the auctioneer that it may begin to perform the next bid + void ready_for_next_bid(); /// Provide a custom evaluator which will be used to choose the best bid /// If no selection is given, Default is: LeastFleetDiffCostEvaluator /// /// \param[in] evaluator - void select_evaluator(std::shared_ptr evaluator); + void set_evaluator(ConstEvaluatorPtr evaluator); class Implementation; @@ -96,21 +107,21 @@ class Auctioneer : public std::enable_shared_from_this class LeastFleetDiffCostEvaluator : public Auctioneer::Evaluator { public: - std::size_t choose(const Submissions& submissions) const final; + std::optional choose(const Responses& responses) const final; }; //============================================================================== class LeastFleetCostEvaluator : public Auctioneer::Evaluator { public: - std::size_t choose(const Submissions& submissions) const final; + std::optional choose(const Responses& submissions) const final; }; //============================================================================== class QuickestFinishEvaluator : public Auctioneer::Evaluator { public: - std::size_t choose(const Submissions& submissions) const final; + std::optional choose(const Responses& submissions) const final; }; } // namespace bidding diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp new file mode 100644 index 000000000..3bf78f407 --- /dev/null +++ b/rmf_task_ros2/include/rmf_task_ros2/bidding/Response.hpp @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2020 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_TASK_ROS2__BIDDING__RESPONSE_HPP +#define RMF_TASK_ROS2__BIDDING__RESPONSE_HPP + +#include +#include +#include +#include + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +struct Response +{ + struct Proposal + { + std::string fleet_name; + std::string expected_robot_name; + double prev_cost; + double new_cost; + rmf_traffic::Time finish_time; + }; + + std::optional proposal; + std::vector errors; +}; + +//============================================================================== +using Responses = std::vector; + +//============================================================================== +using BidResponseMsg = rmf_task_msgs::msg::BidResponse; +using BidProposalMsg = rmf_task_msgs::msg::BidProposal; +using BidNoticeMsg = rmf_task_msgs::msg::BidNotice; + +//============================================================================== +BidProposalMsg convert(const Response::Proposal& proposal); + +//============================================================================== +BidResponseMsg convert( + const Response& response, + const std::string& task_id); + +//============================================================================== +Response convert(const BidResponseMsg& msg); + +} // namespace bidding +} // namespace rmf_task_ros2 + +#endif // RMF_TASK_ROS2__BIDDING__RESPONSE_HPP diff --git a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp b/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp deleted file mode 100644 index b9b8cc813..000000000 --- a/rmf_task_ros2/include/rmf_task_ros2/bidding/Submission.hpp +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright (C) 2020 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_TASK_ROS2__BIDDING__SUBMISSION_HPP -#define RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP - -#include -#include - -namespace rmf_task_ros2 { -namespace bidding { - -//============================================================================== -struct Submission -{ - std::string fleet_name; - std::string robot_name; - double prev_cost = 0.0; - double new_cost = std::numeric_limits::max(); - rmf_traffic::Time finish_time; -}; - -//============================================================================== -using Submissions = std::vector; -using BidNotice = rmf_task_msgs::msg::BidNotice; - -} // namespace bidding -} // namespace rmf_task_ros2 - -#endif // RMF_TASK_ROS2__BIDDING__SUBMISSION_HPP diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 155a2d6c6..1945557a9 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -11,10 +11,13 @@ ament_cmake rmf_utils + rmf_api_msgs rmf_traffic rmf_traffic_ros2 rmf_task_msgs rclcpp + nlohmann-json-dev + nlohmann_json_schema_validator_vendor eigen diff --git a/rmf_task_ros2/src/mock_bidder/main.cpp b/rmf_task_ros2/src/mock_bidder/main.cpp index 7340c6b30..96022a686 100644 --- a/rmf_task_ros2/src/mock_bidder/main.cpp +++ b/rmf_task_ros2/src/mock_bidder/main.cpp @@ -17,14 +17,14 @@ /// Note: This is a testing bidder node script -#include -#include "../rmf_task_ros2/action/Server.hpp" +#include #include #include +#include + using namespace rmf_task_ros2; -using TaskType = bidding::MinimalBidder::TaskType; int main(int argc, char* argv[]) { @@ -39,73 +39,31 @@ int main(int argc, char* argv[]) //============================================================================ // Create Bidder instance - std::shared_ptr bidder = bidding::MinimalBidder::make( + std::shared_ptr bidder = bidding::AsyncBidder::make( node, - fleet_name, - { TaskType::Clean, TaskType::Delivery }, - [](const bidding::BidNotice& notice) + [node]( + const bidding::BidNoticeMsg& notice, + bidding::AsyncBidder::Respond resp) { - // Here user will provice the best robot as a bid submission + // Here user will provide the best robot as a bid submission std::cout << "[MockBidder] Providing best estimates" << std::endl; - auto req_start_time = - rmf_traffic_ros2::convert(notice.task_profile.description.start_time); - - bidding::Submission best_robot_estimate; - best_robot_estimate.robot_name = "dumbot"; - best_robot_estimate.prev_cost = 10.2; - best_robot_estimate.new_cost = 13.5; - best_robot_estimate.finish_time = - rmf_traffic::time::apply_offset(req_start_time, 7); - return best_robot_estimate; - } - ); - - //============================================================================ - // Create sample RMF task action server - auto action_server = action::Server::make(node, fleet_name); - - action_server->register_callbacks( - [&action_server, &node](const TaskProfile& task_profile) - { - std::cout << "[MockBidder] ~Start Queue Task: " - << task_profile.task_id<now()); - status.end_time = - rmf_traffic::time::apply_offset(status.start_time, 7); - - const auto id = profile.task_id; - std::cout << " [MockBidder] Queued, TaskID: " << id << std::endl; - action_server->update_status(status); - - std::this_thread::sleep_for(std::chrono::seconds(2)); - std::cout << " [MockBidder] Executing, TaskID: " << id << std::endl; - status.state = TaskStatus::State::Executing; - action_server->update_status(status); - - std::this_thread::sleep_for(std::chrono::seconds(5)); - std::cout << " [MockBidder] Completed, TaskID: " << id << std::endl; - status.state = TaskStatus::State::Completed; - action_server->update_status(status); - }, task_profile - ); - t.detach(); - - return true; //successs (send State::Queued) - }, - [&action_server](const TaskProfile& task_profile) - { - std::cout << "[MockBidder] ~Cancel Executing Task: " - << task_profile.task_id<())) + : rmf_traffic_ros2::convert(node->now()); + + resp(bidding::Response{ + bidding::Response::Proposal{ + "mockfleet", + "mockbot", + 10.2, + 13.5, + rmf_traffic::time::apply_offset(req_start_time, 7) + }, + {} + }); } ); diff --git a/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp new file mode 100644 index 000000000..b2f2ba2b2 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/DispatchState.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2020 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 + +namespace rmf_task_ros2 { + +//============================================================================== +DispatchState::DispatchState( + std::string task_id_, + rmf_traffic::Time submission_time_) +: task_id(std::move(task_id_)), + submission_time(submission_time_) +{ + // Do nothing +} + +//============================================================================= +rmf_task_msgs::msg::Assignment convert( + const std::optional& assignment) +{ + if (assignment.has_value()) + { + return rmf_task_msgs::build() + .is_assigned(true) + .fleet_name(assignment->fleet_name) + .expected_robot_name(assignment->expected_robot_name); + } + + return rmf_task_msgs::build() + .is_assigned(false) + .fleet_name("") + .expected_robot_name(""); +} + +//============================================================================= +std::vector convert(const std::vector& values) +{ + std::vector output; + output.reserve(values.size()); + for (const auto& v : values) + output.push_back(v.dump()); + + return output; +} + +//============================================================================= +rmf_task_msgs::msg::DispatchState convert(const DispatchState& state) +{ + return rmf_task_msgs::build() + .task_id(state.task_id) + .status(static_cast(state.status)) + .assignment(convert(state.assignment)) + .errors(convert(state.errors)); +} + +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 1932bbed2..9a5fb8d39 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -16,94 +16,245 @@ */ #include +#include #include #include -#include "action/Client.hpp" - +#include +#include #include #include -#include +#include +#include +#include +#include #include +#include +#include #include +#include +#include + +#include +#include +#include +#include +#include + +#include + namespace rmf_task_ros2 { +namespace { +//============================================================================== +std::function +make_schema_loader() +{ + const std::vector schemas = { + rmf_api_msgs::schemas::dispatch_task_request, + rmf_api_msgs::schemas::dispatch_task_response, + rmf_api_msgs::schemas::task_request, + rmf_api_msgs::schemas::task_state, + rmf_api_msgs::schemas::error + }; + + std::unordered_map dictionary; + for (const auto& schema : schemas) + { + const auto json_uri = nlohmann::json_uri(schema["$id"]); + dictionary[json_uri.url()] = schema; + } + + return [dictionary = std::move(dictionary)]( + const nlohmann::json_uri& id, nlohmann::json& value) + { + const auto it = dictionary.find(id.url()); + if (it != dictionary.end()) + value = it->second; + }; +} + +//============================================================================== +static const +std::function +schema_loader = make_schema_loader(); + +//============================================================================== +nlohmann::json_schema::json_validator make_validator(nlohmann::json schema) +{ + return nlohmann::json_schema::json_validator( + std::move(schema), schema_loader); +} +} // anonymous namespace + //============================================================================== class Dispatcher::Implementation { public: + std::shared_ptr node; std::shared_ptr auctioneer; - std::shared_ptr action_client; using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; - using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; - using TasksMsg = rmf_task_msgs::msg::Tasks; + using GetDispatchStatesSrv = rmf_task_msgs::srv::GetDispatchStates; + using DispatchStateMsg = rmf_task_msgs::msg::DispatchState; + using DispatchStatesMsg = rmf_task_msgs::msg::DispatchStates; + using TaskDescription = rmf_task_msgs::msg::TaskDescription; + using DispatchCommandMsg = rmf_task_msgs::msg::DispatchCommand; + using DispatchAckMsg = rmf_task_msgs::msg::DispatchAck; rclcpp::Service::SharedPtr submit_task_srv; rclcpp::Service::SharedPtr cancel_task_srv; - rclcpp::Service::SharedPtr get_task_list_srv; + rclcpp::Service::SharedPtr get_dispatch_states_srv; + + using ApiRequestMsg = rmf_task_msgs::msg::ApiRequest; + using ApiResponseMsg = rmf_task_msgs::msg::ApiResponse; + rclcpp::Subscription::SharedPtr api_request; + rclcpp::Publisher::SharedPtr api_response; + + class ApiMemory + { + public: + + std::optional lookup(const std::string& api_id) const + { + const auto it = _cached_responses.find(api_id); + if (it == _cached_responses.end()) + return std::nullopt; + + return it->second; + } + + void add(ApiResponseMsg msg) + { + if (_tracker.size() > _max_size) + { + _cached_responses.erase(_tracker.front()); + _tracker.pop_front(); + } + + _tracker.push_back(msg.request_id); + _cached_responses[msg.request_id] = std::move(msg); + } + + private: + + std::unordered_map _cached_responses; + std::list _tracker; + std::size_t _max_size = 50; + }; + + ApiMemory api_memory; - using ActiveTasksPub = rclcpp::Publisher; - ActiveTasksPub::SharedPtr ongoing_tasks_pub; + using DispatchStatesPub =rclcpp::Publisher; + DispatchStatesPub::SharedPtr dispatch_states_pub; + rclcpp::TimerBase::SharedPtr dispatch_states_pub_timer; - rclcpp::TimerBase::SharedPtr timer; + uint64_t next_dispatch_command_id = 0; + std::unordered_map lingering_commands; + rclcpp::TimerBase::SharedPtr dispatch_command_timer; + rclcpp::Publisher::SharedPtr dispatch_command_pub; - StatusCallback on_change_fn; + rclcpp::Subscription::SharedPtr dispatch_ack_sub; - std::queue queue_bidding_tasks; + DispatchStateCallback on_change_fn; - /// TODO: should rename "active" to "ongoing" to prevent confusion - /// of with task STATE_ACTIVE - DispatchTasks active_dispatch_tasks; - DispatchTasks terminal_dispatch_tasks; - std::set user_submitted_tasks; // ongoing submitted task_ids + DispatchStates active_dispatch_states; + DispatchStates finished_dispatch_states; std::size_t task_counter = 0; // index for generating task_id - double bidding_time_window; - int terminated_tasks_max_size; + builtin_interfaces::msg::Duration bidding_time_window; + std::size_t terminated_tasks_max_size; int publish_active_tasks_period; - std::unordered_map task_type_name = - { - {0, "Station"}, - {1, "Loop"}, - {2, "Delivery"}, - {3, "ChargeBattery"}, - {4, "Clean"}, - {5, "Patrol"} - }; + std::unordered_map legacy_task_type_names = + { + {1, "patrol"}, + {2, "delivery"}, + {4, "clean"} + }; + + using LegacyConversion = + std::function; + using LegacyConversionMap = std::unordered_map; + LegacyConversionMap legacy_task_types; Implementation(std::shared_ptr node_) : node{std::move(node_)} { // ros2 param - bidding_time_window = + double bidding_time_window_param = node->declare_parameter("bidding_time_window", 2.0); RCLCPP_INFO(node->get_logger(), - " Declared Time Window Param as: %f secs", bidding_time_window); + " Declared Time Window Param as: %f secs", bidding_time_window_param); + bidding_time_window = rmf_traffic_ros2::convert( + rmf_traffic::time::from_seconds(bidding_time_window_param)); + terminated_tasks_max_size = node->declare_parameter("terminated_tasks_max_size", 100); RCLCPP_INFO(node->get_logger(), - " Declared Terminated Tasks Max Size Param as: %d", + " Declared Terminated Tasks Max Size Param as: %lu", terminated_tasks_max_size); publish_active_tasks_period = node->declare_parameter("publish_active_tasks_period", 2); RCLCPP_INFO(node->get_logger(), - " Declared publish_active_tasks_period as: %f secs", + " Declared publish_active_tasks_period as: %d secs", publish_active_tasks_period); const auto qos = rclcpp::ServicesQoS().reliable(); - ongoing_tasks_pub = node->create_publisher( - rmf_task_ros2::ActiveTasksTopicName, qos); + dispatch_states_pub = node->create_publisher( + rmf_task_ros2::DispatchStatesTopicName, qos); + + // TODO(MXG): Sync up with rmf_fleet_adapter/StandardNames on these topic + // names + api_request = node->create_subscription( + "task_api_requests", + rclcpp::SystemDefaultsQoS().reliable().transient_local(), + [this](const ApiRequestMsg::UniquePtr msg) + { + this->handle_api_request(*msg); + }); - timer = node->create_wall_timer( + api_response = node->create_publisher( + "task_api_responses", + rclcpp::SystemDefaultsQoS().reliable().transient_local()); + + // TODO(MXG): The smallest resolution this supports is 1 second. That + // doesn't seem great. + dispatch_states_pub_timer = node->create_wall_timer( std::chrono::seconds(publish_active_tasks_period), - std::bind( - &Dispatcher::Implementation::publish_ongoing_tasks, this)); + [this]() { this->publish_dispatch_states(); }); + + dispatch_command_pub = node->create_publisher( + rmf_task_ros2::DispatchCommandTopicName, + rclcpp::ServicesQoS().keep_last(20).reliable().transient_local()); + + // TODO(MXG): Make this publishing period configurable + dispatch_command_timer = node->create_wall_timer( + std::chrono::seconds(1), + [this](){ this->publish_lingering_commands(); }); + + dispatch_ack_sub = node->create_subscription( + rmf_task_ros2::DispatchAckTopicName, + rclcpp::ServicesQoS().keep_last(20).transient_local(), + [this](const DispatchAckMsg::UniquePtr msg) + { + this->handle_dispatch_ack(*msg); + }); + + auctioneer = bidding::Auctioneer::make( + node, + [this]( + const TaskID& task_id, + const std::optional winner, + const std::vector& errors) + { + this->conclude_bid(task_id, std::move(winner), errors); + }, + std::make_shared()); // Setup up stream srv interfaces submit_task_srv = node->create_service( @@ -136,294 +287,644 @@ class Dispatcher::Implementation } ); - get_task_list_srv = node->create_service( - rmf_task_ros2::GetTaskListSrvName, + get_dispatch_states_srv = node->create_service( + rmf_task_ros2::GetDispatchStatesSrvName, [this]( - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { - for (auto task : (this->active_dispatch_tasks)) - { - response->active_tasks.push_back( - rmf_task_ros2::convert_status(*(task.second))); - } + std::unordered_set relevant_tasks; + relevant_tasks.insert( + request->task_ids.begin(), request->task_ids.end()); + + const auto fill_states = [&relevant_tasks](auto& into, const auto& from) + { + for (const auto& [id, state] : from) + { + if (relevant_tasks.empty()) + into.push_back(convert(*state)); + else if (relevant_tasks.count(id)) + into.push_back(convert(*state)); + } + }; + + fill_states(response->states.active, this->active_dispatch_states); + fill_states(response->states.finished, this->finished_dispatch_states); - // Terminated Tasks - for (auto task : (this->terminal_dispatch_tasks)) - { - response->terminated_tasks.push_back( - rmf_task_ros2::convert_status(*(task.second))); - } response->success = true; } ); - } - void start() - { - using namespace std::placeholders; - auctioneer = bidding::Auctioneer::make(node, - std::bind(&Implementation::receive_bidding_winner_cb, this, _1, _2)); - action_client->on_terminate( - std::bind(&Implementation::terminate_task, this, _1)); - action_client->on_change( - std::bind(&Implementation::task_status_cb, this, _1)); + // Loop + legacy_task_types["patrol"] = + [](const TaskDescription& task_description) -> nlohmann::json + { + const auto& loop = task_description.loop; + nlohmann::json description; + std::vector places; + places.push_back(loop.start_name); + places.push_back(loop.finish_name); + description["places"] = std::move(places); + description["rounds"] = loop.num_loops; + + return description; + }; + + // Delivery + legacy_task_types["delivery"] = + [](const TaskDescription& task_description) -> nlohmann::json + { + const auto& delivery = task_description.delivery; + std::vector payload; + payload.reserve(delivery.items.size()); + for (const auto& item : delivery.items) + { + nlohmann::json item_json; + item_json["sku"] = item.type_guid; + item_json["quantity"] = item.quantity; + item_json["compartment"] = item.compartment_name; + payload.push_back(item_json); + } + + nlohmann::json pickup; + pickup["place"] = delivery.pickup_place_name; + pickup["handler"] = delivery.pickup_dispenser; + pickup["payload"] = payload; + + nlohmann::json dropoff; + dropoff["place"] = delivery.dropoff_place_name; + dropoff["handler"] = delivery.dropoff_ingestor; + dropoff["payload"] = payload; + + nlohmann::json description; + description["pickup"] = pickup; + description["dropoff"] = dropoff; + + return description; + }; + + // Clean + legacy_task_types["clean"] = + [](const TaskDescription& task_description) -> nlohmann::json + { + const auto& clean = task_description.clean; + nlohmann::json description; + description["zone"] = clean.start_waypoint; + + return description; + }; } - std::optional submit_task(const TaskDescription& description) + void handle_api_request(const ApiRequestMsg& msg) { - TaskProfile submitted_task; - submitted_task.submission_time = node->now(); - submitted_task.description = description; + const auto check = api_memory.lookup(msg.request_id); + if (check.has_value()) + { + api_response->publish(*check); + return; + } + + nlohmann::json msg_json; + try + { + msg_json = nlohmann::json::parse(msg.json_msg); + } + catch(const std::exception& e) + { + RCLCPP_ERROR( + node->get_logger(), + "Error parsing json_msg: %s", + e.what()); + return; + } - const auto task_type = static_cast(description.task_type.type); + const auto type_it = msg_json.find("type"); + if (type_it == msg_json.end()) + { + // Whatever type of message this is, we don't support it + return; + } - if (!task_type_name.count(task_type)) + if (!type_it.value().is_string()) { - RCLCPP_ERROR(node->get_logger(), "TaskType: %ld is invalid", task_type); - return std::nullopt; + // We expect the type field to contain a string + return; } - // auto generate a task_id for a given submitted task - submitted_task.task_id = - task_type_name[task_type] + std::to_string(task_counter++); + try + { + const auto& type_str = type_it.value().get(); + if (type_str != "dispatch_task_request") + { + return; + } - RCLCPP_INFO(node->get_logger(), - "Received Task Submission [%s]", submitted_task.task_id.c_str()); + static const auto request_validator = + make_validator(rmf_api_msgs::schemas::dispatch_task_request); - // add task to internal cache - TaskStatus status; - status.task_profile = submitted_task; - auto new_task_status = std::make_shared(status); - active_dispatch_tasks[submitted_task.task_id] = new_task_status; - user_submitted_tasks.insert(submitted_task.task_id); + try + { + request_validator.validate(msg_json); + } + catch (const std::exception& e) + { + nlohmann::json error; + error["code"] = 5; + error["category"] = "Invalid request format"; + error["detail"] = e.what(); + + nlohmann::json response_json; + response_json["success"] = false; + response_json["errors"] = + std::vector({std::move(error)}); + + auto response = rmf_task_msgs::build() + .type(ApiResponseMsg::TYPE_RESPONDING) + .json_msg(std::move(response_json)) + .request_id(msg.request_id); + + api_memory.add(response); + api_response->publish(response); + return; + } - if (on_change_fn) - on_change_fn(new_task_status); + const auto& task_request_json = msg_json["request"]; + const std::string task_id = + task_request_json["category"].get() + + ".dispatch-" + std::to_string(task_counter++); - bidding::BidNotice bid_notice; - bid_notice.task_profile = submitted_task; - bid_notice.time_window = rmf_traffic_ros2::convert( - rmf_traffic::time::from_seconds(bidding_time_window)); - queue_bidding_tasks.push(bid_notice); + const auto task_state = push_bid_notice( + rmf_task_msgs::build() + .request(task_request_json.dump()) + .task_id(task_id) + .time_window(bidding_time_window)); - if (queue_bidding_tasks.size() == 1) - auctioneer->start_bidding(queue_bidding_tasks.front()); + nlohmann::json response_json; + response_json["success"] = true; + response_json["state"] = task_state; - return submitted_task.task_id; + auto response = rmf_task_msgs::build() + .type(ApiResponseMsg::TYPE_RESPONDING) + .json_msg(response_json.dump()) + .request_id(msg.request_id); + + api_memory.add(response); + api_response->publish(response); + + // TODO(MXG): Make some way to keep pushing task state updates to the + // api-server as the bidding process progresses. We could do a websocket + // connection or maybe just a simple ROS2 publisher. + } + catch (const std::exception& e) + { + RCLCPP_ERROR( + node->get_logger(), + "Failed to handle API request message: %s", e.what()); + } } - bool cancel_task(const TaskID& task_id) + std::optional submit_task(const TaskDescription& submission) { - // check if key exists - const auto it = active_dispatch_tasks.find(task_id); - if (it == active_dispatch_tasks.end()) + const auto task_type_index = submission.task_type.type; + const auto desc_it = legacy_task_type_names.find(task_type_index); + if (desc_it == legacy_task_type_names.end()) { - RCLCPP_ERROR(node->get_logger(), - "Task [%s] is not found in active_tasks", task_id.c_str()); - return false; + RCLCPP_ERROR( + node->get_logger(), "TaskType: %u is invalid", task_type_index); + return std::nullopt; } - RCLCPP_WARN(node->get_logger(), "Cancel task: [%s]", task_id.c_str()); + const std::string category = desc_it->second; - // Cancel bidding. This will remove the bidding process - const auto& cancel_task_status = it->second; - if (cancel_task_status->state == TaskStatus::State::Pending) - { - cancel_task_status->state = TaskStatus::State::Canceled; - terminate_task(cancel_task_status); + // auto generate a task_id for a given submitted task + const auto task_id = + category + ".dispatch-" + std::to_string(task_counter++); - if (on_change_fn) - on_change_fn(cancel_task_status); + RCLCPP_INFO(node->get_logger(), + "Received Task Submission [%s]", task_id.c_str()); + + nlohmann::json task_request; + task_request["unix_millis_earliest_start_time"] = + std::chrono::duration_cast( + rmf_traffic_ros2::convert(submission.start_time).time_since_epoch()) + .count(); + + auto& priority = task_request["priority"]; + priority["type"] = "binary"; + priority["value"] = submission.priority.value; + task_request["category"] = category; + task_request["description"] = legacy_task_types.at(category)(submission); + task_request["labels"] = std::vector({"legacy_request"}); + + push_bid_notice( + rmf_task_msgs::build() + .request(task_request.dump()) + .task_id(task_id) + .time_window(bidding_time_window)); + + return task_id; + } - return true; + nlohmann::json push_bid_notice(bidding::BidNoticeMsg bid_notice) + { + nlohmann::json state; + auto& booking = state["booking"]; + booking["id"] = bid_notice.task_id; + + const auto request = nlohmann::json::parse(bid_notice.request); + static const std::vector copy_fields = { + "unix_millis_earliest_start_time", + "priority", + "labels" + }; + + for (const auto& field : copy_fields) + { + const auto f_it = request.find(field); + if (f_it != request.end()) + booking[field] = f_it.value(); } - // only user submitted task is cancelable - if (user_submitted_tasks.find(task_id) == user_submitted_tasks.end()) + state["category"] = request["category"]; + state["detail"] = request["description"]; + + auto& dispatch = state["dispatch"]; + dispatch["status"] = "queued"; + + // TODO(MXG): Publish this initial task state message to the websocket! + + auto new_dispatch_state = + std::make_shared( + bid_notice.task_id, std::chrono::steady_clock::now()); + + active_dispatch_states[bid_notice.task_id] = new_dispatch_state; + + if (on_change_fn) + on_change_fn(*new_dispatch_state); + + auctioneer->request_bid(bid_notice); + return state; + } + + bool cancel_task(const TaskID& task_id) + { + using Status = DispatchState::Status; + + // Check if the task has already terminated for some reason + const auto finished_it = finished_dispatch_states.find(task_id); + if (finished_it != finished_dispatch_states.end()) { - RCLCPP_ERROR(node->get_logger(), - "only user submitted task is cancelable"); + if (finished_it->second->status == Status::FailedToAssign + || finished_it->second->status == Status::CanceledInFlight) + { + // This task was never assigned to a fleet adapter so we will respond + // positively that it is cancelled. + return true; + } + + if (finished_it->second->status == Status::Dispatched) + { + // This task is now the responsibility of a fleet adapter + return false; + } + + // If the dispatch status is Queued or Selected then this task is in the + // wrong dispatch state set. + RCLCPP_ERROR( + node->get_logger(), + "Canceled task [%s] is in the set of finished dispatches but has an " + "invalid status for that set: %d", + task_id.c_str(), + static_cast(finished_it->second->status)); + return false; } - // Curently cancel can only work on Queued Task in Fleet Adapter - if (cancel_task_status->state != TaskStatus::State::Queued) + const auto it = active_dispatch_states.find(task_id); + if (it == active_dispatch_states.end()) { - RCLCPP_ERROR(node->get_logger(), - "Unable to cancel task [%s] as it is not a Queued Task", - task_id.c_str()); + // This must mean that some other system besides the dispatch system + // created the task ..? + RCLCPP_INFO(node->get_logger(), + "Canceled task [%s] was not dispatched", task_id.c_str()); return false; } - // Remove non-user submitted task from "active_dispatch_tasks" - // this is to prevent duplicated task during reassignation - // TODO: a better way to impl this - for (auto it = active_dispatch_tasks.begin(); - it != active_dispatch_tasks.end(); ) + // Cancel bidding. This will remove the bidding process + const auto& canceled_dispatch = it->second; + if (canceled_dispatch->status == DispatchState::Status::Queued) { - const bool is_fleet_name = - (cancel_task_status->fleet_name == it->second->fleet_name); - const bool is_self_gererated = - (user_submitted_tasks.find(it->first) == user_submitted_tasks.end()); + canceled_dispatch->status = DispatchState::Status::CanceledInFlight; + move_to_finished(task_id); - if (is_self_gererated && is_fleet_name) + if (on_change_fn) + on_change_fn(*canceled_dispatch); + + return true; + } + + // Cancel the assignment. We will need to make sure the cancelation is seen + // by the fleet adapter that the task was assigned to. + if (canceled_dispatch->status == DispatchState::Status::Selected) + { + canceled_dispatch->status = DispatchState::Status::CanceledInFlight; + if (!canceled_dispatch->assignment.has_value()) { - it->second->state = TaskStatus::State::Canceled; - terminate_task((it++)->second); + RCLCPP_ERROR( + node->get_logger(), + "Canceled task [%s] has Selected status but its assignment is empty. " + "This indicates a serious bug. Please report this to the RMF " + "developers.", + task_id.c_str()); } else - ++it; - } + { + auto cancel_command = rmf_task_msgs::build() + .fleet_name(canceled_dispatch->assignment->fleet_name) + .task_id(task_id) + .dispatch_id(next_dispatch_command_id++) + .timestamp(node->get_clock()->now()) + .type(DispatchCommandMsg::TYPE_REMOVE); + + lingering_commands[cancel_command.dispatch_id] = cancel_command; + dispatch_command_pub->publish(cancel_command); + } - // Cancel action task, this will only send a cancel to FA. up to - // the FA whether to cancel the task. On change is implemented - // internally in action client - return action_client->cancel_task(cancel_task_status->task_profile); - } + move_to_finished(task_id); - const std::optional get_task_state( - const TaskID& task_id) const - { - // check if taskid exists in active tasks - auto it = active_dispatch_tasks.find(task_id); - if (it != active_dispatch_tasks.end()) - return it->second->state; + if (on_change_fn) + on_change_fn(*canceled_dispatch); + + return true; + } - // check if taskid exists in terminated tasks - it = terminal_dispatch_tasks.find(task_id); - if (it != terminal_dispatch_tasks.end()) - return it->second->state; + RCLCPP_ERROR( + node->get_logger(), + "Canceled task [%s] is in the set of active dispatches but has an " + "invalid status for that set: %d", + task_id.c_str(), + static_cast(canceled_dispatch->status)); - return std::nullopt; + return false; } - void receive_bidding_winner_cb( + void conclude_bid( const TaskID& task_id, - const rmf_utils::optional winner) + const std::optional winner, + const std::vector& errors) { - const auto it = active_dispatch_tasks.find(task_id); - if (it == active_dispatch_tasks.end()) + const auto it = active_dispatch_states.find(task_id); + if (it == active_dispatch_states.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "Received a winning bid for a task request [%s] which is no longer " + "being dispatched. This may indicate a bug and should be reported to " + "the developers of RMF.", + task_id.c_str()); return; - const auto& pending_task_status = it->second; + } + + auto& dispatch_state = it->second; + for (const auto& error : errors) + { + try + { + dispatch_state->errors.push_back(nlohmann::json::parse(error)); + } + catch (const std::exception&) + { + // If the message cannot be converted directly into an error message, + // then we will create an error message for it + nlohmann::json error_msg; + error_msg["code"] = 255; + error_msg["category"] = "unknown"; + error_msg["detail"] = error; + + dispatch_state->errors.push_back(std::move(error_msg)); + } + } if (!winner) { - RCLCPP_WARN(node->get_logger(), "Dispatcher Bidding Result: task [%s]" - " has no submissions during bidding, Task Failed", task_id.c_str()); - pending_task_status->state = TaskStatus::State::Failed; - terminate_task(pending_task_status); + RCLCPP_WARN(node->get_logger(), + "Dispatcher Bidding Result: task [%s] has no submissions during " + "bidding. Dispatching failed, and the task will not be performed.", + task_id.c_str()); + + dispatch_state->status = DispatchState::Status::FailedToAssign; + nlohmann::json error; + // TODO(MXG): Standardize the codes + error["code"] = 10; + error["category"] = "rejection"; + error["detail"] = + "No fleet adapters offered a bid for task [" + task_id + "]"; + + dispatch_state->errors.push_back(std::move(error)); if (on_change_fn) - on_change_fn(pending_task_status); + on_change_fn(*dispatch_state); + + // Print the errors + std::size_t error_count = 1; + for (const auto& error : errors) + { + RCLCPP_ERROR( + node->get_logger(), + "No submission error[%d]: %s", + error_count, + error.c_str()); + ++error_count; + } - queue_bidding_tasks.pop(); - if (!queue_bidding_tasks.empty()) - auctioneer->start_bidding(queue_bidding_tasks.front()); return; } // now we know which fleet will execute the task - pending_task_status->fleet_name = winner->fleet_name; + dispatch_state->assignment = + DispatchState::Assignment{ + winner->fleet_name, + winner->expected_robot_name + }; + dispatch_state->status = DispatchState::Status::Selected; + + RCLCPP_INFO( + node->get_logger(), + "Dispatcher Bidding Result: task [%s] is awarded to fleet adapter [%s], " + "with expected robot [%s].", + task_id.c_str(), + winner->fleet_name.c_str(), + winner->expected_robot_name.c_str()); + + auto award_command = rmf_task_msgs::build() + .fleet_name(winner->fleet_name) + .task_id(task_id) + .dispatch_id(next_dispatch_command_id++) + .timestamp(node->get_clock()->now()) + .type(DispatchCommandMsg::TYPE_AWARD); + + lingering_commands[award_command.dispatch_id] = award_command; + dispatch_command_pub->publish(award_command); + } - RCLCPP_INFO(node->get_logger(), "Dispatcher Bidding Result: task [%s]" - " is accepted by fleet adapter [%s]", - task_id.c_str(), winner->fleet_name.c_str()); + void move_to_finished(const std::string& task_id) + { + const auto active_it = active_dispatch_states.find(task_id); - // Remove non-user submitted charging task from "active_dispatch_tasks" - // this is to prevent duplicated task during reassignation. - // TODO: a better way to impl this - for (auto it = active_dispatch_tasks.begin(); - it != active_dispatch_tasks.end(); ) + // TODO(MXG): We can make this more efficient by having a queue of + // finished dispatch state IDs in chronological order + if (finished_dispatch_states.size() >= terminated_tasks_max_size) { - const bool is_fleet_name = (winner->fleet_name == it->second->fleet_name); - const bool is_self_gererated = - (user_submitted_tasks.find(it->first) == user_submitted_tasks.end()); - - if (is_self_gererated && is_fleet_name) + auto check_it = finished_dispatch_states.begin(); + auto oldest_it = check_it++; + for (; check_it != finished_dispatch_states.end(); ++check_it) { - it->second->state = TaskStatus::State::Canceled; - terminate_task((it++)->second); + const auto& check = check_it->second->submission_time; + const auto& oldest = oldest_it->second->submission_time; + if (check < oldest) + oldest_it = check_it; } - else - ++it; + + finished_dispatch_states.erase(oldest_it); } - // add task to action server - action_client->add_task( - winner->fleet_name, - pending_task_status->task_profile, - pending_task_status); + finished_dispatch_states[active_it->first] = active_it->second; } - void terminate_task(const TaskStatusPtr terminate_status) + void publish_dispatch_states() { - assert(terminate_status->is_terminated()); - publish_ongoing_tasks(); + const auto fill_states = [](auto& into, const auto& from) + { + for (const auto& [id, state] : from) + into.push_back(convert(*state)); + }; + + std::vector active; + std::vector finished; + fill_states(active, active_dispatch_states); + fill_states(finished, finished_dispatch_states); + + dispatch_states_pub->publish( + rmf_task_msgs::build() + .active(std::move(active)) + .finished(std::move(finished))); + } - // prevent terminal_dispatch_tasks from piling up meaning - if (terminal_dispatch_tasks.size() >= terminated_tasks_max_size) - { - RCLCPP_WARN(node->get_logger(), - "Terminated tasks reached max size, remove earliest submited task"); + void publish_lingering_commands() + { + std::vector expired_commands; + const auto now = node->get_clock()->now(); + + // TODO(MXG): Make this timeout period configurable + const auto timeout = std::chrono::seconds(10); - auto rm_task = terminal_dispatch_tasks.begin(); - for (auto it = rm_task++; it != terminal_dispatch_tasks.end(); it++) + for (const auto& [id, r] : lingering_commands) + { + const auto timestamp = rclcpp::Time(r.timestamp); + if (timestamp + timeout < now) { - const auto t1 = it->second->task_profile.submission_time; - const auto t2 = rm_task->second->task_profile.submission_time; - if (rmf_traffic_ros2::convert(t1) < rmf_traffic_ros2::convert(t2)) - rm_task = it; + // This request has expired. + expired_commands.push_back(id); + continue; } - terminal_dispatch_tasks.erase(rm_task); + + dispatch_command_pub->publish(r); } - const auto id = terminate_status->task_profile.task_id; + for (const auto& id : expired_commands) + { + const auto it = lingering_commands.find(id); + if (it == lingering_commands.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "Weird bug, [%lu] is no longer in the lingering requests even though " + "it was just detected as being expired. Please report this to the " + "RMF developers.", id); + continue; + } - // destroy prev status ptr and recreate one - auto status = std::make_shared(*terminate_status); - (terminal_dispatch_tasks)[id] = status; - user_submitted_tasks.erase(id); - active_dispatch_tasks.erase(id); + const auto& request = it->second; + RCLCPP_ERROR( + node->get_logger(), + "Dispatch command [%lu] type [%u] for task [%s] directed at fleet [%s] " + "has expired. This likely means something is wrong with the fleet " + "adapter for [%s] preventing it from responding.", + id, + request.type, + request.task_id.c_str(), + request.fleet_name.c_str(), + request.fleet_name.c_str()); + + if (request.type == request.TYPE_AWARD) + auctioneer->ready_for_next_bid(); + + lingering_commands.erase(it); + } } - void task_status_cb(const TaskStatusPtr status) + void handle_dispatch_ack(const DispatchAckMsg& ack) { - // This is to solve the issue that the dispatcher is not aware of those - // "stray" tasks that are not dispatched by the dispatcher. This will add - // the stray tasks when an unknown TaskSummary is heard. - const std::string id = status->task_profile.task_id; - const auto it = active_dispatch_tasks.find(id); - if (it == active_dispatch_tasks.end()) + const auto command_it = lingering_commands.find(ack.dispatch_id); + if (command_it == lingering_commands.end()) { - active_dispatch_tasks[id] = status; - RCLCPP_WARN(node->get_logger(), - "Add previously unheard task: [%s]", id.c_str()); + // Already processed this acknowledgment + return; } - // check if there's a change in state for the previous completed bidding task - // TODO, better way to impl this - if (!queue_bidding_tasks.empty() - && id == queue_bidding_tasks.front().task_profile.task_id) + const auto command = std::move(command_it->second); + lingering_commands.erase(command_it); + + if (command.type == DispatchCommandMsg::TYPE_AWARD) { - queue_bidding_tasks.pop(); - if (!queue_bidding_tasks.empty()) - auctioneer->start_bidding(queue_bidding_tasks.front()); - } + const auto state_it = active_dispatch_states.find(command.task_id); + if (state_it == active_dispatch_states.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "Could not find active dispatch state for [%s] despite receiving an " + "acknowledgment for its bid award. This indicates a bug. Please " + "report this to the RMF developers.", + command.task_id.c_str()); + return; + } - if (on_change_fn) - on_change_fn(status); - } + const auto state = state_it->second; + if (state->status == DispatchState::Status::Selected) + { + state->status = DispatchState::Status::Dispatched; + move_to_finished(state->task_id); + } + else if (state->status == DispatchState::Status::CanceledInFlight) + { + // If the task was canceled in flight then we will simply ignore this + // acknowledgment. There should be another lingering command telling the + // fleet adapter to cancel the task. + } + else + { + RCLCPP_ERROR( + node->get_logger(), + "Dispatch status for [%s] is [%u], but we have received an award " + "acknowledgment for it. This indicates a bug. Please report this to " + "the RMF developers.", + state->task_id.c_str(), + static_cast(state->status)); + } - void publish_ongoing_tasks() - { - TasksMsg task_msgs; - for (auto task : (this->active_dispatch_tasks)) + auctioneer->ready_for_next_bid(); + return; + } + else if (command.type == DispatchCommandMsg::TYPE_REMOVE) { - task_msgs.tasks.push_back( - rmf_task_ros2::convert_status(*(task.second))); + // No further action is needed. We simply remove the lingering command. } - ongoing_tasks_pub->publish(task_msgs); + + RCLCPP_ERROR( + node->get_logger(), + "Unrecognized command type [%u] in lingering dispatch command queue. " + "This indicates a bug. Please report this to the RMF developers.", + command.type); } }; @@ -446,18 +947,14 @@ std::shared_ptr Dispatcher::make_node( std::shared_ptr Dispatcher::make( const std::shared_ptr& node) { - auto pimpl = rmf_utils::make_impl(node); - pimpl->action_client = action::Client::make(node); - auto dispatcher = std::shared_ptr(new Dispatcher()); - dispatcher->_pimpl = std::move(pimpl); - dispatcher->_pimpl->start(); + dispatcher->_pimpl = rmf_utils::make_impl(node); return dispatcher; } //============================================================================== std::optional Dispatcher::submit_task( - const TaskDescription& task_description) + const rmf_task_msgs::msg::TaskDescription& task_description) { return _pimpl->submit_task(task_description); } @@ -469,35 +966,42 @@ bool Dispatcher::cancel_task(const TaskID& task_id) } //============================================================================== -const std::optional Dispatcher::get_task_state( +std::optional Dispatcher::get_dispatch_state( const TaskID& task_id) const { - return _pimpl->get_task_state(task_id); + const auto active_it = _pimpl->active_dispatch_states.find(task_id); + if (active_it != _pimpl->active_dispatch_states.end()) + return *active_it->second; + + const auto finished_it = _pimpl->finished_dispatch_states.find(task_id); + if (finished_it != _pimpl->finished_dispatch_states.end()) + return *finished_it->second; + + return std::nullopt; } //============================================================================== -const Dispatcher::DispatchTasks& Dispatcher::active_tasks() const +const Dispatcher::DispatchStates& Dispatcher::active_dispatches() const { - return _pimpl->active_dispatch_tasks; + return _pimpl->active_dispatch_states; } //============================================================================== -const Dispatcher::DispatchTasks& Dispatcher::terminated_tasks() const +const Dispatcher::DispatchStates& Dispatcher::finished_dispatches() const { - return _pimpl->terminal_dispatch_tasks; + return _pimpl->finished_dispatch_states; } //============================================================================== -void Dispatcher::on_change(StatusCallback on_change_fn) +void Dispatcher::on_change(DispatchStateCallback on_change_fn) { _pimpl->on_change_fn = on_change_fn; } //============================================================================== -void Dispatcher::evaluator( - std::shared_ptr evaluator) +void Dispatcher::evaluator(bidding::Auctioneer::ConstEvaluatorPtr evaluator) { - _pimpl->auctioneer->select_evaluator(evaluator); + _pimpl->auctioneer->set_evaluator(std::move(evaluator)); } //============================================================================== diff --git a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp b/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp deleted file mode 100644 index cd06e3c94..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/TaskStatus.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright (C) 2020 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_task_ros2 { - -// ============================================================================== -bool TaskStatus::is_terminated() const -{ - return (state == State::Failed) || - (state == State::Completed) || - (state == State::Canceled); -} - -// ============================================================================== -TaskStatus convert_status(const StatusMsg& from) -{ - TaskStatus status; - status.fleet_name = from.fleet_name; - status.task_profile = from.task_profile; - status.start_time = rmf_traffic_ros2::convert(from.start_time); - status.end_time = rmf_traffic_ros2::convert(from.end_time); - status.robot_name = from.robot_name; - status.status = from.status; - status.state = static_cast(from.state); - return status; -} - -// ============================================================================== -StatusMsg convert_status(const TaskStatus& from) -{ - StatusMsg status; - status.fleet_name = from.fleet_name; - status.task_id = from.task_profile.task_id; // duplication - status.task_profile = from.task_profile; - status.start_time = rmf_traffic_ros2::convert(from.start_time); - status.end_time = rmf_traffic_ros2::convert(from.end_time); - status.robot_name = from.robot_name; - status.status = from.status; - status.state = static_cast(from.state); - return status; -} - -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp deleted file mode 100644 index 1c2069d2b..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.cpp +++ /dev/null @@ -1,236 +0,0 @@ -/* - * Copyright (C) 2020 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 "Client.hpp" - -namespace rmf_task_ros2 { -namespace action { - -std::shared_ptr -Client::make(std::shared_ptr node) -{ - return std::shared_ptr(new Client(node)); -} - -//============================================================================== -Client::Client(std::shared_ptr node) -: _node(node) -{ - const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); - - _request_msg_pub = _node->create_publisher( - TaskRequestTopicName, dispatch_qos); - - _status_msg_sub = _node->create_subscription( - TaskStatusTopicName, dispatch_qos, - [&](const std::unique_ptr msg) - { - const auto task_id = msg->task_profile.task_id; - if (task_id.empty()) - return; - - // status update, check if task_id is previously known - if (_active_task_status.count(task_id)) - { - auto weak_status = _active_task_status[task_id].lock(); - - if (!weak_status) - { - RCLCPP_INFO(_node->get_logger(), "Task was previously terminated"); - _active_task_status.erase(task_id); - return; - } - - // TODO: hack to retain task profile and fleet name (to remove) - auto cache_profile = weak_status->task_profile; - // update status to ptr - *weak_status = convert_status(*msg); - weak_status->task_profile = cache_profile; - - if (weak_status->is_terminated()) - RCLCPP_INFO(_node->get_logger(), - "Received status from fleet [%s], task [%s] is now terminated", - msg->fleet_name.c_str(), task_id.c_str()); - - update_task_status(weak_status); - } - else - { - // will still provide onchange even if the task_id is unknown. - RCLCPP_DEBUG(_node->get_logger(), - "[action] Unknown task: [%s]", task_id.c_str()); - auto task_status = std::make_shared(convert_status(*msg)); - _active_task_status[task_id] = task_status; - update_task_status(task_status); - } - }); - - _ack_msg_sub = _node->create_subscription( - TaskAckTopicName, dispatch_qos, - [&](const std::unique_ptr msg) - { - const auto task_id = msg->dispatch_request.task_profile.task_id; - const auto weak_status = _active_task_status[task_id].lock(); - - switch (msg->dispatch_request.method) - { - case RequestMsg::ADD: - if (msg->success) - { - // update this as pending - RCLCPP_INFO(_node->get_logger(), - "Received dispatch ack from fleet [%s] that task [%s] is queued", - msg->dispatch_request.fleet_name.c_str(), task_id.c_str()); - weak_status->state = TaskStatus::State::Queued; - } - else - { - // update this as failed - RCLCPP_ERROR(_node->get_logger(), - "Received dispatch ack from fleet [%s] that task [%s] Add Failed", - msg->dispatch_request.fleet_name.c_str(), task_id.c_str()); - weak_status->state = TaskStatus::State::Failed; - } - break; - case RequestMsg::CANCEL: - // update this as Canceled - if (msg->success) - { - RCLCPP_INFO(_node->get_logger(), - "Received dispatch ack from fleet [%s] that task [%s] is canceled", - msg->dispatch_request.fleet_name.c_str(), task_id.c_str()); - weak_status->state = TaskStatus::State::Canceled; - } - break; - default: - RCLCPP_ERROR(_node->get_logger(), "Invalid Dispatch ack method"); - return; - } - - update_task_status(weak_status); - }); -} - -//============================================================================== -void Client::update_task_status(const TaskStatusPtr status) -{ - // call on_change callback - if (_on_change_callback) - _on_change_callback(status); - - // erase terminated task and call on_terminate callback - if (status->is_terminated()) - { - _active_task_status.erase(status->task_profile.task_id); - if (_on_terminate_callback) - _on_terminate_callback(status); - } -} - -//============================================================================== -// check if task is updated TODO - -//============================================================================== -void Client::add_task( - const std::string& fleet_name, - const TaskProfile& task_profile, - TaskStatusPtr status_ptr) -{ - // send request and wait for acknowledgement - RequestMsg request_msg; - request_msg.fleet_name = fleet_name; - request_msg.task_profile = task_profile; - request_msg.method = RequestMsg::ADD; - _request_msg_pub->publish(request_msg); - - // save status ptr - status_ptr->fleet_name = fleet_name; - status_ptr->task_profile = task_profile; - _active_task_status[task_profile.task_id] = status_ptr; - RCLCPP_DEBUG(_node->get_logger(), "Assign task: [%s] to fleet [%s]", - task_profile.task_id.c_str(), fleet_name.c_str()); - return; -} - -//============================================================================== -bool Client::cancel_task( - const TaskProfile& task_profile) -{ - const auto task_id = task_profile.task_id; - RCLCPP_DEBUG(_node->get_logger(), - "[action] Cancel Task: [%s]", task_id.c_str()); - - // check if task is previously added - if (!_active_task_status.count(task_id)) - { - RCLCPP_WARN(_node->get_logger(), - "Canceling an unknown task [%s]", task_id.c_str()); - return false; - } - - auto weak_status = _active_task_status[task_id].lock(); - - if (!weak_status) - { - RCLCPP_WARN(_node->get_logger(), "Task was previously terminated"); - _active_task_status.erase(task_id); - return false; - } - - // send cancel - RequestMsg request_msg; - request_msg.fleet_name = weak_status->fleet_name; - request_msg.task_profile = task_profile; - request_msg.method = RequestMsg::CANCEL; - _request_msg_pub->publish(request_msg); - return true; -} - -//============================================================================== -int Client::size() -{ - for (auto it = _active_task_status.begin(); it != _active_task_status.end(); ) - { - if (auto weak_status = it->second.lock() ) - { - if (weak_status->is_terminated()) - it = _active_task_status.erase(it); - else - ++it; - } - else - it = _active_task_status.erase(it); - } - return _active_task_status.size(); -} - -//============================================================================== -void Client::on_change( - StatusCallback status_cb_fn) -{ - _on_change_callback = std::move(status_cb_fn); -} - -//============================================================================== -void Client::on_terminate( - StatusCallback status_cb_fn) -{ - _on_terminate_callback = std::move(status_cb_fn); -} - -} // namespace action -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp deleted file mode 100644 index d39f1fbba..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Client.hpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP -#define SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP - -#include - -#include -#include -#include -#include - -namespace rmf_task_ros2 { -namespace action { - -//============================================================================== -// Task Action Client -- responsible for initiating a rmf task to a target -// fleet. The fleet will work on the requested task and provides a status to -// the client when the task progresses. Termination will be triggered when the -// task ends. - -class Client -{ -public: - /// make an action client - /// - /// \param[in] node - /// ros2 node instance - static std::shared_ptr make( - std::shared_ptr node); - - /// Add a task to a targeted fleet - /// - /// \param[in] fleet_name - /// Target fleet which will execute this task - /// - /// \param[in] task_profile - /// Task Description which will be executed - /// - /// \param[out] status_ptr - /// Will update the status of the task here - void add_task( - const std::string& fleet_name, - const TaskProfile& task_profile, - TaskStatusPtr status_ptr); - - /// Cancel an added task - /// - /// \param[in] task_profile - /// Task which to cancel - /// - /// \return bool which indicate if cancel task is success - bool cancel_task(const TaskProfile& task_profile); - - /// Get the number of active task being track by client - /// - /// \return number of active task - int size(); - - /// Callback Function which will trigger during an event - /// - /// \param[in] status - /// Status of a task which the event is triggered - using StatusCallback = std::function; - - /// Callback when a task status has changed - /// - /// \param[in] status_cb_fn - /// Status callback function - void on_change(StatusCallback status_cb_fn); - - /// Callback when a task is terminated - /// - /// \param[in] status_cb_fn - /// Status callback function - void on_terminate(StatusCallback status_cb_fn); - -private: - Client(std::shared_ptr node); - - void update_task_status(const TaskStatusPtr status); - - using RequestMsg = rmf_task_msgs::msg::DispatchRequest; - using AckMsg = rmf_task_msgs::msg::DispatchAck; - - std::shared_ptr _node; - StatusCallback _on_change_callback; - StatusCallback _on_terminate_callback; - std::unordered_map> _active_task_status; - rclcpp::Publisher::SharedPtr _request_msg_pub; - rclcpp::Subscription::SharedPtr _status_msg_sub; - rclcpp::Subscription::SharedPtr _ack_msg_sub; -}; - -} // namespace action -} // namespace rmf_task_ros2 - -#endif // SRC__RMF_TASK_ROS2__ACTION__CLIENT_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp deleted file mode 100644 index 87130837a..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright (C) 2020 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 "Server.hpp" - -namespace rmf_task_ros2 { -namespace action { - -std::shared_ptr Server::make( - std::shared_ptr node, - const std::string& fleet_name) -{ - return std::shared_ptr(new Server(node, fleet_name)); -} - -//============================================================================== -void Server::register_callbacks( - AddTaskCallback add_task_cb_fn, - CancelTaskCallback cancel_task_cb_fn) -{ - _add_task_cb_fn = std::move(add_task_cb_fn); - _cancel_task_cb_fn = std::move(cancel_task_cb_fn); -} - -//============================================================================== -void Server::update_status( - const TaskStatus& task_status) -{ - auto msg = convert_status(task_status); - msg.fleet_name = _fleet_name; - _status_msg_pub->publish(msg); -} - -//============================================================================== -Server::Server( - std::shared_ptr node, - const std::string& fleet_name) -: _node(node), _fleet_name(fleet_name) -{ - const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); - - _request_msg_sub = _node->create_subscription( - TaskRequestTopicName, dispatch_qos, - [&](const std::unique_ptr msg) - { - if (msg->fleet_name != _fleet_name) - return;// not me - - RCLCPP_INFO(_node->get_logger(), - "[Action server] Received task request!"); - AckMsg ack_msg; - ack_msg.dispatch_request = *msg; - ack_msg.success = false; - - switch (msg->method) - { - // Add Request - case RequestMsg::ADD: - { - if (_add_task_cb_fn && _add_task_cb_fn(msg->task_profile)) - ack_msg.success = true; - break; - } - // Cancel Request - case RequestMsg::CANCEL: - { - if (_cancel_task_cb_fn && _cancel_task_cb_fn(msg->task_profile)) - ack_msg.success = true; - break; - } - default: - RCLCPP_ERROR(_node->get_logger(), "Request Method is not supported!"); - } - _ack_msg_pub->publish(ack_msg); - }); - - _ack_msg_pub = _node->create_publisher( - TaskAckTopicName, dispatch_qos); - - _status_msg_pub = _node->create_publisher( - TaskStatusTopicName, dispatch_qos); -} - -} // namespace action -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp b/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp deleted file mode 100644 index d8c0150a6..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/action/Server.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * Copyright (C) 2020 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP -#define SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP - -#include - -#include -#include -#include -#include -#include - -using TaskProfile = rmf_task_msgs::msg::TaskProfile; - -namespace rmf_task_ros2 { -namespace action { - -//============================================================================== -/// Task Action Server - This is used within the fleet adapter with the role of -/// receiving incoming dispatch requests (from a action_client/Dispatcher), -/// then execute the task accordingly. -class Server -{ -public: - /// initialize action server - /// - /// \param[in] node - /// ros2 node instance - /// - /// \param[in] fleet_name - /// action server id (e.g. fleet adapter name) - static std::shared_ptr make( - std::shared_ptr node, - const std::string& fleet_name); - - using AddTaskCallback = std::function; - using CancelTaskCallback = std::function; - - /// Add event callback functions. These functions will be triggered when a - /// relevant task requests msg is received. - /// - /// \param[in] add_task_cb - /// When a new task request is called. - /// - /// \param[in] cancel_task_cb - /// when a cancel task request is called - void register_callbacks( - AddTaskCallback add_task_cb, - CancelTaskCallback cancel_task_cb); - - /// Use this to send a status update to action client - /// A On Change update is recommended to inform the task progress - /// - /// \param[in] task_status - /// latest status of the task - void update_status(const TaskStatus& task_status); - -private: - Server( - std::shared_ptr node, - const std::string& fleet_name); - - using RequestMsg = rmf_task_msgs::msg::DispatchRequest; - using AckMsg = rmf_task_msgs::msg::DispatchAck; - - std::shared_ptr _node; - std::string _fleet_name; - AddTaskCallback _add_task_cb_fn; - CancelTaskCallback _cancel_task_cb_fn; - rclcpp::Subscription::SharedPtr _request_msg_sub; - rclcpp::Publisher::SharedPtr _status_msg_pub; - rclcpp::Publisher::SharedPtr _ack_msg_pub; -}; - -} // namespace action -} // namespace rmf_task_ros2 - -#endif // SRC__RMF_TASK_ROS2__ACTION__SERVER_HPP diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp new file mode 100644 index 000000000..c2fa0a535 --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/AsyncBidder.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2020 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 + +namespace rmf_task_ros2 { +namespace bidding { + +//============================================================================== +class AsyncBidder::Implementation +{ +public: + + std::weak_ptr w_node; + ReceiveNotice receive_notice; + + using BidNoticeSub = rclcpp::Subscription; + BidNoticeSub::SharedPtr bid_notice_sub; + + using BidResponsePub = rclcpp::Publisher; + BidResponsePub::SharedPtr bid_response_pub; + + Implementation( + std::shared_ptr node_, + ReceiveNotice receive_notice) + : w_node{std::move(node_)}, + receive_notice{std::move(receive_notice)} + { + const auto bid_qos = rclcpp::ServicesQoS().reliable(); + const auto node = w_node.lock(); + + bid_notice_sub = node->create_subscription( + rmf_task_ros2::BidNoticeTopicName, bid_qos, + [&](const BidNoticeMsg::UniquePtr msg) + { + this->handle_notice(*msg); + }); + + bid_response_pub = node->create_publisher( + rmf_task_ros2::BidResponseTopicName, bid_qos); + } + + // Callback fn when a dispatch notice is received + void handle_notice(const BidNoticeMsg& msg) + { + const auto node = w_node.lock(); + if (!node) + return; + + RCLCPP_INFO(node->get_logger(), + "[Bidder] Received Bidding notice for task_id [%s]", + msg.task_id.c_str()); + + // check if the user did not supply a receive notice callback + if (!receive_notice) + return; + + // Send the notice + receive_notice( + msg, + [task_id = msg.task_id, pub = bid_response_pub]( + const Response& response) + { + pub->publish(convert(response, task_id)); + }); + } +}; + +//============================================================================== +std::shared_ptr AsyncBidder::make( + const std::shared_ptr& node, + ReceiveNotice receive_notice) +{ + auto bidder = std::shared_ptr(new AsyncBidder()); + bidder->_pimpl = + rmf_utils::make_unique_impl(node, receive_notice); + + return bidder; +} + +//============================================================================== +AsyncBidder::AsyncBidder() +{ + // do nothing +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp index 98bad0b79..21f7860ce 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Auctioneer.cpp @@ -20,146 +20,157 @@ namespace rmf_task_ros2 { namespace bidding { -//============================================================================== -Submission convert(const BidProposal& from) -{ - Submission submission; - submission.fleet_name = from.fleet_name; - submission.robot_name = from.robot_name; - submission.prev_cost = from.prev_cost; - submission.new_cost = from.new_cost; - submission.finish_time = rmf_traffic_ros2::convert(from.finish_time); - return submission; -} - //============================================================================== Auctioneer::Implementation::Implementation( const std::shared_ptr& node_, - BiddingResultCallback result_callback) + BiddingResultCallback result_callback, + ConstEvaluatorPtr evaluator_) : node{std::move(node_)}, - bidding_result_callback{std::move(result_callback)} + bidding_result_callback{std::move(result_callback)}, + evaluator(std::move(evaluator_)) { // default evaluator - evaluator = std::make_shared(); - RCLCPP_INFO( - node->get_logger(), - "Dispatcher evaluator set to QuickestFinishEvaluator"); + if (!evaluator) + { + evaluator = std::make_shared(); + RCLCPP_INFO( + node->get_logger(), + "Dispatcher evaluator set to QuickestFinishEvaluator by default"); + } const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); - bid_notice_pub = node->create_publisher( + bid_notice_pub = node->create_publisher( rmf_task_ros2::BidNoticeTopicName, dispatch_qos); - bid_proposal_sub = node->create_subscription( - rmf_task_ros2::BidProposalTopicName, dispatch_qos, - [&](const BidProposal::UniquePtr msg) + bid_proposal_sub = node->create_subscription( + rmf_task_ros2::BidResponseTopicName, dispatch_qos, + [&](const BidResponseMsg::UniquePtr msg) { - this->receive_proposal(*msg); + this->receive_response(*msg); }); timer = node->create_wall_timer(std::chrono::milliseconds(200), [&]() { - this->check_bidding_process(); + this->finish_bidding_process(); }); } //============================================================================== -void Auctioneer::Implementation::start_bidding( - const BidNotice& bid_notice) +void Auctioneer::Implementation::request_bid( + const BidNoticeMsg& bid_notice) { RCLCPP_INFO(node->get_logger(), "Add Task [%s] to a bidding queue", - bid_notice.task_profile.task_id.c_str()); + bid_notice.task_id.c_str()); - BiddingTask bidding_task; - bidding_task.bid_notice = bid_notice; - bidding_task.start_time = node->now(); - queue_bidding_tasks.push(bidding_task); + open_bid_queue.push(OpenBid{bid_notice, node->now(), {}}); } //============================================================================== -void Auctioneer::Implementation::receive_proposal( - const BidProposal& msg) +void Auctioneer::Implementation::receive_response(const BidResponseMsg& msg) { - const auto id = msg.task_profile.task_id; - RCLCPP_DEBUG(node->get_logger(), - "[Auctioneer] Receive proposal from task_id: %s | from: %s", - id.c_str(), msg.fleet_name.c_str()); + const auto id = msg.task_id; + + const auto response = convert(msg); + if (response.proposal.has_value()) + { + RCLCPP_DEBUG(node->get_logger(), + "[Auctioneer] Receive proposal from task_id: %s | from: %s", + id.c_str(), response.proposal->fleet_name.c_str()); + } + else if (!response.errors.empty()) + { + RCLCPP_DEBUG( + node->get_logger(), + "[Auctioneer] Received %lu errors from a bidder", + response.errors.size()); + } // check if bidding task is initiated by the auctioneer previously // add submited proposal to the current bidding tasks list - if (queue_bidding_tasks.front().bid_notice.task_profile.task_id == id) - queue_bidding_tasks.front().submissions.push_back(convert(msg)); + if (open_bid_queue.front().bid_notice.task_id == id) + open_bid_queue.front().responses.push_back(response); } //============================================================================== // determine the winner within a bidding task instance -void Auctioneer::Implementation::check_bidding_process() +void Auctioneer::Implementation::finish_bidding_process() { - if (queue_bidding_tasks.size() == 0) + if (open_bid_queue.size() == 0) return; // Executing the task at the front queue - auto front_task = queue_bidding_tasks.front(); + auto front_task = open_bid_queue.front(); - if (bidding_in_proccess) + if (bidding_in_process) { if (determine_winner(front_task)) { - queue_bidding_tasks.pop(); - bidding_in_proccess = false; + open_bid_queue.pop(); } } else { - RCLCPP_DEBUG(node->get_logger(), " - Start new bidding task: %s", - front_task.bid_notice.task_profile.task_id.c_str()); - queue_bidding_tasks.front().start_time = node->now(); + RCLCPP_INFO(node->get_logger(), " - Start new bidding task: %s", + front_task.bid_notice.task_id.c_str()); + open_bid_queue.front().start_time = node->now(); bid_notice_pub->publish(front_task.bid_notice); - bidding_in_proccess = true; + bidding_in_process = true; } } //============================================================================== bool Auctioneer::Implementation::determine_winner( - const BiddingTask& bidding_task) + const OpenBid& bidding_task) { const auto duration = node->now() - bidding_task.start_time; + if (duration < bidding_task.bid_notice.time_window) + return false; - if (duration > bidding_task.bid_notice.time_window) - { - auto id = bidding_task.bid_notice.task_profile.task_id; - RCLCPP_DEBUG(node->get_logger(), "Bidding Deadline reached: %s", - id.c_str()); - std::optional winner = std::nullopt; + if (!bidding_result_callback) + return true; - if (bidding_task.submissions.size() == 0) - { - RCLCPP_DEBUG(node->get_logger(), - "Bidding task has not received any bids"); - } - else - { - winner = evaluate(bidding_task.submissions); - RCLCPP_INFO( - node->get_logger(), - "Determined winning Fleet Adapter: [%s], from %ld submissions", - winner->fleet_name.c_str(), - bidding_task.submissions.size()); - } + auto task_id = bidding_task.bid_notice.task_id; + RCLCPP_DEBUG( + node->get_logger(), + "Bidding Deadline reached: %s", + task_id.c_str()); + + std::vector errors; + for (const auto& r : bidding_task.responses) + { + errors.insert(errors.end(), r.errors.begin(), r.errors.end()); + } - // Call the user defined callback function - if (bidding_result_callback) - bidding_result_callback(id, winner); + if (bidding_task.responses.empty()) + { + RCLCPP_DEBUG(node->get_logger(), + "Bidding task has not received any bids"); + bidding_result_callback(task_id, std::nullopt, errors); return true; } - return false; + + auto winner = evaluate(bidding_task.responses); + if (winner.has_value()) + { + RCLCPP_INFO( + node->get_logger(), + "Determined winning Fleet Adapter: [%s], from %ld responses", + winner->fleet_name.c_str(), + bidding_task.responses.size()); + } + + // Call the user defined callback function + bidding_result_callback(task_id, winner, errors); + + return true; } //============================================================================== -std::optional Auctioneer::Implementation::evaluate( - const Submissions& submissions) +std::optional Auctioneer::Implementation::evaluate( + const Responses& responses) { - if (submissions.size() == 0) + if (responses.size() == 0) return std::nullopt; if (!evaluator) @@ -168,35 +179,43 @@ std::optional Auctioneer::Implementation::evaluate( return std::nullopt; } - const std::size_t choice = evaluator->choose(submissions); + const auto choice = evaluator->choose(responses); + if (!choice.has_value()) + return std::nullopt; - if (choice >= submissions.size()) + if (*choice >= responses.size()) return std::nullopt; - return submissions[choice]; + return responses[*choice].proposal; } //============================================================================== std::shared_ptr Auctioneer::make( const std::shared_ptr& node, - BiddingResultCallback result_callback) + BiddingResultCallback result_callback, + ConstEvaluatorPtr evaluator) { - auto pimpl = rmf_utils::make_unique_impl( - node, result_callback); auto auctioneer = std::shared_ptr(new Auctioneer()); - auctioneer->_pimpl = std::move(pimpl); + auctioneer->_pimpl = rmf_utils::make_unique_impl( + node, std::move(result_callback), std::move(evaluator)); + return auctioneer; } //============================================================================== -void Auctioneer::start_bidding(const BidNotice& bid_notice) +void Auctioneer::request_bid(const BidNoticeMsg& bid_notice) +{ + _pimpl->request_bid(bid_notice); +} + +//============================================================================== +void Auctioneer::ready_for_next_bid() { - _pimpl->start_bidding(bid_notice); + _pimpl->bidding_in_process = false; } //============================================================================== -void Auctioneer::select_evaluator( - std::shared_ptr evaluator) +void Auctioneer::set_evaluator(ConstEvaluatorPtr evaluator) { _pimpl->evaluator = std::move(evaluator); } @@ -207,51 +226,60 @@ Auctioneer::Auctioneer() // do nothing } +namespace { //============================================================================== -std::size_t LeastFleetDiffCostEvaluator::choose( - const Submissions& submissions) const +std::optional select_best( + const Responses& responses, + const std::function eval) { - auto winner_it = submissions.begin(); - float winner_cost_diff = winner_it->new_cost - winner_it->prev_cost; - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) + std::optional best_index; + std::optional best_cost; + for (std::size_t i=0; i < responses.size(); ++i) { - float nominee_cost_diff = nominee_it->new_cost - nominee_it->prev_cost; - if (nominee_cost_diff < winner_cost_diff) + if (responses[i].proposal.has_value()) { - winner_it = nominee_it; - winner_cost_diff = nominee_cost_diff; + const auto cost = eval(*responses[i].proposal); + if (!best_cost.has_value() || cost < *best_cost) + { + best_index = i; + best_cost = cost; + } } } - return std::distance(submissions.begin(), winner_it); + + return best_index; } +} // anonymous namespace //============================================================================== -std::size_t LeastFleetCostEvaluator::choose( - const Submissions& submissions) const +std::optional LeastFleetDiffCostEvaluator::choose( + const Responses& responses) const { - auto winner_it = submissions.begin(); - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) - { - if (nominee_it->new_cost < winner_it->new_cost) - winner_it = nominee_it; - } - return std::distance(submissions.begin(), winner_it); + return select_best( + responses, + [](const auto& nominee) { return nominee.new_cost - nominee.prev_cost; }); } //============================================================================== -std::size_t QuickestFinishEvaluator::choose( - const Submissions& submissions) const +std::optional LeastFleetCostEvaluator::choose( + const Responses& responses) const { - auto winner_it = submissions.begin(); - for (auto nominee_it = ++submissions.begin(); - nominee_it != submissions.end(); ++nominee_it) - { - if (nominee_it->finish_time < winner_it->finish_time) - winner_it = nominee_it; - } - return std::distance(submissions.begin(), winner_it); + return select_best( + responses, + [](const auto& nominee) { return nominee.new_cost; }); +} + +//============================================================================== +std::optional QuickestFinishEvaluator::choose( + const Responses& responses) const +{ + return select_best( + responses, + [](const auto& nominee) + { + return rmf_traffic::time::to_seconds( + nominee.finish_time.time_since_epoch()); + }); } } // namespace bidding diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp deleted file mode 100644 index 524097a4a..000000000 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/MinimalBidder.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* - * Copyright (C) 2020 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 - -namespace rmf_task_ros2 { -namespace bidding { - -using BidProposal = rmf_task_msgs::msg::BidProposal; - -//============================================================================== -BidProposal convert(const Submission& from) -{ - BidProposal proposal_msg; - proposal_msg.fleet_name = from.fleet_name; - proposal_msg.robot_name = from.robot_name; - proposal_msg.prev_cost = from.prev_cost; - proposal_msg.new_cost = from.new_cost; - proposal_msg.finish_time = rmf_traffic_ros2::convert(from.finish_time); - return proposal_msg; -} - -//============================================================================== -class MinimalBidder::Implementation -{ -public: - - std::shared_ptr node; - std::string fleet_name; - std::unordered_set valid_task_types; - ParseSubmissionCallback get_submission_fn; - - using BidNoticeSub = rclcpp::Subscription; - BidNoticeSub::SharedPtr dispatch_notice_sub; - - using BidProposalPub = rclcpp::Publisher; - BidProposalPub::SharedPtr dispatch_proposal_pub; - - Implementation( - std::shared_ptr node_, - const std::string& fleet_name_, - const std::unordered_set& valid_task_types_, - ParseSubmissionCallback submission_cb) - : node{std::move(node_)}, - fleet_name{std::move(fleet_name_)}, - valid_task_types{std::move(valid_task_types_)}, - get_submission_fn{std::move(submission_cb)} - { - const auto dispatch_qos = rclcpp::ServicesQoS().reliable(); - - dispatch_notice_sub = node->create_subscription( - rmf_task_ros2::BidNoticeTopicName, dispatch_qos, - [&](const BidNotice::UniquePtr msg) - { - this->receive_notice(*msg); - }); - - dispatch_proposal_pub = node->create_publisher( - rmf_task_ros2::BidProposalTopicName, dispatch_qos); - } - - // Callback fn when a dispatch notice is received - void receive_notice(const BidNotice& msg) - { - RCLCPP_INFO(node->get_logger(), - "[Bidder] Received Bidding notice for task_id [%s]", - msg.task_profile.task_id.c_str()); - - const auto task_type = (msg.task_profile.description.task_type.type); - - // check if task type is valid - if (!valid_task_types.count(static_cast(task_type))) - { - RCLCPP_WARN(node->get_logger(), "[%s]: task type %d is not supported", - fleet_name.c_str(), task_type); - return; - } - - // check if get submission function is declared - if (!get_submission_fn) - return; - - // Submit proposal - const auto bid_submission = get_submission_fn(msg); - auto best_proposal = convert(bid_submission); - best_proposal.fleet_name = fleet_name; - best_proposal.task_profile = msg.task_profile; - dispatch_proposal_pub->publish(best_proposal); - } -}; - -//============================================================================== -std::shared_ptr MinimalBidder::make( - const std::shared_ptr& node, - const std::string& fleet_name, - const std::unordered_set& valid_task_types, - ParseSubmissionCallback submission_cb) -{ - auto pimpl = rmf_utils::make_unique_impl( - node, fleet_name, valid_task_types, submission_cb); - auto bidder = std::shared_ptr(new MinimalBidder()); - bidder->_pimpl = std::move(pimpl); - return bidder; -} - -//============================================================================== -MinimalBidder::MinimalBidder() -{ - // do nothing -} - -} // namespace bidding -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp new file mode 100644 index 000000000..d0bde5cec --- /dev/null +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/Response.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2022 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_task_ros2 { +namespace bidding { + +//============================================================================== +BidProposalMsg convert(const Response::Proposal& proposal) +{ + return rmf_task_msgs::build() + .fleet_name(proposal.fleet_name) + .expected_robot_name(proposal.expected_robot_name) + .prev_cost(proposal.prev_cost) + .new_cost(proposal.new_cost) + .finish_time(rmf_traffic_ros2::convert(proposal.finish_time)); +} + +//============================================================================== +BidResponseMsg convert( + const Response& response, + const std::string& task_id) +{ + if (response.proposal.has_value()) + { + return rmf_task_msgs::build() + .task_id(task_id) + .has_proposal(true) + .proposal(convert(*response.proposal)) + .errors(response.errors); + } + + return rmf_task_msgs::build() + .task_id(task_id) + .has_proposal(false) + .proposal(rmf_task_msgs::msg::BidProposal()) + .errors(response.errors); +} + +//============================================================================== +Response convert(const BidResponseMsg& msg) +{ + if (msg.has_proposal) + { + return Response{ + Response::Proposal{ + msg.proposal.fleet_name, + msg.proposal.expected_robot_name, + msg.proposal.prev_cost, + msg.proposal.new_cost, + rmf_traffic_ros2::convert(msg.proposal.finish_time) + }, + msg.errors + }; + } + + return Response{ + std::nullopt, + msg.errors + }; +} + +} // namespace bidding +} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp index 57cf1a0eb..a30848f6d 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp +++ b/rmf_task_ros2/src/rmf_task_ros2/bidding/internal_Auctioneer.hpp @@ -19,7 +19,7 @@ #ifndef SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP #define SRC__RMF_TASK_ROS2__BIDDING__INTERNAL_AUCTIONEER_HPP -#include +#include #include #include @@ -29,49 +29,49 @@ namespace rmf_task_ros2 { namespace bidding { -using BidProposal = rmf_task_msgs::msg::BidProposal; - //============================================================================== class Auctioneer::Implementation { public: + std::shared_ptr node; rclcpp::TimerBase::SharedPtr timer; BiddingResultCallback bidding_result_callback; - std::shared_ptr evaluator; + ConstEvaluatorPtr evaluator; - struct BiddingTask + struct OpenBid { - BidNotice bid_notice; + BidNoticeMsg bid_notice; builtin_interfaces::msg::Time start_time; - std::vector submissions; + std::vector responses; }; - bool bidding_in_proccess = false; - std::queue queue_bidding_tasks; + bool bidding_in_process = false; + std::queue open_bid_queue; - using BidNoticePub = rclcpp::Publisher; + using BidNoticePub = rclcpp::Publisher; BidNoticePub::SharedPtr bid_notice_pub; - using BidProposalSub = rclcpp::Subscription; - BidProposalSub::SharedPtr bid_proposal_sub; + using BidResponseSub = rclcpp::Subscription; + BidResponseSub::SharedPtr bid_proposal_sub; Implementation( const std::shared_ptr& node_, - BiddingResultCallback result_callback); + BiddingResultCallback result_callback, + ConstEvaluatorPtr evaluator); /// Start a bidding process - void start_bidding(const BidNotice& bid_notice); + void request_bid(const BidNoticeMsg& bid_notice); // Receive proposal and evaluate - void receive_proposal(const BidProposal& msg); + void receive_response(const BidResponseMsg& msg); // determine the winner within a bidding task instance - void check_bidding_process(); + void finish_bidding_process(); - bool determine_winner(const BiddingTask& bidding_task); + bool determine_winner(const OpenBid& bidding_task); - std::optional evaluate(const Submissions& submissions); + std::optional evaluate(const Responses& responses); static const Implementation& get(const Auctioneer& auctioneer) { @@ -80,12 +80,12 @@ class Auctioneer::Implementation }; //============================================================================== -std::optional evaluate( +std::optional evaluate( const Auctioneer& auctioneer, - const Submissions& submissions) + const Responses& responses) { auto fimpl = Auctioneer::Implementation::get(auctioneer); - return fimpl.evaluate(submissions); + return fimpl.evaluate(responses); } } // namespace bidding diff --git a/rmf_task_ros2/test/action/test_ActionInterface.cpp b/rmf_task_ros2/test/action/test_ActionInterface.cpp deleted file mode 100644 index 297d40aff..000000000 --- a/rmf_task_ros2/test/action/test_ActionInterface.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/* - * Copyright (C) 2020 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 "../../src/rmf_task_ros2/action/Client.hpp" -#include "../../src/rmf_task_ros2/action/Server.hpp" -#include - -#include -#include - -namespace rmf_task_ros2 { -namespace action { - -//============================================================================== -SCENARIO("Action communication with client and server", "[ActionInterface]") -{ - const auto rcl_context = std::make_shared(); - rcl_context->init(0, nullptr); - - TaskProfile task_profile1; - task_profile1.task_id = "task1"; - task_profile1.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; - - TaskProfile task_profile2; - task_profile2.task_id = "task2"; - task_profile2.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; - - //============================================================================ - - // received task to test - // NOTE(MXG): We do not appear to be placing any test expectations on - // the value of test_add_task. That might mean we're not testing as thoroughly - // as we could. - const auto test_add_task = std::make_shared>(); - const auto test_cancel_task = std::make_shared>(); - - // Creating 1 auctioneer and 1 bidder - auto node = rclcpp::Node::make_shared( - "test_ActionInferface", - rclcpp::NodeOptions().context(rcl_context)); - - auto action_server = Server::make(node, "test_server"); - auto action_client = Client::make(node); - - rclcpp::ExecutorOptions exec_options; - exec_options.context = rcl_context; - rclcpp::executors::SingleThreadedExecutor executor(exec_options); - executor.add_node(node); - - // received test request msg from client - action_server->register_callbacks( - // add task - [test_add_task](const TaskProfile& task_profile) - { - *test_add_task = task_profile; - return true; - }, - // cancel task - [test_cancel_task](const TaskProfile& task_profile) - { - *test_cancel_task = task_profile; - return true; - } - ); - - // ROS Spin: forever incompleted future - std::promise ready_promise; - std::shared_future ready_future(ready_promise.get_future()); - - WHEN("Add Task") - { - // Add invalid Task! - TaskStatusPtr status_ptr(new TaskStatus); - - action_client->add_task("wrong_server", task_profile1, status_ptr); - - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - // should not receive cuz incorrect serverid - REQUIRE(status_ptr->state == TaskStatus::State::Pending); - - action_client->add_task("test_server", task_profile1, status_ptr); - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - // check status - REQUIRE(status_ptr->state == TaskStatus::State::Queued); - - // status ptr is destroyed, should not have anymore tracking - status_ptr.reset(); - REQUIRE(action_client->size() == 0); - } - - WHEN("Cancel Task") - { - // send valid task - TaskStatusPtr status_ptr(new TaskStatus); - action_client->add_task("test_server", task_profile2, status_ptr); - - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - REQUIRE(test_cancel_task.get()); - - // Invalid Cancel Task! - bool cancel_success = action_client->cancel_task(task_profile1); - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - REQUIRE(!test_cancel_task->has_value()); - REQUIRE(cancel_success == false); // cancel failed - REQUIRE(action_client->size() == 1); - - // Valid Cancel task - action_client->cancel_task(task_profile2); - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - REQUIRE(test_cancel_task->has_value()); - REQUIRE(*test_cancel_task == task_profile2); - REQUIRE(status_ptr->is_terminated()); - REQUIRE(action_client->size() == 0); - } - - //============================================================================ - - const auto test_task_onchange = std::make_shared>(); - const auto test_task_onterminate = - std::make_shared>(); - - // received status update from server - action_client->on_change( - [test_task_onchange](const TaskStatusPtr status) - { - *test_task_onchange = *status; - } - ); - action_client->on_terminate( - [test_task_onterminate](const TaskStatusPtr status) - { - *test_task_onterminate = *status; - } - ); - - WHEN("On Change and On Terminate Task") - { - TaskStatusPtr status_ptr(new TaskStatus); - action_client->add_task("test_server", task_profile1, status_ptr); - - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - REQUIRE(test_task_onchange.get()); - REQUIRE(test_task_onchange->has_value()); - CHECK((*test_task_onchange)->state == TaskStatus::State::Queued); - - TaskStatus server_task; - server_task.task_profile = task_profile1; - server_task.state = TaskStatus::State::Executing; - - // Update it as executing - action_server->update_status(server_task); - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(1.5)); - - CHECK((*test_task_onchange)->state == TaskStatus::State::Executing); - CHECK(!test_task_onterminate->has_value()); - - // completion - server_task.state = TaskStatus::State::Completed; - // Update it as executing - action_server->update_status(server_task); - executor.spin_until_future_complete(ready_future, - rmf_traffic::time::from_seconds(0.5)); - - REQUIRE(test_task_onterminate.get()); - REQUIRE(test_task_onterminate->has_value()); - CHECK((*test_task_onterminate)->state == TaskStatus::State::Completed); - } - - rclcpp::shutdown(rcl_context); -} - -} // namespace action -} // namespace rmf_task_ros2 diff --git a/rmf_task_ros2/test/bidding/test_Evaluator.cpp b/rmf_task_ros2/test/bidding/test_Evaluator.cpp index 3da0b9ad6..f3615ee6b 100644 --- a/rmf_task_ros2/test/bidding/test_Evaluator.cpp +++ b/rmf_task_ros2/test/bidding/test_Evaluator.cpp @@ -25,19 +25,19 @@ namespace bidding { //============================================================================== auto now = std::chrono::steady_clock::now(); -Submission submission1{ +Response::Proposal submission1{ "fleet1", "", 2.3, 3.4, rmf_traffic::time::apply_offset(now, 5) }; -Submission submission2{ +Response::Proposal submission2{ "fleet2", "", 3.5, 3.6, rmf_traffic::time::apply_offset(now, 5.5) }; -Submission submission3{ +Response::Proposal submission3{ "fleet3", "", 0.0, 1.4, rmf_traffic::time::apply_offset(now, 3) }; -Submission submission4{ +Response::Proposal submission4{ "fleet4", "", 5.0, 5.4, rmf_traffic::time::apply_offset(now, 4) }; -Submission submission5{ +Response::Proposal submission5{ "fleet5", "", 0.5, 0.8, rmf_traffic::time::apply_offset(now, 3.5) }; @@ -50,24 +50,29 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]") "test_selfbidding", rclcpp::NodeOptions().context(rcl_context)); auto auctioneer = Auctioneer::make(node, - [](const std::string&, const std::optional) {}); + [](const auto&, const auto, const auto&) {}, + nullptr); WHEN("Least Diff Cost Evaluator") { auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); + auctioneer->set_evaluator(eval); AND_WHEN("0 submissions") { - std::vector submissions{}; - auto winner = evaluate(*auctioneer, submissions); + std::vector responses{}; + auto winner = evaluate(*auctioneer, responses); REQUIRE(!winner); // no winner } AND_WHEN("5 submissions") { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; - auto winner = evaluate(*auctioneer, submissions); + std::vector responses{ + Response{submission1, {}}, + Response{submission2, {}}, + Response{submission3, {}}, + Response{submission4, {}}, + Response{submission5, {}} }; + auto winner = evaluate(*auctioneer, responses); REQUIRE(winner->fleet_name == "fleet2"); // least diff cost agent } } @@ -75,18 +80,22 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]") WHEN("Least Fleet Cost Evaluator") { auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); + auctioneer->set_evaluator(eval); AND_WHEN("0 submissions") { - std::vector submissions{}; + std::vector submissions{}; auto winner = evaluate(*auctioneer, submissions); REQUIRE(!winner); // no winner } AND_WHEN("5 submissions") { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; + std::vector submissions{ + Response{submission1, {}}, + Response{submission2, {}}, + Response{submission3, {}}, + Response{submission4, {}}, + Response{submission5, {}} }; auto winner = evaluate(*auctioneer, submissions); REQUIRE(winner->fleet_name == "fleet5"); // least diff cost agent } @@ -95,18 +104,22 @@ SCENARIO("Auctioneer Winner Evaluator", "[Evaluator]") WHEN("Quickest Finish Time Evaluator") { auto eval = std::make_shared(); - auctioneer->select_evaluator(eval); + auctioneer->set_evaluator(eval); AND_WHEN("0 submissions") { - std::vector submissions{}; + std::vector submissions{}; auto winner = evaluate(*auctioneer, submissions); REQUIRE(!winner); // no winner } AND_WHEN("5 submissions") { - std::vector submissions{ - submission1, submission2, submission3, submission4, submission5 }; + std::vector submissions{ + Response{submission1, {}}, + Response{submission2, {}}, + Response{submission3, {}}, + Response{submission4, {}}, + Response{submission5, {}} }; auto winner = evaluate(*auctioneer, submissions); REQUIRE(winner->fleet_name == "fleet3"); // least diff cost agent } diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp index 766ed3c78..0eed54f06 100644 --- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -15,11 +15,13 @@ * */ -#include +#include #include #include #include +#include + #include #include #include @@ -27,12 +29,9 @@ namespace rmf_task_ros2 { namespace bidding { -using TaskProfile = rmf_task_msgs::msg::TaskProfile; -using TaskType = bidding::MinimalBidder::TaskType; - //============================================================================== -BidNotice bidding_task1; -BidNotice bidding_task2; +BidNoticeMsg bidding_task1; +BidNoticeMsg bidding_task2; // set time window to 2s auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0)); @@ -41,20 +40,26 @@ auto timeout = rmf_traffic_ros2::convert(rmf_traffic::time::from_seconds(2.0)); SCENARIO("Auction with 2 Bids", "[TwoBids]") { // Initializing bidding task - bidding_task1.task_profile.task_id = "bid1"; + nlohmann::json req_1; + req_1["category"] = "patrol"; + req_1["description"] = "mocking a patrol"; + + nlohmann::json req_2; + req_2["category"] = "delivery"; + req_2["description"] = "mocking a delivery"; + + bidding_task1.task_id = "bid1"; bidding_task1.time_window = timeout; - bidding_task1.task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_STATION; + bidding_task1.request = req_1.dump(); - bidding_task2.task_profile.task_id = "bid2"; + bidding_task2.task_id = "bid2"; bidding_task2.time_window = timeout; - bidding_task2.task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_DELIVERY; + bidding_task2.request = req_2.dump(); //============================================================================ // test received msg - std::optional test_notice_bidder1; - std::optional test_notice_bidder2; + std::optional test_notice_bidder1; + std::optional test_notice_bidder2; std::string r_result_id = ""; std::string r_result_winner = ""; @@ -68,14 +73,17 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") node, /// Bidding Result Callback Function [&r_result_id, &r_result_winner]( - const std::string& task_id, const std::optional winner) + const auto& task_id, + const auto winner, + const auto&) { if (!winner) return; r_result_id = task_id; r_result_winner = winner->fleet_name; return; - } + }, + nullptr ); rclcpp::ExecutorOptions exec_options; @@ -83,29 +91,37 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") rclcpp::executors::SingleThreadedExecutor executor(exec_options); executor.add_node(node); - auto bidder1 = MinimalBidder::make( - node, "bidder1", { TaskType::Station, TaskType::Delivery }, - [&test_notice_bidder1](const BidNotice& notice) + auto bidder1 = AsyncBidder::make( + node, + [&test_notice_bidder1](const auto& notice, auto respond) { - Submission best_robot_estimate; - test_notice_bidder1 = notice.task_profile; + Response::Proposal best_robot_estimate; + test_notice_bidder1 = notice.request; + best_robot_estimate.fleet_name = "bidder1"; best_robot_estimate.finish_time = - std::chrono::steady_clock::time_point::max(); - return best_robot_estimate; + std::chrono::steady_clock::time_point::max(); + + respond(Response{best_robot_estimate, {}}); } ); - auto bidder2 = MinimalBidder::make( - node, "bidder2", { TaskType::Delivery, TaskType::Clean }, - [&test_notice_bidder2](const BidNotice& notice) + auto bidder2 = AsyncBidder::make( + node, + [&test_notice_bidder2](const auto& notice, auto respond) { + auto request = nlohmann::json::parse(notice.request); + if (request["category"] == "patrol") + return respond(Response{std::nullopt, {"Cannot patrol"}}); + // TaskType should not be supported - Submission best_robot_estimate; + Response::Proposal best_robot_estimate; best_robot_estimate.new_cost = 2.3; // lower cost than bidder1 + best_robot_estimate.fleet_name = "bidder2"; best_robot_estimate.finish_time = std::chrono::steady_clock::time_point::min(); - test_notice_bidder2 = notice.task_profile; - return best_robot_estimate; + test_notice_bidder2 = notice.request; + + respond(Response{best_robot_estimate, {}}); } ); @@ -113,19 +129,17 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") std::promise ready_promise; std::shared_future ready_future(ready_promise.get_future()); - WHEN("First 'Station' Task Bid") + WHEN("First 'patrol' Task Bid") { - // start bidding - bidding_task1.task_profile.submission_time = node->now(); - auctioneer->start_bidding(bidding_task1); + auctioneer->request_bid(bidding_task1); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(1.0)); // Check if bidder 1 & 2 receive BidNotice1 - REQUIRE(test_notice_bidder1); - REQUIRE(test_notice_bidder1->task_id == bidding_task1.task_profile.task_id); - REQUIRE(!test_notice_bidder2); // bidder2 doesnt support tasktype + REQUIRE(test_notice_bidder1.has_value()); + CHECK(*test_notice_bidder1 == bidding_task1.request); + REQUIRE(!test_notice_bidder2.has_value()); // bidder2 doesn't support patrol executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(2.5)); @@ -135,19 +149,19 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") REQUIRE(r_result_id == "bid1"); } - WHEN("Second 'Delivery' Task bid") + WHEN("Second 'delivery' Task bid") { // start bidding - bidding_task2.task_profile.submission_time = node->now(); - auctioneer->start_bidding(bidding_task2); + auctioneer->request_bid(bidding_task2); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(1.0)); // Check if bidder 1 & 2 receive BidNotice2 - auto task2_profile = bidding_task2.task_profile; - REQUIRE(test_notice_bidder1->task_id == task2_profile.task_id); - REQUIRE(test_notice_bidder2->task_id == task2_profile.task_id); + REQUIRE(test_notice_bidder1.has_value()); + CHECK(*test_notice_bidder1 == bidding_task2.request); + REQUIRE(test_notice_bidder2.has_value()); + REQUIRE(*test_notice_bidder2 == bidding_task2.request); executor.spin_until_future_complete(ready_future, rmf_traffic::time::from_seconds(2.5)); diff --git a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp index 2fbb532b8..0374946a2 100644 --- a/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp +++ b/rmf_task_ros2/test/dispatcher/test_Dispatcher.cpp @@ -19,12 +19,14 @@ #include // mock Fleet Adapter to test dispatcher -#include -#include "../../src/rmf_task_ros2/action/Server.hpp" +#include +#include #include #include -#include +#include + +#include #include #include @@ -35,8 +37,8 @@ namespace rmf_task_ros2 { //============================================================================== SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") { - Dispatcher::TaskDescription task_desc1; - Dispatcher::TaskDescription task_desc2; + rmf_task_msgs::msg::TaskDescription task_desc1; + rmf_task_msgs::msg::TaskDescription task_desc2; task_desc1.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_STATION; task_desc2.task_type.type = rmf_task_msgs::msg::TaskType::TYPE_CLEAN; @@ -58,7 +60,7 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") { using SubmitTaskSrv = rmf_task_msgs::srv::SubmitTask; using CancelTaskSrv = rmf_task_msgs::srv::CancelTask; - using GetTaskListSrv = rmf_task_msgs::srv::GetTaskList; + using GetDispatchStatesSrv = rmf_task_msgs::srv::GetDispatchStates; auto submit_client = dispatcher->node()->create_client( rmf_task_ros2::SubmitTaskSrvName); @@ -66,8 +68,9 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") auto cancel_client = dispatcher->node()->create_client( rmf_task_ros2::CancelTaskSrvName); REQUIRE(cancel_client->wait_for_service(std::chrono::milliseconds(0))); - auto get_tasks_client = dispatcher->node()->create_client( - rmf_task_ros2::GetTaskListSrvName); + auto get_tasks_client = + dispatcher->node()->create_client( + rmf_task_ros2::GetDispatchStatesSrvName); REQUIRE(get_tasks_client->wait_for_service(std::chrono::milliseconds(0))); } @@ -76,17 +79,19 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") // add task const auto id = dispatcher->submit_task(task_desc1); REQUIRE(id.has_value()); - REQUIRE(dispatcher->active_tasks().size() == 1); - REQUIRE(dispatcher->terminated_tasks().size() == 0); - REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + CHECK(dispatcher->active_dispatches().size() == 1); + CHECK(dispatcher->finished_dispatches().size() == 0); + const auto state = dispatcher->get_dispatch_state(*id); + REQUIRE(state.has_value()); + REQUIRE(state->status == DispatchState::Status::Queued); // cancel task REQUIRE(dispatcher->cancel_task(*id)); - REQUIRE(dispatcher->active_tasks().size() == 0); - REQUIRE(dispatcher->terminated_tasks().size() == 1); + REQUIRE(dispatcher->active_dispatches().size() == 0); + REQUIRE(dispatcher->finished_dispatches().size() == 1); // check nonsense id - REQUIRE(!(dispatcher->get_task_state("non_existent_id"))); + REQUIRE(!(dispatcher->get_dispatch_state("non_existent_id"))); // Try sleeping for a moment here to mitigate race conditions. // TODO(MXG): We should rework these tests so that we don't need to put in @@ -102,11 +107,11 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") //============================================================================ // test on change fn callback const auto change_times = std::make_shared(0); - const auto test_taskprofile = std::make_shared(); + const auto test_id = std::make_shared(); dispatcher->on_change( - [change_times, test_taskprofile](const TaskStatusPtr status) + [change_times, test_id](const auto& state) { - *test_taskprofile = status->task_profile; + *test_id = state.task_id; (*change_times)++; } ); @@ -115,151 +120,48 @@ SCENARIO("Dispatcher API Test", "[Dispatcher][.flaky]") { // Submit first task and wait for bidding auto id = dispatcher->submit_task(task_desc1); - REQUIRE(dispatcher->active_tasks().size() == 1); - REQUIRE(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); + REQUIRE(dispatcher->active_dispatches().size() == 1); + const auto state = dispatcher->get_dispatch_state(*id); + REQUIRE(state.has_value()); + REQUIRE(state->status == DispatchState::Status::Queued); // Default 2s timeout, wait 3s for timetout, should fail here std::this_thread::sleep_for(std::chrono::milliseconds(3500)); - CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Failed); - REQUIRE(dispatcher->terminated_tasks().size() == 1); + CHECK(dispatcher->get_dispatch_state(*id)->status == + DispatchState::Status::FailedToAssign); + + CHECK(dispatcher->finished_dispatches().size() == 1); // TODO(MXG): Flake out after previous line: SIGABRT - REQUIRE(test_taskprofile->task_id == id); + CHECK(*test_id == id); CHECK(*change_times == 2); // add and failed // Submit another task id = dispatcher->submit_task(task_desc2); + REQUIRE(id.has_value()); std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - REQUIRE(dispatcher->terminated_tasks().size() == 2); - REQUIRE(test_taskprofile->task_id == *id); + REQUIRE(dispatcher->finished_dispatches().size() == 2); + REQUIRE(*test_id == *id); CHECK(*change_times == 4); // add and failed x2 } //============================================================================ // Setup Mock Fleetadapter: mock bidder to test - using TaskType = bidding::MinimalBidder::TaskType; - auto bidder = bidding::MinimalBidder::make( + auto bidder = bidding::AsyncBidder::make( node, - "dummy_fleet", - { TaskType::Station, TaskType::Clean }, - [](const bidding::BidNotice&) + [](const bidding::BidNoticeMsg& notice, auto respond) { + const auto request = nlohmann::json::parse(notice.request); + if (request["category"] != "patrol") + return respond(bidding::Response{std::nullopt, {}}); + // Provide a best estimate - bidding::Submission best_robot_estimate; + bidding::Response::Proposal best_robot_estimate; + best_robot_estimate.fleet_name = "dummy_fleet"; best_robot_estimate.new_cost = 13.5; - return best_robot_estimate; - } - ); - - //============================================================================ - // Setup Mock Fleetadapter: action server to test - auto action_server = action::Server::make(node, "dummy_fleet"); - - const auto task_canceled_flag = std::make_shared(false); - - // We use the action_mutex to make sure that the main thread does not stop - // until the action response callback has had enough time to do its job. - const auto action_mutex = std::make_shared(); - - action_server->register_callbacks( - // Add Task callback - [ - a = std::weak_ptr(action_server), - action_mutex, - task_canceled_flag - ](const TaskProfile& task_profile) - { - // Start action task - auto t = std::thread( - [a, action_mutex, task_canceled_flag](auto profile) - { - std::lock_guard lock(*action_mutex); - const auto action_server = a.lock(); - if (!action_server) - return; - - TaskStatus status; - status.task_profile = profile; - status.robot_name = "dumbot"; - std::this_thread::sleep_for(std::chrono::seconds(2)); - - if (*task_canceled_flag) - { - // std::cout << "[task impl] Cancelled!" << std::endl; - return; - } - - // Executing - status.state = TaskStatus::State::Executing; - action_server->update_status(status); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Completed - status.state = TaskStatus::State::Completed; - action_server->update_status(status); - }, task_profile - ); - t.detach(); - return true; //successs (send State::Queued) - }, - // Cancel Task callback - [task_canceled_flag](const TaskProfile&) - { - *task_canceled_flag = true; - return true; //success ,send State::Canceled when dispatcher->cancel_task + respond(bidding::Response{best_robot_estimate, {}}); } ); - //============================================================================ - WHEN("Full Dispatch cycle") - { - const auto id = dispatcher->submit_task(task_desc1); - CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); - std::this_thread::sleep_for(std::chrono::milliseconds(3500)); - - // now should queue the task - CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Queued); - REQUIRE(dispatcher->terminated_tasks().size() == 0); - CHECK(*change_times == 2); // Pending and Queued - - std::this_thread::sleep_for(std::chrono::seconds(3)); - CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Completed); - REQUIRE(dispatcher->active_tasks().size() == 0); - REQUIRE(dispatcher->terminated_tasks().size() == 1); - CHECK(*change_times == 4); // Pending > Queued > Executing > Completed - - // Add auto generated ChargeBattery Task from fleet adapter - TaskStatus status; - status.task_profile.task_id = "ChargeBattery10"; - status.state = TaskStatus::State::Queued; - status.task_profile.description.task_type.type = - rmf_task_msgs::msg::TaskType::TYPE_CHARGE_BATTERY; - action_server->update_status(status); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - CHECK(*change_times == 5); // new stray charge task - REQUIRE(dispatcher->active_tasks().size() == 1); - } - - WHEN("Half way cancel Dispatch cycle") - { - const auto id = dispatcher->submit_task(task_desc2); - CHECK(dispatcher->get_task_state(*id) == TaskStatus::State::Pending); - REQUIRE(dispatcher->active_tasks().size() == 1); - std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - - // cancel the task after QUEUED State - REQUIRE(dispatcher->cancel_task(*id)); - std::this_thread::sleep_for(std::chrono::seconds(1)); - - REQUIRE(dispatcher->active_tasks().size() == 0); - REQUIRE(dispatcher->terminated_tasks().size() == 1); - REQUIRE(dispatcher->terminated_tasks().begin()->first == *id); - auto status = dispatcher->terminated_tasks().begin()->second; - CHECK(status->state == TaskStatus::State::Canceled); - CHECK(*change_times == 3); // Pending -> Queued -> Canceled - } - - std::lock_guard lock(*action_mutex); rclcpp::shutdown(rcl_context); dispatcher_spin_thread.join(); } diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp index c953d0239..e52454890 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/schedule/MirrorManager.cpp @@ -408,12 +408,12 @@ class MirrorManager::Implementation { request.version = minimum_version.value(); request.full_update = false; - RCLCPP_INFO( - node->get_logger(), - "[rmf_traffic_ros2::MirrorManager::request_update] Requesting changes " - "for query ID [%ld] since version [%ld]", - request.query_id, - request.version); +// RCLCPP_INFO( +// node->get_logger(), +// "[rmf_traffic_ros2::MirrorManager::request_update] Requesting changes " +// "for query ID [%ld] since version [%ld]", +// request.query_id, +// request.version); } else { diff --git a/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp b/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp index 6e19ad44d..3d7ac7996 100644 --- a/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp +++ b/rmf_traffic_ros2/test/unit/test_ParticipantRegistry.cpp @@ -49,15 +49,6 @@ SCENARIO("Test idempotency of shape type") REQUIRE_THROWS(shape_type(node["type"])); } -namespace rmf_traffic { - -bool operator==(const Profile p1, const Profile p2) -{ - return rmf_traffic_ros2::convert(p1) == rmf_traffic_ros2::convert(p2); -} - -} - namespace rmf_traffic_ros2 { namespace schedule {