From 68f006405df430a0567bbf7c937299fc87b273db Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 15 Jun 2023 12:15:07 +0800 Subject: [PATCH 1/3] First draft of robot charging task Signed-off-by: Luca Della Vedova --- .../schemas/event_description__charge.json | 9 ++++++++ .../schemas/task_description__charge.json | 7 ++++++ .../agv/FleetUpdateHandle.cpp | 1 + .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 22 +++++++++++++++++++ .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 2 ++ 5 files changed, 41 insertions(+) create mode 100644 rmf_fleet_adapter/schemas/event_description__charge.json create mode 100644 rmf_fleet_adapter/schemas/task_description__charge.json diff --git a/rmf_fleet_adapter/schemas/event_description__charge.json b/rmf_fleet_adapter/schemas/event_description__charge.json new file mode 100644 index 000000000..8d293466c --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description__charge.json @@ -0,0 +1,9 @@ +{ + "$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__charge.json", + "title": "Charge Event", + "description": "Charge a robot", + "type": "object", + "properties": { + } +} diff --git a/rmf_fleet_adapter/schemas/task_description__charge.json b/rmf_fleet_adapter/schemas/task_description__charge.json new file mode 100644 index 000000000..9108c4849 --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description__charge.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__charge.json", + "title": "Charge Task", + "description": "Charge a robot", + "$ref": "event_description__charge.json" +} 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 803dc3243..04edbf223 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1104,6 +1104,7 @@ void FleetUpdateHandle::Implementation::add_standard_tasks() node->clock()); tasks::add_charge_battery( + deserialization, *activation.task, activation.phase, *activation.event, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp index 06c5a02bb..86c4181e9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -26,6 +26,9 @@ #include #include +#include +#include + #include namespace rmf_fleet_adapter { @@ -165,6 +168,7 @@ struct ChargeBatteryEventDescription //============================================================================== void add_charge_battery( + agv::TaskDeserialization& deserialization, rmf_task::Activator& task_activator, const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator, rmf_task_sequence::Event::Initializer& event_initializer, @@ -180,6 +184,24 @@ void add_charge_battery( WaitForChargeDescription::add(*private_initializer); GoToChargerDescription::add(*private_initializer); + // TODO(luca) Populate charge event with properties such as time, desired battery soc + deserialization.add_schema(schemas::event_description__charge); + + auto validate_charge_task = + deserialization.make_validator_shared( + schemas::task_description__charge); + deserialization.add_schema(schemas::task_description__charge); + + auto deserialize_charge = + [ + ](const nlohmann::json&) -> agv::DeserializedTask + { + // Always accept + agv::FleetUpdateHandle::Confirmation confirm; + return {rmf_task::requests::ChargeBattery::Description::make(), {}}; + }; + deserialization.task->add("charge", validate_charge_task, deserialize_charge); + auto charge_battery_event_unfolder = [](const ChargeBatteryEventDescription&) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp index 55657bbcf..158b153df 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -19,6 +19,7 @@ #define SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP #include "../LegacyTask.hpp" +#include "../agv/internal_FleetUpdateHandle.hpp" #include "../agv/RobotContext.hpp" #include @@ -43,6 +44,7 @@ std::shared_ptr make_charge_battery( //============================================================================== void add_charge_battery( + agv::TaskDeserialization& deserialization, rmf_task::Activator& task_activator, const rmf_task_sequence::Phase::ConstActivatorPtr& phase_activator, rmf_task_sequence::Event::Initializer& event_initializer, From 99304d6168796ebe8ca61c9bffc4d494c03033cf Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 15 Jun 2023 13:26:04 +0800 Subject: [PATCH 2/3] Rename charge to charge battery Signed-off-by: Luca Della Vedova --- ...on => event_description__charge_battery.json} | 6 +++--- .../schemas/task_description__charge.json | 7 ------- .../task_description__charge_battery.json | 7 +++++++ .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 16 ++++++++-------- 4 files changed, 18 insertions(+), 18 deletions(-) rename rmf_fleet_adapter/schemas/{event_description__charge.json => event_description__charge_battery.json} (53%) delete mode 100644 rmf_fleet_adapter/schemas/task_description__charge.json create mode 100644 rmf_fleet_adapter/schemas/task_description__charge_battery.json diff --git a/rmf_fleet_adapter/schemas/event_description__charge.json b/rmf_fleet_adapter/schemas/event_description__charge_battery.json similarity index 53% rename from rmf_fleet_adapter/schemas/event_description__charge.json rename to rmf_fleet_adapter/schemas/event_description__charge_battery.json index 8d293466c..42f14c44d 100644 --- a/rmf_fleet_adapter/schemas/event_description__charge.json +++ b/rmf_fleet_adapter/schemas/event_description__charge_battery.json @@ -1,8 +1,8 @@ { "$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__charge.json", - "title": "Charge Event", - "description": "Charge a robot", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/event_description__charge_battery.json", + "title": "Charge Battery Event", + "description": "Charge a robot's battery", "type": "object", "properties": { } diff --git a/rmf_fleet_adapter/schemas/task_description__charge.json b/rmf_fleet_adapter/schemas/task_description__charge.json deleted file mode 100644 index 9108c4849..000000000 --- a/rmf_fleet_adapter/schemas/task_description__charge.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "$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__charge.json", - "title": "Charge Task", - "description": "Charge a robot", - "$ref": "event_description__charge.json" -} diff --git a/rmf_fleet_adapter/schemas/task_description__charge_battery.json b/rmf_fleet_adapter/schemas/task_description__charge_battery.json new file mode 100644 index 000000000..4b40f85bd --- /dev/null +++ b/rmf_fleet_adapter/schemas/task_description__charge_battery.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__charge_battery.json", + "title": "Charge Battery Task", + "description": "Charge a robot's battery", + "$ref": "event_description__charge_battery.json" +} 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 86c4181e9..c4f7ebef2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #include @@ -185,22 +185,22 @@ void add_charge_battery( GoToChargerDescription::add(*private_initializer); // TODO(luca) Populate charge event with properties such as time, desired battery soc - deserialization.add_schema(schemas::event_description__charge); + deserialization.add_schema(schemas::event_description__charge_battery); auto validate_charge_task = deserialization.make_validator_shared( - schemas::task_description__charge); - deserialization.add_schema(schemas::task_description__charge); + schemas::task_description__charge_battery); + deserialization.add_schema(schemas::task_description__charge_battery); auto deserialize_charge = - [ - ](const nlohmann::json&) -> agv::DeserializedTask + [] + (const nlohmann::json&) -> agv::DeserializedTask { // Always accept agv::FleetUpdateHandle::Confirmation confirm; return {rmf_task::requests::ChargeBattery::Description::make(), {}}; }; - deserialization.task->add("charge", validate_charge_task, deserialize_charge); + deserialization.task->add("charge_battery", validate_charge_task, deserialize_charge); auto charge_battery_event_unfolder = [](const ChargeBatteryEventDescription&) From d87b87bfdb952cc4cc581be5583d1cf6409cb1b4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 15 Jun 2023 16:04:48 +0800 Subject: [PATCH 3/3] Prevent fleet adapter from bidding for battery charging tasks Signed-off-by: Luca Della Vedova --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 13 +++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 3 +++ .../src/rmf_fleet_adapter/tasks/ChargeBattery.cpp | 3 ++- 3 files changed, 18 insertions(+), 1 deletion(-) 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 04edbf223..069669c8d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -353,6 +353,16 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( }); } + const auto& category = request_msg["category"].get(); + if (bid_ignored_categories.find(category) != bid_ignored_categories.end()) + { + RCLCPP_WARN( + node->get_logger(), + "Fleet [%s] received bid request for category [%s] that can only " + "be a direct task assignment", name.c_str(), category.c_str()); + return; + } + std::vector errors = {}; const auto new_request = convert(task_id, request_msg, errors); if (!new_request) @@ -1114,6 +1124,9 @@ void FleetUpdateHandle::Implementation::add_standard_tasks() deserialization, activation, node->clock()); + + // TODO(luca) make this configurable to system integrators + bid_ignored_categories.insert("charge_battery"); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 56e8e5a7a..6575e14e7 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 @@ -253,6 +253,9 @@ class FleetUpdateHandle::Implementation TaskActivation activation = TaskActivation(); TaskDeserialization deserialization = TaskDeserialization(); + // Categories of tasks that the fleet adapter should not bid for but should + // still accept as direct tasks, such as battery charging + std::unordered_set bid_ignored_categories = {}; // LegacyTask planner params std::shared_ptr cost_calculator = 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 c4f7ebef2..0a481f2b8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -200,7 +200,8 @@ void add_charge_battery( agv::FleetUpdateHandle::Confirmation confirm; return {rmf_task::requests::ChargeBattery::Description::make(), {}}; }; - deserialization.task->add("charge_battery", validate_charge_task, deserialize_charge); + deserialization.task->add("charge_battery", validate_charge_task, + deserialize_charge); auto charge_battery_event_unfolder = [](const ChargeBatteryEventDescription&)