diff --git a/rmf_fleet_adapter/schemas/event_description__charge_battery.json b/rmf_fleet_adapter/schemas/event_description__charge_battery.json new file mode 100644 index 000000000..42f14c44d --- /dev/null +++ b/rmf_fleet_adapter/schemas/event_description__charge_battery.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_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_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/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index eb40d6e2c..fab4ccfe9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -379,6 +379,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) @@ -1130,6 +1140,7 @@ void FleetUpdateHandle::Implementation::add_standard_tasks() node->clock()); tasks::add_charge_battery( + deserialization, *activation.task, activation.phase, *activation.event, @@ -1139,6 +1150,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 06c5a02bb..0a481f2b8 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,25 @@ 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_battery); + + auto validate_charge_task = + deserialization.make_validator_shared( + schemas::task_description__charge_battery); + deserialization.add_schema(schemas::task_description__charge_battery); + + 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_battery", 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,