From 8ddbf04a8fa83ee9fff34bd3acc4451be1ee5695 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 17 Oct 2020 13:33:42 +0800 Subject: [PATCH 01/50] Added set_queue and requests methods to TaskManager --- rmf_fleet_adapter/CMakeLists.txt | 3 ++ rmf_fleet_adapter/package.xml | 1 + .../src/rmf_fleet_adapter/Task.cpp | 20 ++++++++-- .../src/rmf_fleet_adapter/Task.hpp | 15 ++++++- .../src/rmf_fleet_adapter/TaskManager.cpp | 23 +++++++++++ .../src/rmf_fleet_adapter/TaskManager.hpp | 12 ++++++ .../src/rmf_fleet_adapter/tasks/Clean.hpp | 40 +++++++++++++++++++ rmf_task/include/rmf_task/Request.hpp | 3 ++ 8 files changed, 111 insertions(+), 6 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 13c014078..35c0a16c0 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -37,6 +37,7 @@ set(dep_pkgs rmf_task_msgs rmf_traffic rmf_traffic_ros2 + rmf_task std_msgs ) foreach(pkg ${dep_pkgs}) @@ -58,6 +59,7 @@ add_library(rmf_fleet_adapter SHARED target_link_libraries(rmf_fleet_adapter PUBLIC rmf_traffic_ros2::rmf_traffic_ros2 + rmf_task::rmf_task yaml-cpp ${rmf_fleet_msgs_LIBRARIES} ${rclcpp_LIBRARIES} @@ -78,6 +80,7 @@ target_include_directories(rmf_fleet_adapter ${rmf_fleet_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ${rmf_task_msgs_INCLUDE_DIRS} + ${rmf_task_INCLUDE_DIRS} PRIVATE ${rmf_door_msgs_INCLUDE_DIRS} ${rmf_lift_msgs_INCLUDE_DIRS} diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 36b8bf26d..cf4a4ed60 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -21,6 +21,7 @@ rmf_traffic rmf_traffic_ros2 rmf_traffic_msgs + rmf_task eigen yaml-cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index f0a4e6d71..9540eeb43 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -30,10 +30,14 @@ namespace rmf_fleet_adapter { std::shared_ptr Task::make( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker) + rxcpp::schedulers::worker worker, + rmf_task::RequestPtr request) { return std::make_shared( - Task(std::move(id), std::move(phases), std::move(worker))); + Task(std::move(id), + std::move(phases), + std::move(worker), + std::move(request))); } //============================================================================== @@ -80,14 +84,22 @@ const std::string& Task::id() const return _id; } +//============================================================================== +const rmf_task::RequestPtr Task::request() const +{ + return _request; +} + //============================================================================== Task::Task( std::string id, std::vector> phases, - rxcpp::schedulers::worker worker) + rxcpp::schedulers::worker worker, + rmf_task::RequestPtr request) : _id(std::move(id)), _pending_phases(std::move(phases)), - _worker(std::move(worker)) + _worker(std::move(worker)), + _request(std::move(request)) { _status_obs = _status_publisher.get_observable(); std::reverse(_pending_phases.begin(), _pending_phases.end()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index d3358da4a..e7abbba7e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -25,6 +25,8 @@ #include +#include + #include #include @@ -93,10 +95,13 @@ class Task : public std::enable_shared_from_this using PendingPhases = std::vector>; // Make a new task + // TODO(YV) Remove default nullptr for request after refactoring Loop and + // Delivery static std::shared_ptr make( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker); + rxcpp::schedulers::worker worker, + rmf_task::RequestPtr request = nullptr); void begin(); @@ -117,12 +122,16 @@ class Task : public std::enable_shared_from_this const std::string& id() const; + /// Get the request used to generate this task + const rmf_task::RequestPtr request() const; + private: Task( std::string id, PendingPhases phases, - rxcpp::schedulers::worker worker); + rxcpp::schedulers::worker worker, + rmf_task::RequestPtr request); std::string _id; @@ -140,6 +149,8 @@ class Task : public std::enable_shared_from_this rmf_utils::optional _initial_time; + rmf_task::RequestPtr _request; + void _start_next_phase(); StatusMsg _process_summary(const StatusMsg& input_msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3e044d686..35c05688b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -96,6 +96,23 @@ agv::ConstRobotContextPtr TaskManager::context() const return _context; } +//============================================================================== +void TaskManager::set_queue(TaskManager::Assignments assignments) +{ + // TODO(YV) +} + +//============================================================================== +const std::vector TaskManager::requests() const +{ + std::vector requests; + requests.reserve(_queue.size()); + for (const auto& task : _queue) + requests.push_back(task->request()); + + return requests; +} + //============================================================================== void TaskManager::_begin_next_task() { @@ -159,4 +176,10 @@ void TaskManager::_begin_next_task() _active_task->begin(); } +//============================================================================== +void TaskManager::clear_queue() +{ + _queue.clear(); +} + } // 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 c36496de2..05792e737 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -23,6 +23,8 @@ #include +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -39,6 +41,7 @@ class TaskManager : public std::enable_shared_from_this using Start = rmf_traffic::agv::Plan::Start; using StartSet = rmf_traffic::agv::Plan::StartSet; + using Assignments = rmf_task::agv::TaskPlanner::Assignments; /// Add a task to the queue of this manager. void queue_task(std::shared_ptr task, Start expected_finish); @@ -51,6 +54,13 @@ class TaskManager : public std::enable_shared_from_this agv::ConstRobotContextPtr context() const; + /// Set the queue for this task manager with assignments generated from the + /// task planner + void set_queue(Assignments assignments); + + /// Get the requests used to create the tasks currently in the queue + const std::vector requests() const; + private: TaskManager(agv::RobotContextPtr context); @@ -63,6 +73,8 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _emergency_sub; void _begin_next_task(); + + void clear_queue(); }; using TaskManagerPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp new file mode 100644 index 000000000..9d919a60e --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -0,0 +1,40 @@ +/* + * 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_FLEET_ADAPTER__TASKS__CLEAN_HPP +#define SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +// TODO(MXG): This is a sloppy design. We should have a task estimator + factory +// interface to handle the task dispatch and creation pipeline more elegantly. +std::shared_ptr make_clean( + const rmf_task::RequestPtr request, + const agv::RobotContextPtr& context, + rmf_traffic::agv::Plan::Start clean_start); + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__TASKS__CLEAN_HPP diff --git a/rmf_task/include/rmf_task/Request.hpp b/rmf_task/include/rmf_task/Request.hpp index 1c24b6ee2..1cfbe52a7 100644 --- a/rmf_task/include/rmf_task/Request.hpp +++ b/rmf_task/include/rmf_task/Request.hpp @@ -54,6 +54,9 @@ class Request virtual ~Request() = default; }; +using RequestPtr = std::shared_ptr; +using ConstRequestPtr = std::shared_ptr; + } // namespace rmf_task #endif // INCLUDE__RMF_TASK__TASK_HPP From b693d10d35cb5c30c7faf35fbd70af7a836bba49 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 17 Oct 2020 17:59:13 +0800 Subject: [PATCH 02/50] Added make_clean to convert from request to task --- .../src/rmf_fleet_adapter/tasks/Clean.cpp | 41 +++++++++++++++++++ .../src/rmf_fleet_adapter/tasks/Clean.hpp | 7 ++-- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 4 +- 3 files changed, 47 insertions(+), 5 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp new file mode 100644 index 000000000..8c1782a30 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -0,0 +1,41 @@ +/* + * 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 "../phases/GoToPlace.hpp" + +#include "Clean.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_clean( + const rmf_task::RequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::agv::Plan::Goal clean_goal) +{ + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); + + return Task::make( + std::to_string(request->id()), std::move(phases), context->worker()); +} + +} // 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 9d919a60e..fca81468b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -21,18 +21,19 @@ #include "../Task.hpp" #include "../agv/RobotContext.hpp" +#include + #include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -// TODO(MXG): This is a sloppy design. We should have a task estimator + factory -// interface to handle the task dispatch and creation pipeline more elegantly. std::shared_ptr make_clean( const rmf_task::RequestPtr request, const agv::RobotContextPtr& context, - rmf_traffic::agv::Plan::Start clean_start); + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::agv::Plan::Goal clean_goal); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 9d185fdcd..c6b5afdde 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -198,7 +198,7 @@ SCENARIO("Grid World") const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - + display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); @@ -359,7 +359,7 @@ SCENARIO("Grid World") const auto optimal_assignments = task_planner.optimal_plan( now, initial_states, state_configs, requests, nullptr); const double optimal_cost = task_planner.compute_cost(optimal_assignments); - + display_solution("Greedy", greedy_assignments, greedy_cost); display_solution("Optimal", optimal_assignments, optimal_cost); From 15cd8c7c9c82d7458a1a1d4064c6b8d77e423c6f Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 20 Oct 2020 10:08:40 +0800 Subject: [PATCH 03/50] Commented out Delivery and Loop request subs in Adapter --- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 107 +++++++++--------- 1 file changed, 53 insertions(+), 54 deletions(-) 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 ff647ec13..f4434c276 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -66,23 +66,22 @@ class Adapter::Implementation std::shared_ptr writer; rmf_traffic_ros2::schedule::MirrorManager mirror_manager; + // using Delivery = rmf_task_msgs::msg::Delivery; + // using DeliverySub = rclcpp::Subscription::SharedPtr; + // DeliverySub delivery_sub; - using Delivery = rmf_task_msgs::msg::Delivery; - using DeliverySub = rclcpp::Subscription::SharedPtr; - DeliverySub delivery_sub; - - using Loop = rmf_task_msgs::msg::Loop; - using LoopSub = rclcpp::Subscription::SharedPtr; - LoopSub loop_sub; + // using Loop = rmf_task_msgs::msg::Loop; + // using LoopSub = rclcpp::Subscription::SharedPtr; + // LoopSub loop_sub; std::vector> fleets = {}; // TODO(MXG): This mutex probably isn't needed std::mutex mutex; - std::unordered_set received_tasks; - std::map task_times; - rclcpp::TimerBase::SharedPtr task_purge_timer; + // std::unordered_set received_tasks; + // std::map task_times; + // rclcpp::TimerBase::SharedPtr task_purge_timer; Implementation( rxcpp::schedulers::worker worker_, @@ -96,50 +95,50 @@ class Adapter::Implementation writer{std::move(writer_)}, mirror_manager{std::move(mirror_manager_)} { - const auto default_qos = rclcpp::SystemDefaultsQoS(); - delivery_sub = node->create_subscription( - DeliveryTopicName, default_qos, - [this](Delivery::SharedPtr msg) - { - std::lock_guard lock(mutex); - if (!received_tasks.insert(msg->task_id).second) - return; - - task_times.insert( - task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - rmf_fleet_adapter::agv::request_delivery(*msg, fleets); - }); - - loop_sub = node->create_subscription( - LoopRequestTopicName, default_qos, - [this](Loop::SharedPtr msg) - { - std::lock_guard lock(mutex); - if (!received_tasks.insert(msg->task_id).second) - return; - - task_times.insert( - task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); - - rmf_fleet_adapter::agv::request_loop(*msg, fleets); - }); - - task_purge_timer = node->create_wall_timer( - std::chrono::minutes(60), [this]() - { - // This purge of task ids is to prevent the log of tasks from growing - // infinitely. - const auto purge_end = - std::chrono::steady_clock::now() - std::chrono::minutes(60); - - auto it = task_times.begin(); - for (; it != task_times.end() && it->first < purge_end; ++it) - received_tasks.erase(it->second); - - if (it != task_times.begin()) - task_times.erase(task_times.begin(), it); - }); + // const auto default_qos = rclcpp::SystemDefaultsQoS(); + // delivery_sub = node->create_subscription( + // DeliveryTopicName, default_qos, + // [this](Delivery::SharedPtr msg) + // { + // std::lock_guard lock(mutex); + // if (!received_tasks.insert(msg->task_id).second) + // return; + + // task_times.insert( + // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); + + // rmf_fleet_adapter::agv::request_delivery(*msg, fleets); + // }); + + // loop_sub = node->create_subscription( + // LoopRequestTopicName, default_qos, + // [this](Loop::SharedPtr msg) + // { + // std::lock_guard lock(mutex); + // if (!received_tasks.insert(msg->task_id).second) + // return; + + // task_times.insert( + // task_times.end(), {std::chrono::steady_clock::now(), msg->task_id}); + + // rmf_fleet_adapter::agv::request_loop(*msg, fleets); + // }); + + // task_purge_timer = node->create_wall_timer( + // std::chrono::minutes(60), [this]() + // { + // // This purge of task ids is to prevent the log of tasks from growing + // // infinitely. + // const auto purge_end = + // std::chrono::steady_clock::now() - std::chrono::minutes(60); + + // auto it = task_times.begin(); + // for (; it != task_times.end() && it->first < purge_end; ++it) + // received_tasks.erase(it->second); + + // if (it != task_times.begin()) + // task_times.erase(task_times.begin(), it); + // }); } static rmf_utils::unique_impl_ptr make( From 1cfbe5efcd5c26c42103fba10c46cc1d825a6e15 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 20 Oct 2020 13:33:56 +0800 Subject: [PATCH 04/50] Commented out Delivery and Loop related code in FleetUpdateHandle --- .../rmf_fleet_adapter/StandardNames.hpp | 4 + .../src/rmf_fleet_adapter/agv/Adapter.cpp | 4 +- .../agv/FleetUpdateHandle.cpp | 522 +++++++++--------- .../agv/internal_FleetUpdateHandle.hpp | 86 +-- .../agv/test/MockAdapter.cpp | 4 +- 5 files changed, 320 insertions(+), 300 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 6859c3b09..4962d398b 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -49,6 +49,10 @@ const std::string DeliveryTopicName = "delivery_requests"; const std::string LoopRequestTopicName = "loop_requests"; const std::string TaskSummaryTopicName = "task_summaries"; +const std::string BidNoticeTopicName = "bid_notice"; +const std::string BidProposalTopicName = "bid_proposal"; +const std::string DispatchRequestTopicName = "dispatch_request"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP 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 f4434c276..dc73081e6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -25,8 +25,8 @@ #include #include -#include -#include +// #include +// #include #include "internal_TrafficLight.hpp" 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 070aef3b9..ed3357b72 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -61,218 +61,218 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator } // anonymous namespace //============================================================================== -auto FleetUpdateHandle::Implementation::estimate_delivery( - const rmf_task_msgs::msg::Delivery& request) const --> rmf_utils::optional -{ - const auto& graph = planner->get_configuration().graph(); - const auto pickup_wp = graph.find_waypoint(request.pickup_place_name); - if (!pickup_wp) - return rmf_utils::nullopt; - - const auto dropoff_wp = graph.find_waypoint(request.dropoff_place_name); - if (!dropoff_wp) - return rmf_utils::nullopt; - - const auto pickup_goal = rmf_traffic::agv::Plan::Goal(pickup_wp->index()); - const auto dropoff_goal = rmf_traffic::agv::Plan::Goal(dropoff_wp->index()); - - // TODO(MXG): At some point we should consider parallelizing this estimation - // process and taking the existing schedule into account, but for now we'll - // try to use a very quick rough estimate. - DeliveryEstimate best; - for (const auto& element : task_managers) - { - const auto& mgr = *element.second; - auto start = mgr.expected_finish_location(); - const auto pickup_plan = planner->plan(start, pickup_goal); - if (!pickup_plan) - continue; - - const auto& pickup_plan_end = pickup_plan->get_waypoints().back(); - assert(pickup_plan_end.graph_index()); - const auto dropoff_start = rmf_traffic::agv::Plan::Start( - pickup_plan_end.time(), - *pickup_plan_end.graph_index(), - pickup_plan_end.position()[2]); - - const auto dropoff_plan = planner->plan(dropoff_start, dropoff_goal); - if (!dropoff_plan) - continue; - - const auto& final_wp = dropoff_plan->get_waypoints().back(); - - const auto estimate = final_wp.time(); - rmf_traffic::agv::Plan::Start finish{ - estimate, - *final_wp.graph_index(), - final_wp.position()[2] - }; - - if (estimate < best.time) - { - best = DeliveryEstimate{ - estimate, - element.first, - std::move(start.front()), - std::move(dropoff_start), - std::move(finish) - }; - } - } - - if (best.robot) - return best; - - return rmf_utils::nullopt; -} +// auto FleetUpdateHandle::Implementation::estimate_delivery( +// const rmf_task_msgs::msg::Delivery& request) const +// -> rmf_utils::optional +// { +// const auto& graph = planner->get_configuration().graph(); +// const auto pickup_wp = graph.find_waypoint(request.pickup_place_name); +// if (!pickup_wp) +// return rmf_utils::nullopt; + +// const auto dropoff_wp = graph.find_waypoint(request.dropoff_place_name); +// if (!dropoff_wp) +// return rmf_utils::nullopt; + +// const auto pickup_goal = rmf_traffic::agv::Plan::Goal(pickup_wp->index()); +// const auto dropoff_goal = rmf_traffic::agv::Plan::Goal(dropoff_wp->index()); + +// // TODO(MXG): At some point we should consider parallelizing this estimation +// // process and taking the existing schedule into account, but for now we'll +// // try to use a very quick rough estimate. +// DeliveryEstimate best; +// for (const auto& element : task_managers) +// { +// const auto& mgr = *element.second; +// auto start = mgr.expected_finish_location(); +// const auto pickup_plan = planner->plan(start, pickup_goal); +// if (!pickup_plan) +// continue; + +// const auto& pickup_plan_end = pickup_plan->get_waypoints().back(); +// assert(pickup_plan_end.graph_index()); +// const auto dropoff_start = rmf_traffic::agv::Plan::Start( +// pickup_plan_end.time(), +// *pickup_plan_end.graph_index(), +// pickup_plan_end.position()[2]); + +// const auto dropoff_plan = planner->plan(dropoff_start, dropoff_goal); +// if (!dropoff_plan) +// continue; + +// const auto& final_wp = dropoff_plan->get_waypoints().back(); + +// const auto estimate = final_wp.time(); +// rmf_traffic::agv::Plan::Start finish{ +// estimate, +// *final_wp.graph_index(), +// final_wp.position()[2] +// }; + +// if (estimate < best.time) +// { +// best = DeliveryEstimate{ +// estimate, +// element.first, +// std::move(start.front()), +// std::move(dropoff_start), +// std::move(finish) +// }; +// } +// } + +// if (best.robot) +// return best; + +// return rmf_utils::nullopt; +// } //============================================================================== -void FleetUpdateHandle::Implementation::perform_delivery( - const rmf_task_msgs::msg::Delivery& request, - const DeliveryEstimate& estimate) -{ - auto& mgr = *task_managers.at(estimate.robot); - mgr.queue_task( - tasks::make_delivery( - request, - estimate.robot, - *estimate.pickup_start, - *estimate.dropoff_start), - *estimate.finish); -} +// void FleetUpdateHandle::Implementation::perform_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const DeliveryEstimate& estimate) +// { +// auto& mgr = *task_managers.at(estimate.robot); +// mgr.queue_task( +// tasks::make_delivery( +// request, +// estimate.robot, +// *estimate.pickup_start, +// *estimate.dropoff_start), +// *estimate.finish); +// } //============================================================================== -auto FleetUpdateHandle::Implementation::estimate_loop( - const rmf_task_msgs::msg::Loop& request) const --> rmf_utils::optional -{ - if (request.robot_type != name) - return rmf_utils::nullopt; - - const std::size_t n = request.num_loops; - if (n == 0) - return rmf_utils::nullopt; - - const auto& graph = planner->get_configuration().graph(); - const auto loop_start_wp = graph.find_waypoint(request.start_name); - if (!loop_start_wp) - return rmf_utils::nullopt; - - const auto loop_end_wp = graph.find_waypoint(request.finish_name); - if (!loop_end_wp) - return rmf_utils::nullopt; - - const auto loop_start_goal = - rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); - - const auto loop_end_goal = - rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); - - LoopEstimate best; - for (const auto& element : task_managers) - { - LoopEstimate estimate; - estimate.robot = element.first; - - const auto& mgr = *element.second; - auto start = mgr.expected_finish_location(); - const auto loop_init_plan = planner->plan(start, loop_start_goal); - if (!loop_init_plan) - continue; - - rmf_traffic::Duration init_duration = std::chrono::seconds(0); - if (loop_init_plan->get_waypoints().size() > 1) - { - // If loop_init_plan is not empty, then that means we are not starting at - // the starting point of the loop. Therefore we will need an initial plan - // to reach the first point in the loop. - estimate.init_start = start.front(); - - init_duration = - loop_init_plan->get_waypoints().back().time() - - loop_init_plan->get_waypoints().front().time(); - } - - const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet - { - if (loop_init_plan->get_waypoints().empty()) - return start; - - const auto& loop_init_wp = loop_init_plan->get_waypoints().back(); - assert(loop_init_wp.graph_index()); - return {rmf_traffic::agv::Plan::Start( - loop_init_wp.time(), - *loop_init_wp.graph_index(), - loop_init_wp.position()[2])}; - }(); - - const auto loop_forward_plan = - planner->plan(loop_forward_start, loop_end_goal); - if (!loop_forward_plan) - continue; - - // If the forward plan is empty then that means the start and end of the - // loop are the same, making it a useless request. - // TODO(MXG): We should probably make noise here instead of just ignoring - // the request. - if (loop_forward_plan->get_waypoints().empty()) - return rmf_utils::nullopt; - - estimate.loop_start = loop_forward_start.front(); - - const auto loop_duration = - loop_forward_plan->get_waypoints().back().time() - - loop_forward_plan->get_waypoints().front().time(); - - // We only need to provide this if there is supposed to be more than one - // loop. - const auto& final_wp = loop_forward_plan->get_waypoints().back(); - assert(final_wp.graph_index()); - estimate.loop_end = rmf_traffic::agv::Plan::Start{ - final_wp.time(), - *final_wp.graph_index(), - final_wp.position()[2] - }; - - const auto start_time = [&]() - { - if (loop_init_plan->get_waypoints().empty()) - return loop_forward_plan->get_waypoints().front().time(); - - return loop_init_plan->get_waypoints().front().time(); - }(); - - estimate.time = - start_time + init_duration + (2*n - 1)*loop_duration; - - estimate.loop_end->time(estimate.time); - - if (estimate.time < best.time) - best = std::move(estimate); - } - - if (best.robot) - return best; - - return rmf_utils::nullopt; -} +// auto FleetUpdateHandle::Implementation::estimate_loop( +// const rmf_task_msgs::msg::Loop& request) const +// -> rmf_utils::optional +// { +// if (request.robot_type != name) +// return rmf_utils::nullopt; + +// const std::size_t n = request.num_loops; +// if (n == 0) +// return rmf_utils::nullopt; + +// const auto& graph = planner->get_configuration().graph(); +// const auto loop_start_wp = graph.find_waypoint(request.start_name); +// if (!loop_start_wp) +// return rmf_utils::nullopt; + +// const auto loop_end_wp = graph.find_waypoint(request.finish_name); +// if (!loop_end_wp) +// return rmf_utils::nullopt; + +// const auto loop_start_goal = +// rmf_traffic::agv::Plan::Goal(loop_start_wp->index()); + +// const auto loop_end_goal = +// rmf_traffic::agv::Plan::Goal(loop_end_wp->index()); + +// LoopEstimate best; +// for (const auto& element : task_managers) +// { +// LoopEstimate estimate; +// estimate.robot = element.first; + +// const auto& mgr = *element.second; +// auto start = mgr.expected_finish_location(); +// const auto loop_init_plan = planner->plan(start, loop_start_goal); +// if (!loop_init_plan) +// continue; + +// rmf_traffic::Duration init_duration = std::chrono::seconds(0); +// if (loop_init_plan->get_waypoints().size() > 1) +// { +// // If loop_init_plan is not empty, then that means we are not starting at +// // the starting point of the loop. Therefore we will need an initial plan +// // to reach the first point in the loop. +// estimate.init_start = start.front(); + +// init_duration = +// loop_init_plan->get_waypoints().back().time() +// - loop_init_plan->get_waypoints().front().time(); +// } + +// const auto loop_forward_start = [&]() -> rmf_traffic::agv::Plan::StartSet +// { +// if (loop_init_plan->get_waypoints().empty()) +// return start; + +// const auto& loop_init_wp = loop_init_plan->get_waypoints().back(); +// assert(loop_init_wp.graph_index()); +// return {rmf_traffic::agv::Plan::Start( +// loop_init_wp.time(), +// *loop_init_wp.graph_index(), +// loop_init_wp.position()[2])}; +// }(); + +// const auto loop_forward_plan = +// planner->plan(loop_forward_start, loop_end_goal); +// if (!loop_forward_plan) +// continue; + +// // If the forward plan is empty then that means the start and end of the +// // loop are the same, making it a useless request. +// // TODO(MXG): We should probably make noise here instead of just ignoring +// // the request. +// if (loop_forward_plan->get_waypoints().empty()) +// return rmf_utils::nullopt; + +// estimate.loop_start = loop_forward_start.front(); + +// const auto loop_duration = +// loop_forward_plan->get_waypoints().back().time() +// - loop_forward_plan->get_waypoints().front().time(); + +// // We only need to provide this if there is supposed to be more than one +// // loop. +// const auto& final_wp = loop_forward_plan->get_waypoints().back(); +// assert(final_wp.graph_index()); +// estimate.loop_end = rmf_traffic::agv::Plan::Start{ +// final_wp.time(), +// *final_wp.graph_index(), +// final_wp.position()[2] +// }; + +// const auto start_time = [&]() +// { +// if (loop_init_plan->get_waypoints().empty()) +// return loop_forward_plan->get_waypoints().front().time(); + +// return loop_init_plan->get_waypoints().front().time(); +// }(); + +// estimate.time = +// start_time + init_duration + (2*n - 1)*loop_duration; + +// estimate.loop_end->time(estimate.time); + +// if (estimate.time < best.time) +// best = std::move(estimate); +// } + +// if (best.robot) +// return best; + +// return rmf_utils::nullopt; +// } //============================================================================== -void FleetUpdateHandle::Implementation::perform_loop( - const rmf_task_msgs::msg::Loop& request, - const LoopEstimate& estimate) -{ - auto& mgr = task_managers.at(estimate.robot); - mgr->queue_task( - tasks::make_loop( - request, - estimate.robot, - estimate.init_start, - *estimate.loop_start, - estimate.loop_end), - *estimate.loop_end); -} +// void FleetUpdateHandle::Implementation::perform_loop( +// const rmf_task_msgs::msg::Loop& request, +// const LoopEstimate& estimate) +// { +// auto& mgr = task_managers.at(estimate.robot); +// mgr->queue_task( +// tasks::make_loop( +// request, +// estimate.robot, +// estimate.init_start, +// *estimate.loop_start, +// estimate.loop_end), +// *estimate.loop_end); +// } //============================================================================== void FleetUpdateHandle::add_robot( @@ -381,63 +381,63 @@ FleetUpdateHandle::FleetUpdateHandle() } //============================================================================== -void request_delivery( - const rmf_task_msgs::msg::Delivery& request, - const std::vector>& fleets) -{ - FleetUpdateHandle::Implementation::DeliveryEstimate best; - FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - - for (auto& fleet : fleets) - { - auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); - if (!fimpl.accept_delivery || !fimpl.accept_delivery(request)) - continue; - - const auto estimate = fimpl.estimate_delivery(request); - if (!estimate) - continue; - - if (estimate->time < best.time) - { - best = *estimate; - chosen_fleet = &fimpl; - } - } - - if (!chosen_fleet) - return; - - chosen_fleet->perform_delivery(request, best); -} +// void request_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const std::vector>& fleets) +// { +// FleetUpdateHandle::Implementation::DeliveryEstimate best; +// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; + +// for (auto& fleet : fleets) +// { +// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); +// if (!fimpl.accept_delivery || !fimpl.accept_delivery(request)) +// continue; + +// const auto estimate = fimpl.estimate_delivery(request); +// if (!estimate) +// continue; + +// if (estimate->time < best.time) +// { +// best = *estimate; +// chosen_fleet = &fimpl; +// } +// } + +// if (!chosen_fleet) +// return; + +// chosen_fleet->perform_delivery(request, best); +// } //============================================================================== -void request_loop( - const rmf_task_msgs::msg::Loop& request, - const std::vector>& fleets) -{ - FleetUpdateHandle::Implementation::LoopEstimate best; - FleetUpdateHandle::Implementation* chosen_fleet = nullptr; - - for (auto& fleet : fleets) - { - auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); - const auto estimate = fimpl.estimate_loop(request); - if (!estimate) - continue; - - if (estimate->time < best.time) - { - best = *estimate; - chosen_fleet = &fimpl; - } - } - - if (!chosen_fleet) - return; - - chosen_fleet->perform_loop(request, best); -} +// void request_loop( +// const rmf_task_msgs::msg::Loop& request, +// const std::vector>& fleets) +// { +// FleetUpdateHandle::Implementation::LoopEstimate best; +// FleetUpdateHandle::Implementation* chosen_fleet = nullptr; + +// for (auto& fleet : fleets) +// { +// auto& fimpl = FleetUpdateHandle::Implementation::get(*fleet); +// const auto estimate = fimpl.estimate_loop(request); +// if (!estimate) +// continue; + +// if (estimate->time < best.time) +// { +// best = *estimate; +// chosen_fleet = &fimpl; +// } +// } + +// if (!chosen_fleet) +// return; + +// chosen_fleet->perform_loop(request, best); +// } } // namespace agv 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 f36415a48..a49a0676f 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,6 +20,10 @@ #include +#include +#include +#include + #include #include "Node.hpp" @@ -123,6 +127,18 @@ class FleetUpdateHandle::Implementation AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map> task_managers = {}; + using BidNotice = rmf_task_msgs::msg::BidNotice; + using BidNoticeSub = rclcpp::Subscription::SharedPtr; + BidNoticeSub bid_notice_sub; + + using BidProposal = rmf_task_msgs::msg::BidProposal; + using BidProposalPub = rclcpp::Publisher::SharedPtr; + BidProposalPub bid_proposal_pub; + + using DispatchRequest = rmf_task_msgs::msg::DispatchRequest; + using DispatchRequestSub = rclcpp::Subscription::SharedPtr; + DispatchRequestSub dispatch_request_sub; + template static std::shared_ptr make(Args&&... args) { @@ -132,23 +148,23 @@ class FleetUpdateHandle::Implementation return std::make_shared(std::move(handle)); } - struct DeliveryEstimate - { - rmf_traffic::Time time = rmf_traffic::Time::max(); - RobotContextPtr robot = nullptr; - rmf_utils::optional pickup_start; - rmf_utils::optional dropoff_start; - rmf_utils::optional finish; - }; - - struct LoopEstimate - { - rmf_traffic::Time time = rmf_traffic::Time::max(); - RobotContextPtr robot = nullptr; - rmf_utils::optional init_start; - rmf_utils::optional loop_start; - rmf_utils::optional loop_end; - }; + // struct DeliveryEstimate + // { + // rmf_traffic::Time time = rmf_traffic::Time::max(); + // RobotContextPtr robot = nullptr; + // rmf_utils::optional pickup_start; + // rmf_utils::optional dropoff_start; + // rmf_utils::optional finish; + // }; + + // struct LoopEstimate + // { + // rmf_traffic::Time time = rmf_traffic::Time::max(); + // RobotContextPtr robot = nullptr; + // rmf_utils::optional init_start; + // rmf_utils::optional loop_start; + // rmf_utils::optional loop_end; + // }; static Implementation& get(FleetUpdateHandle& fleet) { @@ -161,30 +177,30 @@ class FleetUpdateHandle::Implementation } // TODO(MXG): Come up with a better design for task dispatch - rmf_utils::optional estimate_delivery( - const rmf_task_msgs::msg::Delivery& request) const; + // rmf_utils::optional estimate_delivery( + // const rmf_task_msgs::msg::Delivery& request) const; - void perform_delivery( - const rmf_task_msgs::msg::Delivery& request, - const DeliveryEstimate& estimate); + // void perform_delivery( + // const rmf_task_msgs::msg::Delivery& request, + // const DeliveryEstimate& estimate); - rmf_utils::optional estimate_loop( - const rmf_task_msgs::msg::Loop& request) const; + // rmf_utils::optional estimate_loop( + // const rmf_task_msgs::msg::Loop& request) const; - void perform_loop( - const rmf_task_msgs::msg::Loop& request, - const LoopEstimate& estimate); + // void perform_loop( + // const rmf_task_msgs::msg::Loop& request, + // const LoopEstimate& estimate); }; //============================================================================== -void request_delivery( - const rmf_task_msgs::msg::Delivery& request, - const std::vector>& fleets); - -//============================================================================== -void request_loop( - const rmf_task_msgs::msg::Loop& request, - const std::vector>& fleets); +// void request_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const std::vector>& fleets); + +// //============================================================================== +// void request_loop( +// const rmf_task_msgs::msg::Loop& request, +// const std::vector>& fleets); } // namespace agv } // namespace rmf_fleet_adapter 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 e90e4f7fa..af906c610 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 @@ -133,13 +133,13 @@ void MockAdapter::stop() //============================================================================== void MockAdapter::request_delivery(const rmf_task_msgs::msg::Delivery& request) { - rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); + // rmf_fleet_adapter::agv::request_delivery(request, _pimpl->fleets); } //============================================================================== void MockAdapter::request_loop(const rmf_task_msgs::msg::Loop& request) { - rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); + // rmf_fleet_adapter::agv::request_loop(request, _pimpl->fleets); } } // namespace test From b6ac6430c2ba336096394e3d83ebbe2d586c6650 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 20 Oct 2020 16:32:23 +0800 Subject: [PATCH 05/50] Updated Adapter::add_fleet() --- rmf_fleet_adapter/CMakeLists.txt | 3 + .../include/rmf_fleet_adapter/agv/Adapter.hpp | 28 ++++++- rmf_fleet_adapter/package.xml | 1 + rmf_fleet_adapter/src/full_control/main.cpp | 74 ++++++++++++++++++- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 10 ++- .../agv/internal_FleetUpdateHandle.hpp | 56 +++++++++++++- .../src/rmf_fleet_adapter/load_param.cpp | 41 ++++++++++ .../src/rmf_fleet_adapter/load_param.hpp | 16 ++++ 8 files changed, 222 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 35c0a16c0..6462d5334 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -37,6 +37,7 @@ set(dep_pkgs rmf_task_msgs rmf_traffic rmf_traffic_ros2 + rmf_battery rmf_task std_msgs ) @@ -59,6 +60,7 @@ add_library(rmf_fleet_adapter SHARED target_link_libraries(rmf_fleet_adapter PUBLIC rmf_traffic_ros2::rmf_traffic_ros2 + rmf_battery::rmf_battery rmf_task::rmf_task yaml-cpp ${rmf_fleet_msgs_LIBRARIES} @@ -80,6 +82,7 @@ target_include_directories(rmf_fleet_adapter ${rmf_fleet_msgs_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} ${rmf_task_msgs_INCLUDE_DIRS} + ${rmf_battery_INCLUDE_DIRS} ${rmf_task_INCLUDE_DIRS} PRIVATE ${rmf_door_msgs_INCLUDE_DIRS} 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 62902e2f9..081fba867 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp @@ -23,6 +23,10 @@ #include #include +#include +#include +#include + #include namespace rmf_fleet_adapter { @@ -94,10 +98,32 @@ 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] battery_system + /// Specify the battery system used by the vehicles in this fleet. + /// + /// \param[in] motion_sink + /// Specify the motion sink that describes the vehicles in this fleet. + /// + /// \param[in] ambient_sink + /// Specify the device sink for ambient sensors used by the vehicles in this fleet. + /// + /// \param[in] tool_sink + /// Specify the device sink for special tools used by the vehicles in this fleet. + /// + /// \param[in] drain_battery + /// If false, battery drain will not be considered when planning for tasks. + /// As a consequence, charging tasks will not be automatically assigned to + /// vehicles in this fleet when battery levels fall below their thresholds. 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::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink = nullptr, + const bool drain_battery = true); /// Create a traffic light to help manage robots that can only support pause /// and resume commands. diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index cf4a4ed60..14c570e08 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -21,6 +21,7 @@ rmf_traffic rmf_traffic_ros2 rmf_traffic_msgs + rmf_battery rmf_task eigen diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index fd1a32100..3105a1096 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -42,6 +42,10 @@ #include #include +#include +#include +#include + #include //============================================================================== @@ -471,8 +475,76 @@ std::shared_ptr make_fleet( for (const auto& key : connections->graph->keys()) std::cout << " -- " << key.first << std::endl; + // Parameters required for task planner + // Battery system + auto battery_system = std::make_shared( + rmf_fleet_adapter::get_battery_system(*node, 24.0, 40.0, 8.8)); + if (!battery_system->valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for battery system"); + + return nullptr; + } + + // Mechanical system and motion_sink + auto mechanical_system = rmf_fleet_adapter::get_mechanical_system( + *node, 70.0, 40.0, 0.22); + if (!mechanical_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for mechanical system"); + + return nullptr; + } + std::shared_ptr motion_sink = + std::make_shared( + *battery_system, mechanical_system); + + // Ambient power system + const double ambient_power_drain = + rmf_fleet_adapter::get_parameter_or_default( + *node, "ambient_power_drain", 20.0); + rmf_battery::agv::PowerSystem ambient_power_system{ + "ambient", ambient_power_drain}; + if (!ambient_power_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for ambient power system"); + + return nullptr; + } + std::shared_ptr ambient_sink = + std::make_shared( + *battery_system, ambient_power_system); + + // Tool power system + const double tool_power_drain = rmf_fleet_adapter::get_parameter_or_default( + *node, "tool_power_drain", 10.0); + rmf_battery::agv::PowerSystem tool_power_system{ + "ambient", tool_power_drain}; + if (!tool_power_system.valid()) + { + RCLCPP_ERROR( + node->get_logger(), + "Invalid values supplied for tool power system"); + + return nullptr; + } + std::shared_ptr tool_sink = + std::make_shared( + *battery_system, tool_power_system); + + // Drain battery + const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( + *node, "drain_battery", true); + connections->fleet = adapter->add_fleet( - fleet_name, *connections->traits, *connections->graph); + fleet_name, *connections->traits, *connections->graph, + battery_system, motion_sink, ambient_sink, tool_sink, drain_battery); // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. 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 dc73081e6..699deec6f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -244,7 +244,12 @@ 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::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + const bool drain_battery) { auto planner = std::make_shared( rmf_traffic::agv::Planner::Configuration( @@ -255,7 +260,8 @@ std::shared_ptr Adapter::add_fleet( auto fleet = FleetUpdateHandle::Implementation::make( fleet_name, std::move(planner), _pimpl->node, _pimpl->worker, _pimpl->writer, _pimpl->mirror_manager.snapshot_handle(), - _pimpl->negotiation); + _pimpl->negotiation, std::move(battery_system), std::move(motion_sink), + std::move(ambient_sink), std::move(tool_sink), drain_battery); _pimpl->fleets.push_back(fleet); return fleet; 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 a49a0676f..ed2f481ab 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 @@ -24,7 +24,10 @@ #include #include +#include + #include +#include #include "Node.hpp" #include "RobotContext.hpp" @@ -120,6 +123,13 @@ class FleetUpdateHandle::Implementation std::shared_ptr writer; std::shared_ptr snappable; std::shared_ptr negotiation; + std::shared_ptr battery_system; + std::shared_ptr motion_sink; + std::shared_ptr ambient_sink; + std::shared_ptr tool_sink; + const bool drain_battery; + + std::shared_ptr task_planner = nullptr; rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); @@ -129,15 +139,15 @@ class FleetUpdateHandle::Implementation using BidNotice = rmf_task_msgs::msg::BidNotice; using BidNoticeSub = rclcpp::Subscription::SharedPtr; - BidNoticeSub bid_notice_sub; + BidNoticeSub bid_notice_sub = nullptr; using BidProposal = rmf_task_msgs::msg::BidProposal; using BidProposalPub = rclcpp::Publisher::SharedPtr; - BidProposalPub bid_proposal_pub; + BidProposalPub bid_proposal_pub = nullptr; using DispatchRequest = rmf_task_msgs::msg::DispatchRequest; using DispatchRequestSub = rclcpp::Subscription::SharedPtr; - DispatchRequestSub dispatch_request_sub; + DispatchRequestSub dispatch_request_sub = nullptr; template static std::shared_ptr make(Args&&... args) @@ -145,6 +155,46 @@ class FleetUpdateHandle::Implementation FleetUpdateHandle handle; handle._pimpl = rmf_utils::make_unique_impl( Implementation{std::forward(args)...}); + + // Setup the task planner + std::shared_ptr task_config = + std::make_shared( + *handle._pimpl->battery_system, + handle._pimpl->motion_sink, + handle._pimpl->ambient_sink, + handle._pimpl->planner); + + handle._pimpl->task_planner = std::make_shared( + task_config); + + // Create subs and pubs for bidding + auto default_qos = rclcpp::SystemDefaultsQoS(); + + // Publish BidProposal + handle._pimpl->bid_proposal_pub = + handle._pimpl->node->create_publisher( + BidProposalTopicName, default_qos); + + // Subscribe BidNotice + handle._pimpl->bid_notice_sub = + handle._pimpl->node->create_subscription( + BidNoticeTopicName, + default_qos, + [&](const BidNotice::SharedPtr msg) + { + // TODO(YV) + }); + + // Subscribe DispatchRequest + handle._pimpl->dispatch_request_sub = + handle._pimpl->node->create_subscription( + DispatchRequestTopicName, + default_qos, + [&](const DispatchRequest::SharedPtr msg) + { + // TODO(YV) + }); + return std::make_shared(std::move(handle)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp index 735783d30..16caa24aa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.cpp @@ -88,4 +88,45 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node, return traits; } + +//============================================================================== +rmf_battery::agv::BatterySystem get_battery_system( + rclcpp::Node& node, + const double default_voltage, + const double default_capacity, + const double default_charging_current) +{ + const double voltage = + get_parameter_or_default(node, "battery_voltage", default_voltage); + const double capacity = + get_parameter_or_default(node, "battery_capacity", default_capacity); + const double charging_current = + get_parameter_or_default( + node, "battery_charging_current", default_charging_current); + + rmf_battery::agv::BatterySystem battery_system{ + voltage, capacity, charging_current}; + + return battery_system; +} + +rmf_battery::agv::MechanicalSystem get_mechanical_system( + rclcpp::Node& node, + const double default_mass, + const double default_inertia, + const double default_friction) +{ + const double mass = + get_parameter_or_default(node, "mass", default_mass); + const double inertia = + get_parameter_or_default(node, "inertia", default_inertia); + const double friction = + get_parameter_or_default(node, "friction_coefficient", default_friction); + + rmf_battery::agv::MechanicalSystem mechanical_system{ + mass, inertia, friction}; + + return mechanical_system; +} + } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp index 6df2349b9..dafda60ff 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/load_param.hpp @@ -20,6 +20,9 @@ #include +#include +#include + #include #include @@ -60,6 +63,19 @@ rmf_traffic::agv::VehicleTraits get_traits_or_default( const double default_a_nom, const double default_alpha_nom, const double default_r_f, const double default_r_v); +//============================================================================== +rmf_battery::agv::BatterySystem get_battery_system( + rclcpp::Node& node, + const double default_voltage, + const double default_capacity, + const double default_charging_current); + +rmf_battery::agv::MechanicalSystem get_mechanical_system( + rclcpp::Node& node, + const double default_mass, + const double default_inertia, + const double default_friction); + } // namespace rmf_fleet_adapter #endif // SRC__RMF_FLEET_ADAPTER__LOAD_PARAM_HPP From 157a9170a460b565e7ba53703f493a065785adc8 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 20 Oct 2020 18:42:54 +0800 Subject: [PATCH 06/50] Undid changes to add_fleet which broke API/ABI. Added set_task_planner_params() to FleetUpdateHandle --- .../include/rmf_fleet_adapter/agv/Adapter.hpp | 28 +--------------- .../agv/FleetUpdateHandle.hpp | 31 +++++++++++++++++ rmf_fleet_adapter/src/full_control/main.cpp | 16 +++++++-- .../src/rmf_fleet_adapter/agv/Adapter.cpp | 10 ++---- .../agv/FleetUpdateHandle.cpp | 33 +++++++++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 23 ++++--------- 6 files changed, 87 insertions(+), 54 deletions(-) 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 081fba867..62902e2f9 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/Adapter.hpp @@ -23,10 +23,6 @@ #include #include -#include -#include -#include - #include namespace rmf_fleet_adapter { @@ -98,32 +94,10 @@ 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] battery_system - /// Specify the battery system used by the vehicles in this fleet. - /// - /// \param[in] motion_sink - /// Specify the motion sink that describes the vehicles in this fleet. - /// - /// \param[in] ambient_sink - /// Specify the device sink for ambient sensors used by the vehicles in this fleet. - /// - /// \param[in] tool_sink - /// Specify the device sink for special tools used by the vehicles in this fleet. - /// - /// \param[in] drain_battery - /// If false, battery drain will not be considered when planning for tasks. - /// As a consequence, charging tasks will not be automatically assigned to - /// vehicles in this fleet when battery levels fall below their thresholds. std::shared_ptr add_fleet( const std::string& fleet_name, rmf_traffic::agv::VehicleTraits traits, - rmf_traffic::agv::Graph navigation_graph, - std::shared_ptr battery_system, - std::shared_ptr motion_sink, - std::shared_ptr ambient_sink, - std::shared_ptr tool_sink = nullptr, - const bool drain_battery = true); + rmf_traffic::agv::Graph navigation_graph); /// 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 4e195e75c..0d1d6de4b 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -23,6 +23,10 @@ #include +#include +#include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -63,6 +67,33 @@ class FleetUpdateHandle : public std::enable_shared_from_this rmf_traffic::agv::Plan::StartSet start, std::function handle)> handle_cb); + /// Set the parameters required for task planning + /// + /// \param[in] battery_system + /// Specify the battery system used by the vehicles in this fleet. + /// + /// \param[in] motion_sink + /// Specify the motion sink that describes the vehicles in this fleet. + /// + /// \param[in] ambient_sink + /// Specify the device sink for ambient sensors used by the vehicles in this fleet. + /// + /// \param[in] tool_sink + /// Specify the device sink for special tools used by the vehicles in this fleet. + /// + /// \param[in] drain_battery + /// If false, battery drain will not be considered when planning for tasks. + /// As a consequence, charging tasks will not be automatically assigned to + /// vehicles in this fleet when battery levels fall below their thresholds. + /// + /// \return true if task planner parameters were successfully updated. + bool set_task_planner_params( + std::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + const bool drain_battery); + /// A callback function that evaluates whether a fleet will accept a delivery /// request. /// diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 3105a1096..3f8a8faf7 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -475,6 +475,10 @@ std::shared_ptr make_fleet( for (const auto& key : connections->graph->keys()) std::cout << " -- " << key.first << std::endl; + + connections->fleet = adapter->add_fleet( + fleet_name, *connections->traits, *connections->graph); + // Parameters required for task planner // Battery system auto battery_system = std::make_shared( @@ -542,9 +546,15 @@ std::shared_ptr make_fleet( const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( *node, "drain_battery", true); - connections->fleet = adapter->add_fleet( - fleet_name, *connections->traits, *connections->graph, - battery_system, motion_sink, ambient_sink, tool_sink, drain_battery); + if (!connections->fleet->set_task_planner_params( + battery_system, motion_sink, ambient_sink, tool_sink, drain_battery)) + { + RCLCPP_ERROR( + node->get_logger(), + "Failed to initialize task planner parameters"); + + return nullptr; + } // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. 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 699deec6f..dc73081e6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -244,12 +244,7 @@ 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, - std::shared_ptr battery_system, - std::shared_ptr motion_sink, - std::shared_ptr ambient_sink, - std::shared_ptr tool_sink, - const bool drain_battery) + rmf_traffic::agv::Graph navigation_graph) { auto planner = std::make_shared( rmf_traffic::agv::Planner::Configuration( @@ -260,8 +255,7 @@ std::shared_ptr Adapter::add_fleet( auto fleet = FleetUpdateHandle::Implementation::make( fleet_name, std::move(planner), _pimpl->node, _pimpl->worker, _pimpl->writer, _pimpl->mirror_manager.snapshot_handle(), - _pimpl->negotiation, std::move(battery_system), std::move(motion_sink), - std::move(ambient_sink), std::move(tool_sink), drain_battery); + _pimpl->negotiation); _pimpl->fleets.push_back(fleet); return fleet; 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 ed3357b72..a233f3aef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -374,6 +374,39 @@ FleetUpdateHandle::default_maximum_delay() const return _pimpl->default_maximum_delay; } +bool FleetUpdateHandle::set_task_planner_params( + std::shared_ptr battery_system, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr tool_sink, + const bool drain_battery) +{ + if (battery_system && motion_sink && ambient_sink && tool_sink) + { + + _pimpl->battery_system = battery_system; + _pimpl->motion_sink = motion_sink; + _pimpl->ambient_sink = ambient_sink; + _pimpl->tool_sink = tool_sink; + + std::shared_ptr task_config = + std::make_shared( + *battery_system, + motion_sink, + ambient_sink, + _pimpl->planner); + + _pimpl->task_planner = std::make_shared( + task_config); + + _pimpl->initialized_task_planner = true; + + return _pimpl->initialized_task_planner; + } + + return false; +} + //============================================================================== FleetUpdateHandle::FleetUpdateHandle() { 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 ed2f481ab..c801ce683 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 @@ -123,13 +123,15 @@ class FleetUpdateHandle::Implementation std::shared_ptr writer; std::shared_ptr snappable; std::shared_ptr negotiation; - std::shared_ptr battery_system; - std::shared_ptr motion_sink; - std::shared_ptr ambient_sink; - std::shared_ptr tool_sink; - const bool drain_battery; + // Task planner params + std::shared_ptr battery_system = nullptr; + std::shared_ptr motion_sink = nullptr; + std::shared_ptr ambient_sink = nullptr; + std::shared_ptr tool_sink = nullptr; + const bool drain_battery = true; std::shared_ptr task_planner = nullptr; + bool initialized_task_planner = false; rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); @@ -156,17 +158,6 @@ class FleetUpdateHandle::Implementation handle._pimpl = rmf_utils::make_unique_impl( Implementation{std::forward(args)...}); - // Setup the task planner - std::shared_ptr task_config = - std::make_shared( - *handle._pimpl->battery_system, - handle._pimpl->motion_sink, - handle._pimpl->ambient_sink, - handle._pimpl->planner); - - handle._pimpl->task_planner = std::make_shared( - task_config); - // Create subs and pubs for bidding auto default_qos = rclcpp::SystemDefaultsQoS(); From 83cdcd2ee5ec1d4742942da0b28fe5b5dc075243 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 21 Oct 2020 18:12:39 +0800 Subject: [PATCH 07/50] Added accept_task_request to FleetUpdateHandle --- rmf_fleet_adapter/CMakeLists.txt | 2 ++ .../agv/FleetUpdateHandle.hpp | 26 +++++++++++++++++++ rmf_fleet_adapter/src/full_control/main.cpp | 26 +++++++++++++++++++ .../agv/FleetUpdateHandle.cpp | 8 ++++++ .../agv/internal_FleetUpdateHandle.hpp | 2 ++ 5 files changed, 64 insertions(+) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 6462d5334..15b83f8ba 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -164,11 +164,13 @@ target_link_libraries(full_control PRIVATE rmf_fleet_adapter ${rmf_fleet_msgs_LIBRARIES} + ${rmf_task_msgs_LIBRARIES} ) target_include_directories(full_control PRIVATE ${rmf_fleet_msgs_INCLUDE_DIRS} + ${rmf_task_msgs_INCLUDE_DIRS} ) # ----------------------------------------------------------------------------- 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 0d1d6de4b..3e32e97b1 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -94,6 +95,28 @@ class FleetUpdateHandle : public std::enable_shared_from_this std::shared_ptr tool_sink, const bool drain_battery); + + /// A callback function that evaluates whether a fleet will accept a task + /// request + /// + /// \param[in] request + /// Information about the task request that is being considered. + /// + /// \return true to indicate that this fleet should accept the request, false + /// to reject the request. + using AcceptTaskRequest = + std::function; + + /// Provide a callback that indicates whether this fleet will accept a + /// BidNotice request. By default all requests will be rejected. + /// + /// \note The callback function that you give should ideally be non-blocking + /// and return quickly. It's meant to check whether this fleet's vehicles are + /// compatible with the requested payload, pickup, and dropoff behavior + /// settings. The path planning feasibility will be taken care of by the + /// adapter internally. + FleetUpdateHandle& accept_task_requests(AcceptTaskRequest check); + /// A callback function that evaluates whether a fleet will accept a delivery /// request. /// @@ -102,6 +125,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this /// /// \return true to indicate that this fleet should accept the request, false /// to reject the request. + /// + /// \note This interface will be deprecated. Use the more general + /// AcceptTaskRequest callback using AcceptDeliveryRequest = std::function; diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 3f8a8faf7..88047e332 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -31,6 +31,10 @@ #include #include +// RMF Task messages +#include +#include + // ROS2 utilities for rmf_traffic #include @@ -47,6 +51,7 @@ #include #include +#include //============================================================================== class FleetDriverRobotCommandHandle @@ -556,14 +561,35 @@ std::shared_ptr make_fleet( return nullptr; } + std::unordered_set task_types; + if (node->declare_parameter("perform_loop", true)) + { + task_types.insert(rmf_task_msgs::msg::TaskType::LOOP_TASK); + } + // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. if (node->declare_parameter("perform_deliveries", false)) { + task_types.insert(rmf_task_msgs::msg::TaskType::DELIVERY_TASK); connections->fleet->accept_delivery_requests( [](const rmf_task_msgs::msg::Delivery&){ return true; }); } + if (node->declare_parameter("perform_cleaning", true)) + { + task_types.insert(rmf_task_msgs::msg::TaskType::CLEANING_TASK); + } + + connections->fleet->accept_task_requests( + [task_types](const rmf_task_msgs::msg::TaskProfile& msg) + { + if (task_types.find(msg.type.value) != task_types.end()) + return true; + + return false; + }); + 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/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index a233f3aef..a69686939 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -351,6 +351,14 @@ void FleetUpdateHandle::add_robot( }); } +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::accept_task_requests( + AcceptTaskRequest check) +{ + _pimpl->accept_task = std::move(check); + return *this; +} + //============================================================================== FleetUpdateHandle& FleetUpdateHandle::accept_delivery_requests( AcceptDeliveryRequest check) 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 c801ce683..351583390 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 @@ -139,6 +139,8 @@ class FleetUpdateHandle::Implementation AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map> task_managers = {}; + AcceptTaskRequest accept_task = nullptr; + using BidNotice = rmf_task_msgs::msg::BidNotice; using BidNoticeSub = rclcpp::Subscription::SharedPtr; BidNoticeSub bid_notice_sub = nullptr; From af3e9d4a66e02403ea85fdccb4f394794076a500 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 22 Oct 2020 13:53:53 +0800 Subject: [PATCH 08/50] Added BidNotice callback --- rmf_fleet_adapter/src/full_control/main.cpp | 16 +++-- .../agv/internal_FleetUpdateHandle.hpp | 71 ++++++++++++++++++- 2 files changed, 80 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 88047e332..0e048da49 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -581,13 +581,19 @@ std::shared_ptr make_fleet( task_types.insert(rmf_task_msgs::msg::TaskType::CLEANING_TASK); } + // connections->fleet->accept_task_requests( + // [task_types](const rmf_task_msgs::msg::TaskProfile& msg) + // { + // if (task_types.find(msg.type.value) != task_types.end()) + // return true; + + // return false; + // }); + connections->fleet->accept_task_requests( - [task_types](const rmf_task_msgs::msg::TaskProfile& msg) + [](const rmf_task_msgs::msg::TaskProfile& msg) { - if (task_types.find(msg.type.value) != task_types.end()) - return true; - - return false; + return true; }); if (node->declare_parameter("disable_delay_threshold", false)) 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 351583390..8b436f999 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 @@ -38,6 +38,8 @@ #include #include +#include + namespace rmf_fleet_adapter { namespace agv { @@ -140,7 +142,7 @@ class FleetUpdateHandle::Implementation std::unordered_map> task_managers = {}; AcceptTaskRequest accept_task = nullptr; - + using BidNotice = rmf_task_msgs::msg::BidNotice; using BidNoticeSub = rclcpp::Subscription::SharedPtr; BidNoticeSub bid_notice_sub = nullptr; @@ -175,7 +177,7 @@ class FleetUpdateHandle::Implementation default_qos, [&](const BidNotice::SharedPtr msg) { - // TODO(YV) + handle._pimpl->bid_notice_cb(msg); }); // Subscribe DispatchRequest @@ -209,6 +211,71 @@ class FleetUpdateHandle::Implementation // rmf_utils::optional loop_end; // }; + void bid_notice_cb(const BidNotice::SharedPtr msg) + { + std::cout << "Here 0" << std::endl; + if (!accept_task) + { + std::cout << "Here 1" << std::endl; + 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()); + + std::cout << "Here 2" << std::endl; + return; + } + + std::cout << "Here 3" << std::endl; + + if (!accept_task(msg->task_profile)) + { + std::cout << "Here 4" << std::endl; + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] is configured to not accept request [%s]", + name.c_str(), + msg->task_profile.task_id.c_str()); + + std::cout << "Here 6" << std::endl; + return; + } + + std::cout << "Here 7" << std::endl; + + if (!task_planner + || !initialized_task_planner) + { + std::cout << "Here 8" << std::endl; + + 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()); + + std::cout << "Here 9" << std::endl; + + return; + } + + std::cout << "Here 10" << std::endl; + + // TODO(YV) + // Determine task type and convert to request pointer + + // Combine new request ptr with request ptr of tasks in task manager queues + + // Update robot states + + // Generate new task assignments while accommodating for the new + // request + // Call greedy_plan but run optimal_plan() in a separate thread + + // Store results in internal map and publish BidProposal + } + static Implementation& get(FleetUpdateHandle& fleet) { return *fleet._pimpl; From 5156445a92d8789bb05311a46857857094c9fae9 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 22 Oct 2020 16:28:13 +0800 Subject: [PATCH 09/50] Updated bidding topic names and started processing clean task request --- .../rmf_fleet_adapter/StandardNames.hpp | 6 +- rmf_fleet_adapter/src/full_control/main.cpp | 16 +--- .../agv/internal_FleetUpdateHandle.hpp | 94 +++++++++++++++---- 3 files changed, 83 insertions(+), 33 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 4962d398b..d361e723d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -49,9 +49,9 @@ const std::string DeliveryTopicName = "delivery_requests"; const std::string LoopRequestTopicName = "loop_requests"; const std::string TaskSummaryTopicName = "task_summaries"; -const std::string BidNoticeTopicName = "bid_notice"; -const std::string BidProposalTopicName = "bid_proposal"; -const std::string DispatchRequestTopicName = "dispatch_request"; +const std::string BidNoticeTopicName = "rmf_task/bid_notice"; +const std::string BidProposalTopicName = "rmf_task/bid_proposal"; +const std::string DispatchRequestTopicName = "rmf_task/dispatch_request"; } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 0e048da49..88047e332 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -581,19 +581,13 @@ std::shared_ptr make_fleet( task_types.insert(rmf_task_msgs::msg::TaskType::CLEANING_TASK); } - // connections->fleet->accept_task_requests( - // [task_types](const rmf_task_msgs::msg::TaskProfile& msg) - // { - // if (task_types.find(msg.type.value) != task_types.end()) - // return true; - - // return false; - // }); - connections->fleet->accept_task_requests( - [](const rmf_task_msgs::msg::TaskProfile& msg) + [task_types](const rmf_task_msgs::msg::TaskProfile& msg) { - return true; + if (task_types.find(msg.type.value) != task_types.end()) + return true; + + return false; }); if (node->declare_parameter("disable_delay_threshold", false)) 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 8b436f999..6589205d4 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 @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include @@ -37,6 +39,7 @@ #include #include +#include #include @@ -141,6 +144,11 @@ class FleetUpdateHandle::Implementation AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map> task_managers = {}; + // map task id to pair of + using Assignments = rmf_task::agv::TaskPlanner::Assignments; + std::unordered_map> task_map = {}; + AcceptTaskRequest accept_task = nullptr; using BidNotice = rmf_task_msgs::msg::BidNotice; @@ -175,9 +183,9 @@ class FleetUpdateHandle::Implementation handle._pimpl->node->create_subscription( BidNoticeTopicName, default_qos, - [&](const BidNotice::SharedPtr msg) + [p = handle._pimpl.get()](const BidNotice::SharedPtr msg) { - handle._pimpl->bid_notice_cb(msg); + p->bid_notice_cb(msg); }); // Subscribe DispatchRequest @@ -185,7 +193,7 @@ class FleetUpdateHandle::Implementation handle._pimpl->node->create_subscription( DispatchRequestTopicName, default_qos, - [&](const DispatchRequest::SharedPtr msg) + [p = handle._pimpl.get()](const DispatchRequest::SharedPtr msg) { // TODO(YV) }); @@ -213,58 +221,106 @@ class FleetUpdateHandle::Implementation void bid_notice_cb(const BidNotice::SharedPtr msg) { - std::cout << "Here 0" << std::endl; if (!accept_task) { - std::cout << "Here 1" << std::endl; 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()); - std::cout << "Here 2" << std::endl; return; } - std::cout << "Here 3" << std::endl; - if (!accept_task(msg->task_profile)) { - std::cout << "Here 4" << std::endl; RCLCPP_INFO( node->get_logger(), - "Fleet [%s] is configured to not accept request [%s]", + "Fleet [%s] is configured to not accept task [%s]", name.c_str(), msg->task_profile.task_id.c_str()); - std::cout << "Here 6" << std::endl; return; } - std::cout << "Here 7" << std::endl; - if (!task_planner || !initialized_task_planner) { - std::cout << "Here 8" << std::endl; - 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()); - std::cout << "Here 9" << std::endl; - return; } - std::cout << "Here 10" << std::endl; - // TODO(YV) // Determine task type and convert to request pointer + rmf_task::RequestPtr request; + const auto& task_profile = msg->task_profile; + const auto& task_type = task_profile.type; + const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.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(); + + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] is processing BidNotice with task_id:[%s] and type:[%d]...", + name.c_str(), id.c_str(), task_type.value); + + // Process Cleaning task + if (task_type.value == rmf_task_msgs::msg::TaskType::CLEANING_TASK) + { + if (task_profile.params.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [zone] missing in TaskProfile. Rejecting BidNotice " + " with task_id:[%s]" , id.c_str()); + + return; + } + const std::string start_wp_name = task_profile.params[0].value; + const auto start_wp = graph.find_waypoint(start_wp_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(), start_wp_name.c_str(), id.c_str()); + + return; + } + + + + + } + else if (task_type.value == rmf_task_msgs::msg::TaskType::DELIVERY_TASK) + { + + } + else if (task_type.value == rmf_task_msgs::msg::TaskType::LOOP_TASK) + { + + } + else + { + RCLCPP_INFO( + node->get_logger(), + "Invalid TaskType in TaskProfile. Rejecting BidNotice with task_id:[%s]", + id.c_str()); + + return; + } + + + + // } // Combine new request ptr with request ptr of tasks in task manager queues // Update robot states From 106e157b0e438a07dcb4f4ef6703e9898c956171 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 22 Oct 2020 18:07:34 +0800 Subject: [PATCH 10/50] Added dock related messages to rmf_fleet_msgs. Finished processing clean request. --- .../rmf_fleet_adapter/StandardNames.hpp | 2 + .../agv/internal_FleetUpdateHandle.hpp | 103 +++++++++++++++++- rmf_fleet_msgs/CMakeLists.txt | 3 + rmf_fleet_msgs/msg/Dock.msg | 2 + rmf_fleet_msgs/msg/DockParameter.msg | 8 ++ rmf_fleet_msgs/msg/DockSummary.msg | 1 + 6 files changed, 114 insertions(+), 5 deletions(-) create mode 100644 rmf_fleet_msgs/msg/Dock.msg create mode 100644 rmf_fleet_msgs/msg/DockParameter.msg create mode 100644 rmf_fleet_msgs/msg/DockSummary.msg diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index d361e723d..df718cab3 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -53,6 +53,8 @@ const std::string BidNoticeTopicName = "rmf_task/bid_notice"; const std::string BidProposalTopicName = "rmf_task/bid_proposal"; const std::string DispatchRequestTopicName = "rmf_task/dispatch_request"; +const std::string DockSummaryTopicName = "dock_summary"; + } // namespace rmf_fleet_adapter #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP 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 6589205d4..352a06c3f 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 @@ -28,6 +28,8 @@ #include #include +#include + #include #include @@ -36,6 +38,8 @@ #include "../TaskManager.hpp" #include +#include +#include #include #include @@ -144,11 +148,15 @@ class FleetUpdateHandle::Implementation AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map> task_managers = {}; - // map task id to pair of + // Map task id to pair of using Assignments = rmf_task::agv::TaskPlanner::Assignments; std::unordered_map> task_map = {}; + // Map of dock name to dock parameters + std::unordered_map dock_param_map = {}; + AcceptTaskRequest accept_task = nullptr; using BidNotice = rmf_task_msgs::msg::BidNotice; @@ -163,6 +171,10 @@ class FleetUpdateHandle::Implementation using DispatchRequestSub = rclcpp::Subscription::SharedPtr; DispatchRequestSub dispatch_request_sub = nullptr; + using DockSummary = rmf_fleet_msgs::msg::DockSummary; + using DockSummarySub = rclcpp::Subscription::SharedPtr; + DockSummarySub dock_summary_sub = nullptr; + template static std::shared_ptr make(Args&&... args) { @@ -172,6 +184,7 @@ class FleetUpdateHandle::Implementation // Create subs and pubs for bidding auto default_qos = rclcpp::SystemDefaultsQoS(); + auto transient_qos = default_qos; transient_qos.transient_local(); // Publish BidProposal handle._pimpl->bid_proposal_pub = @@ -198,6 +211,16 @@ class FleetUpdateHandle::Implementation // TODO(YV) }); + // Subscribe DockSummary + handle._pimpl->dock_summary_sub = + handle._pimpl->node->create_subscription( + DockSummaryTopicName, + transient_qos, + [p = handle._pimpl.get()](const DockSummary::SharedPtr msg) + { + p->dock_summary_cb(msg); + }); + return std::make_shared(std::move(handle)); } @@ -219,6 +242,22 @@ class FleetUpdateHandle::Implementation // rmf_utils::optional loop_end; // }; + void dock_summary_cb(const DockSummary::SharedPtr msg) + { + for (const auto& dock : msg->docks) + { + if (dock.fleet_name == name) + { + dock_param_map.clear(); + for (const auto& param : dock.params) + dock_param_map.insert({param.start, param}); + break; + } + } + + return; + } + void bid_notice_cb(const BidNotice::SharedPtr msg) { if (!accept_task) @@ -255,9 +294,8 @@ class FleetUpdateHandle::Implementation return; } - // TODO(YV) // Determine task type and convert to request pointer - rmf_task::RequestPtr request; + rmf_task::RequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; const auto& task_type = task_profile.type; const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.start_time); @@ -282,6 +320,8 @@ class FleetUpdateHandle::Implementation return; } + + // Check for valid start waypoint const std::string start_wp_name = task_profile.params[0].value; const auto start_wp = graph.find_waypoint(start_wp_name); if (!start_wp) @@ -295,10 +335,63 @@ class FleetUpdateHandle::Implementation return; } + // 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 + const std::string finish_wp_name = clean_param.finish; + const auto finish_wp = graph.find_waypoint(finish_wp_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(), finish_wp_name.c_str(), id.c_str()); + return; + } + // 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); + + // TODO(YV) get rid of id field in RequestPtr + std::stringstream id_stream(id); + std::size_t request_id; + id_stream >> request_id; + + new_request = rmf_task::requests::Clean::make( + request_id, + start_wp->index(), + finish_wp->index(), + cleaning_trajectory, + motion_sink, + ambient_sink, + tool_sink, + planner, + start_time, + drain_battery); } + + // TODO(YV) else if (task_type.value == rmf_task_msgs::msg::TaskType::DELIVERY_TASK) { @@ -318,9 +411,9 @@ class FleetUpdateHandle::Implementation } - + if (!new_request) + return; - // } // Combine new request ptr with request ptr of tasks in task manager queues // Update robot states diff --git a/rmf_fleet_msgs/CMakeLists.txt b/rmf_fleet_msgs/CMakeLists.txt index 4a20f5222..831bca927 100644 --- a/rmf_fleet_msgs/CMakeLists.txt +++ b/rmf_fleet_msgs/CMakeLists.txt @@ -25,6 +25,9 @@ set(msg_files "msg/DestinationRequest.msg" "msg/PathRequest.msg" "msg/ModeParameter.msg" + "msg/DockParameter.msg" + "msg/Dock.msg" + "msg/DockSummary.msg" ) # set(srv_files diff --git a/rmf_fleet_msgs/msg/Dock.msg b/rmf_fleet_msgs/msg/Dock.msg new file mode 100644 index 000000000..05837cd2b --- /dev/null +++ b/rmf_fleet_msgs/msg/Dock.msg @@ -0,0 +1,2 @@ +string fleet_name +DockParameter[] params \ No newline at end of file diff --git a/rmf_fleet_msgs/msg/DockParameter.msg b/rmf_fleet_msgs/msg/DockParameter.msg new file mode 100644 index 000000000..42c017b94 --- /dev/null +++ b/rmf_fleet_msgs/msg/DockParameter.msg @@ -0,0 +1,8 @@ +# The name of the waypoint where the docking begins +string start + +# The name of the waypoint where the docking ends +string finish + +# The points in the docking path +Location[] path \ No newline at end of file diff --git a/rmf_fleet_msgs/msg/DockSummary.msg b/rmf_fleet_msgs/msg/DockSummary.msg new file mode 100644 index 000000000..1d4dea1c1 --- /dev/null +++ b/rmf_fleet_msgs/msg/DockSummary.msg @@ -0,0 +1 @@ +Dock[] docks \ No newline at end of file From 79617ac0b690963b0a16ad0da9e63b6f9b760343 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 23 Oct 2020 16:48:22 +0800 Subject: [PATCH 11/50] Moved BidNotice callback into FleetUpdateHandle.cpp --- .../agv/FleetUpdateHandle.cpp | 200 ++++++++++++++++++ .../agv/internal_FleetUpdateHandle.hpp | 183 +--------------- 2 files changed, 202 insertions(+), 181 deletions(-) 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 a69686939..d863e88bf 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -60,6 +60,206 @@ class LiaisonNegotiator : public rmf_traffic::schedule::Negotiator }; } // anonymous namespace +//============================================================================== +void FleetUpdateHandle::Implementation::dock_summary_cb( + const DockSummary::SharedPtr msg) +{ + for (const auto& dock : msg->docks) + { + if (dock.fleet_name == name) + { + dock_param_map.clear(); + for (const auto& param : dock.params) + dock_param_map.insert({param.start, param}); + break; + } + } + + return; +} + +//============================================================================== +void FleetUpdateHandle::Implementation::bid_notice_cb( + const BidNotice::SharedPtr msg) +{ + if (task_managers.empty()) + return; + + if (!accept_task) + { + 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; + } + + if (!accept_task(msg->task_profile)) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] is configured to not accept task [%s]", + name.c_str(), + msg->task_profile.task_id.c_str()); + + return; + } + + if (!task_planner + || !initialized_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; + } + + // Determine task type and convert to request pointer + rmf_task::RequestPtr new_request = nullptr; + const auto& task_profile = msg->task_profile; + const auto& task_type = task_profile.type; + const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.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(); + + // RCLCPP_INFO( + // node->get_logger(), + // "Fleet [%s] is processing BidNotice with task_id:[%s] and type:[%d]...", + // name.c_str(), id.c_str(), task_type.value); + + // Process Cleaning task + if (task_type.value == rmf_task_msgs::msg::TaskType::CLEANING_TASK) + { + if (task_profile.params.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [zone] missing in TaskProfile. Rejecting BidNotice " + " with task_id:[%s]" , id.c_str()); + + return; + } + + // Check for valid start waypoint + const std::string start_wp_name = task_profile.params[0].value; + const auto start_wp = graph.find_waypoint(start_wp_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(), start_wp_name.c_str(), id.c_str()); + + return; + } + + // 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 + const std::string finish_wp_name = clean_param.finish; + const auto finish_wp = graph.find_waypoint(finish_wp_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(), finish_wp_name.c_str(), id.c_str()); + + return; + } + + // 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); + + // TODO(YV) get rid of id field in RequestPtr + std::stringstream id_stream(id); + std::size_t request_id; + id_stream >> request_id; + + new_request = rmf_task::requests::Clean::make( + request_id, + start_wp->index(), + finish_wp->index(), + cleaning_trajectory, + motion_sink, + ambient_sink, + tool_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Clean request"); + } + + else if (task_type.value == rmf_task_msgs::msg::TaskType::DELIVERY_TASK) + { + // TODO(YV) + } + else if (task_type.value == rmf_task_msgs::msg::TaskType::LOOP_TASK) + { + // TODO(YV) + } + else + { + RCLCPP_INFO( + node->get_logger(), + "Invalid TaskType in TaskProfile. Rejecting BidNotice with task_id:[%s]", + id.c_str()); + + return; + } + + if (!new_request) + return; + + // Combine new request ptr with request ptr of tasks in task manager queues + std::vector pending_requests; + pending_requests.push_back(new_request); + for (const auto& t : task_managers) + { + const auto requests = t.second->requests(); + pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); + } + + // Update robot states + RCLCPP_INFO(node->get_logger(), "Planning for [%d] request(s)", pending_requests.size()); + + // Generate new task assignments while accommodating for the new + // request + // Call greedy_plan but run optimal_plan() in a separate thread + + // Store results in internal map and publish BidProposal +} + //============================================================================== // auto FleetUpdateHandle::Implementation::estimate_delivery( // const rmf_task_msgs::msg::Delivery& request) const 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 352a06c3f..1b5add7a7 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 @@ -242,188 +242,9 @@ class FleetUpdateHandle::Implementation // rmf_utils::optional loop_end; // }; - void dock_summary_cb(const DockSummary::SharedPtr msg) - { - for (const auto& dock : msg->docks) - { - if (dock.fleet_name == name) - { - dock_param_map.clear(); - for (const auto& param : dock.params) - dock_param_map.insert({param.start, param}); - break; - } - } - - return; - } - - void bid_notice_cb(const BidNotice::SharedPtr msg) - { - if (!accept_task) - { - 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; - } - - if (!accept_task(msg->task_profile)) - { - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] is configured to not accept task [%s]", - name.c_str(), - msg->task_profile.task_id.c_str()); - - return; - } - - if (!task_planner - || !initialized_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; - } - - // Determine task type and convert to request pointer - rmf_task::RequestPtr new_request = nullptr; - const auto& task_profile = msg->task_profile; - const auto& task_type = task_profile.type; - const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.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(); - - RCLCPP_INFO( - node->get_logger(), - "Fleet [%s] is processing BidNotice with task_id:[%s] and type:[%d]...", - name.c_str(), id.c_str(), task_type.value); - - // Process Cleaning task - if (task_type.value == rmf_task_msgs::msg::TaskType::CLEANING_TASK) - { - if (task_profile.params.empty()) - { - RCLCPP_INFO( - node->get_logger(), - "Required param [zone] missing in TaskProfile. Rejecting BidNotice " - " with task_id:[%s]" , id.c_str()); - - return; - } - - // Check for valid start waypoint - const std::string start_wp_name = task_profile.params[0].value; - const auto start_wp = graph.find_waypoint(start_wp_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(), start_wp_name.c_str(), id.c_str()); - - return; - } - - // 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 - const std::string finish_wp_name = clean_param.finish; - const auto finish_wp = graph.find_waypoint(finish_wp_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(), finish_wp_name.c_str(), id.c_str()); - - return; - } - - // 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); - - // TODO(YV) get rid of id field in RequestPtr - std::stringstream id_stream(id); - std::size_t request_id; - id_stream >> request_id; - - new_request = rmf_task::requests::Clean::make( - request_id, - start_wp->index(), - finish_wp->index(), - cleaning_trajectory, - motion_sink, - ambient_sink, - tool_sink, - planner, - start_time, - drain_battery); - } - - // TODO(YV) - else if (task_type.value == rmf_task_msgs::msg::TaskType::DELIVERY_TASK) - { - - } - else if (task_type.value == rmf_task_msgs::msg::TaskType::LOOP_TASK) - { - - } - else - { - RCLCPP_INFO( - node->get_logger(), - "Invalid TaskType in TaskProfile. Rejecting BidNotice with task_id:[%s]", - id.c_str()); - - return; - } - + void dock_summary_cb(const DockSummary::SharedPtr msg); - if (!new_request) - return; - - // Combine new request ptr with request ptr of tasks in task manager queues - - // Update robot states - - // Generate new task assignments while accommodating for the new - // request - // Call greedy_plan but run optimal_plan() in a separate thread - - // Store results in internal map and publish BidProposal - } + void bid_notice_cb(const BidNotice::SharedPtr msg); static Implementation& get(FleetUpdateHandle& fleet) { From d16e9a4518c9669ace2352cc9277e657f597bfca Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 23 Oct 2020 17:56:03 +0800 Subject: [PATCH 12/50] RobotContext constructor taskes in State and StateConfig which are used for task planning --- .../agv/FleetUpdateHandle.hpp | 9 ++++++++ .../agv/FleetUpdateHandle.cpp | 20 +++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.cpp | 22 +++++++++++++++++-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 16 +++++++++++++- .../agv/internal_FleetUpdateHandle.hpp | 6 +++++ 5 files changed, 68 insertions(+), 5 deletions(-) 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 3e32e97b1..d556544b8 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -96,6 +96,15 @@ class FleetUpdateHandle : public std::enable_shared_from_this const bool drain_battery); + /// Set the threshold below which the robot should automatically head back to + /// its charging dock. The user is responsible to set this value such that the + /// robot is capable of reaching its nearest charging station from anywhere + /// on the map. Default value is 0.2. + /// + /// \param[in] threshold + /// The fraction of the total battery capacity + FleetUpdateHandle& set_recharge_threshold(const double threshold); + /// A callback function that evaluates whether a fleet will accept a task /// request /// 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 d863e88bf..ca2bd75ad 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -251,7 +251,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } // Update robot states - RCLCPP_INFO(node->get_logger(), "Planning for [%d] request(s)", pending_requests.size()); + std::vector states; + + RCLCPP_INFO(node->get_logger(), "Planning for [%d] request(s)", pending_requests.size()); // Generate new task assignments while accommodating for the new // request @@ -497,6 +499,10 @@ void FleetUpdateHandle::add_robot( fleet = shared_from_this()]( rmf_traffic::schedule::Participant participant) { + // TODO(YV) Get the charging location for this robot along with battery % + rmf_task::agv::State state = rmf_task::agv::State{start[0], 1, 1.0}; + rmf_task::agv::StateConfig state_config = rmf_task::agv::StateConfig{ + fleet->_pimpl->recharge_threshold}; auto context = std::make_shared( RobotContext{ std::move(command), @@ -506,7 +512,9 @@ void FleetUpdateHandle::add_robot( fleet->_pimpl->planner, fleet->_pimpl->node, fleet->_pimpl->worker, - fleet->_pimpl->default_maximum_delay + fleet->_pimpl->default_maximum_delay, + state, + state_config }); // We schedule the following operations on the worker to make sure we do not @@ -615,6 +623,14 @@ bool FleetUpdateHandle::set_task_planner_params( return false; } +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::set_recharge_threshold( + const double threshold) +{ + _pimpl->recharge_threshold = threshold; + return *this; +} + //============================================================================== FleetUpdateHandle::FleetUpdateHandle() { 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 a2d2626db..7c8d3b3f5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -213,6 +213,20 @@ RobotContext& RobotContext::maximum_delay( return *this; } +//============================================================================== +const rmf_task::agv::State RobotContext::state() const +{ + return _state; +} + +//============================================================================== +RobotContext& RobotContext::state( + const rmf_task::agv::State& state) +{ + _state = state; + return *this; +} + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, @@ -241,7 +255,9 @@ RobotContext::RobotContext( std::shared_ptr planner, std::shared_ptr node, const rxcpp::schedulers::worker& worker, - rmf_utils::optional maximum_delay) + rmf_utils::optional maximum_delay, + rmf_task::agv::State state, + rmf_task::agv::StateConfig state_config) : _command_handle(std::move(command_handle)), _location(std::move(_initial_location)), _itinerary(std::move(itinerary)), @@ -251,7 +267,9 @@ RobotContext::RobotContext( _worker(worker), _maximum_delay(maximum_delay), _requester_id( - _itinerary.description().owner() + "/" + _itinerary.description().name()) + _itinerary.description().owner() + "/" + _itinerary.description().name()), + _state(state), + _state_config(state_config) { _profile = std::make_shared( _itinerary.description().profile()); 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 6f6e54f7d..d33d915d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -9,6 +9,9 @@ #include #include +#include +#include + #include #include @@ -112,6 +115,12 @@ class RobotContext const TableViewerPtr& table_viewer, const ResponderPtr& responder) final; + /// Set the state of this robot + RobotContext& state(const rmf_task::agv::State& state); + + /// Get the state of this robot + const rmf_task::agv::State state() const; + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -124,7 +133,9 @@ class RobotContext std::shared_ptr planner, std::shared_ptr node, const rxcpp::schedulers::worker& worker, - rmf_utils::optional maximum_delay); + rmf_utils::optional maximum_delay, + rmf_task::agv::State state, + rmf_task::agv::StateConfig state_config); std::weak_ptr _command_handle; std::vector _location; @@ -144,6 +155,9 @@ class RobotContext std::string _requester_id; rmf_traffic::schedule::Negotiator* _negotiator = nullptr; + + rmf_task::agv::State _state; + rmf_task::agv::StateConfig _state_config; }; using RobotContextPtr = std::shared_ptr; 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 1b5add7a7..e96e7eb03 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 @@ -157,6 +157,12 @@ class FleetUpdateHandle::Implementation std::unordered_map dock_param_map = {}; + // Threshold soc for battery recharging + double recharge_threshold = 0.2; + + std::vector charging_waypoints; + std::vector unassigned_charging_waypoints; + AcceptTaskRequest accept_task = nullptr; using BidNotice = rmf_task_msgs::msg::BidNotice; From a880b1b638f5ef3130e5479c6141dfc516239452 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 24 Oct 2020 14:03:57 +0800 Subject: [PATCH 13/50] Added is_charger() and set_charger() methods to Waypoint API --- .../agv/internal_FleetUpdateHandle.hpp | 2 ++ rmf_traffic/include/rmf_traffic/agv/Graph.hpp | 9 +++++++++ rmf_traffic/src/rmf_traffic/agv/Graph.cpp | 15 +++++++++++++++ 3 files changed, 26 insertions(+) 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 e96e7eb03..a758f561f 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 @@ -160,7 +160,9 @@ class FleetUpdateHandle::Implementation // Threshold soc for battery recharging double recharge_threshold = 0.2; + // TODO Support for various charging configurations std::vector charging_waypoints; + // We assume each robot has a designated charging waypoint std::vector unassigned_charging_waypoints; AcceptTaskRequest accept_task = nullptr; diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp index c83dcb086..c6221cfd0 100644 --- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp +++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp @@ -78,6 +78,15 @@ class Graph /// Set this Waypoint to be a parking spot. Waypoint& set_parking_spot(bool _is_parking_spot); + + /// Returns true if this Waypoint is a charger spot. Robots are routed to + /// these spots when their batteries charge levels drop below the threshold + /// value. + bool is_charger() const; + + /// Set this Waypoint to be a parking spot. + Waypoint& set_charger(bool _is_charger); + /// The index of this waypoint within the Graph. This cannot be changed /// after the waypoint is created. std::size_t index() const; diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp index 0b919d493..79bc39bcf 100644 --- a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp +++ b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp @@ -44,6 +44,8 @@ class Graph::Waypoint::Implementation bool parking_spot = false; + bool charger = false; + template static Waypoint make(Args&& ... args) { @@ -125,6 +127,19 @@ auto Graph::Waypoint::set_parking_spot(bool _is_parking_spot) -> Waypoint& return *this; } +//============================================================================== +bool Graph::Waypoint::is_charger() const +{ + return _pimpl->charger; +} + +//============================================================================== +auto Graph::Waypoint::set_charger(bool _is_charger) -> Waypoint& +{ + _pimpl->charger = _is_charger; + return *this; +} + //============================================================================== std::size_t Graph::Waypoint::index() const { From 0feebc14093243f0d604842624e843e3572bebcb Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 24 Oct 2020 14:44:53 +0800 Subject: [PATCH 14/50] add_robot uses get_nearest_charger() --- .../agv/FleetUpdateHandle.cpp | 35 +++++++++++++++++-- .../agv/internal_FleetUpdateHandle.hpp | 17 +++++++-- 2 files changed, 48 insertions(+), 4 deletions(-) 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 ca2bd75ad..2438c859a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -476,6 +476,34 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // *estimate.loop_end); // } +//============================================================================== +std::size_t FleetUpdateHandle::Implementation::get_nearest_charger( + const rmf_traffic::agv::Planner::Start& start, + const std::unordered_set& charging_waypoints) +{ + assert(!charging_waypoints.empty()); + const auto& graph = planner->get_configuration().graph(); + Eigen::Vector2d p = graph.get_waypoint(start.waypoint()).get_location(); + + if (start.location().has_value()) + p = *start.location(); + + double min_dist = std::numeric_limits::max(); + std::size_t nearest_charger; + for (const auto& wp : charging_waypoints) + { + const auto loc = graph.get_waypoint(wp).get_location(); + const double dist = (loc - p).norm(); + if (dist < min_dist) + { + min_dist = dist; + nearest_charger = wp; + } + } + + return nearest_charger; +} + //============================================================================== void FleetUpdateHandle::add_robot( std::shared_ptr command, @@ -484,6 +512,7 @@ void FleetUpdateHandle::add_robot( rmf_traffic::agv::Plan::StartSet start, std::function)> handle_cb) { + assert(!start.empty()); rmf_traffic::schedule::ParticipantDescription description( name, _pimpl->name, @@ -499,8 +528,10 @@ void FleetUpdateHandle::add_robot( fleet = shared_from_this()]( rmf_traffic::schedule::Participant participant) { - // TODO(YV) Get the charging location for this robot along with battery % - rmf_task::agv::State state = rmf_task::agv::State{start[0], 1, 1.0}; + // TODO(YV) Get the battery % of this robot + const std::size_t charger = fleet->_pimpl->get_nearest_charger( + start[0], fleet->_pimpl->charging_waypoints); + rmf_task::agv::State state = rmf_task::agv::State{start[0], charger, 1.0}; rmf_task::agv::StateConfig state_config = rmf_task::agv::StateConfig{ fleet->_pimpl->recharge_threshold}; auto context = std::make_shared( 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 a758f561f..507d51cb3 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 @@ -46,6 +46,7 @@ #include #include +#include namespace rmf_fleet_adapter { namespace agv { @@ -161,9 +162,9 @@ class FleetUpdateHandle::Implementation double recharge_threshold = 0.2; // TODO Support for various charging configurations - std::vector charging_waypoints; + std::unordered_set charging_waypoints; // We assume each robot has a designated charging waypoint - std::vector unassigned_charging_waypoints; + std::unordered_set available_charging_waypoints; AcceptTaskRequest accept_task = nullptr; @@ -229,6 +230,14 @@ class FleetUpdateHandle::Implementation p->dock_summary_cb(msg); }); + // Populate charging waypoints + const std::size_t num_waypoints = + handle._pimpl->planner->get_configuration().graph().num_waypoints(); + for (std::size_t i = 0; i < num_waypoints; ++i) + handle._pimpl->charging_waypoints.insert(i); + handle._pimpl->available_charging_waypoints = + handle._pimpl->charging_waypoints; + return std::make_shared(std::move(handle)); } @@ -254,6 +263,10 @@ class FleetUpdateHandle::Implementation void bid_notice_cb(const BidNotice::SharedPtr msg); + std::size_t get_nearest_charger( + const rmf_traffic::agv::Planner::Start& start, + const std::unordered_set& charging_waypoints); + static Implementation& get(FleetUpdateHandle& fleet) { return *fleet._pimpl; From 418a53800b6d99f39fc407b4f90285635f1c4529 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 24 Oct 2020 14:52:41 +0800 Subject: [PATCH 15/50] parse_graph parses is_charger option --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 7 ++++++- .../src/rmf_fleet_adapter/agv/parse_graph.cpp | 8 ++++++++ 2 files changed, 14 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 2438c859a..ba09a5301 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -252,8 +252,13 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Update robot states std::vector states; + for (const auto& t : task_managers) + states.push_back(t.first->state()); - RCLCPP_INFO(node->get_logger(), "Planning for [%d] request(s)", pending_requests.size()); + RCLCPP_INFO( + node->get_logger(), + "Planning for [%d] robot and [%d] request(s)", + states.size(), pending_requests.size()); // Generate new task assignments while accommodating for the new // request diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp index 85f419fe4..23acf9047 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/parse_graph.cpp @@ -111,6 +111,14 @@ rmf_traffic::agv::Graph parse_graph( wp.set_passthrough_point(true); } + const YAML::Node& charger_option = options["is_charger"]; + if (charger_option) + { + const bool is_charger = charger_option.as(); + if (is_charger) + wp.set_charger(true); + } + const YAML::Node& lift_option = options["lift"]; if (lift_option) { From 0b9158503d37ee4c9a5f0e9c2125b0a76ba9454c Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Sat, 24 Oct 2020 15:21:37 +0800 Subject: [PATCH 16/50] Added state_config() to RobotContext --- .../src/rmf_fleet_adapter/TaskManager.cpp | 9 +++++++++ .../src/rmf_fleet_adapter/TaskManager.hpp | 2 +- .../agv/FleetUpdateHandle.cpp | 18 ++++++++++++------ .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++++++ .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 3 +++ 5 files changed, 31 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 35c05688b..8e6d82156 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -17,6 +17,8 @@ #include "TaskManager.hpp" +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -108,7 +110,14 @@ const std::vector TaskManager::requests() const std::vector requests; requests.reserve(_queue.size()); for (const auto& task : _queue) + { + if (std::dynamic_pointer_cast( + task->request())) + continue; + requests.push_back(task->request()); + + } return requests; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 05792e737..c3b71ff0b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -58,7 +58,7 @@ class TaskManager : public std::enable_shared_from_this /// task planner void set_queue(Assignments assignments); - /// Get the requests used to create the tasks currently in the queue + /// Get the non-charging requests for pending tasks const std::vector requests() const; private: 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 ba09a5301..59543bffd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -241,20 +241,20 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (!new_request) return; - // Combine new request ptr with request ptr of tasks in task manager queues + // Update robot states and combine new requestptr with requestptr of + // non-charging tasks in task manager queues + std::vector states; + std::vector state_configs; std::vector pending_requests; pending_requests.push_back(new_request); for (const auto& t : task_managers) { + states.push_back(t.first->state()); + state_configs.push_back(t.first->state_config()); const auto requests = t.second->requests(); pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); } - // Update robot states - std::vector states; - for (const auto& t : task_managers) - states.push_back(t.first->state()); - RCLCPP_INFO( node->get_logger(), "Planning for [%d] robot and [%d] request(s)", @@ -263,6 +263,12 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // Generate new task assignments while accommodating for the new // request // Call greedy_plan but run optimal_plan() in a separate thread + const auto assignments = task_planner->optimal_plan( + rmf_traffic_ros2::convert(node->now()), + states, + state_configs, + pending_requests, + nullptr); // Store results in internal map and publish BidProposal } 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 7c8d3b3f5..217ecfe66 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -227,6 +227,12 @@ RobotContext& RobotContext::state( return *this; } +//============================================================================== +const rmf_task::agv::StateConfig RobotContext::state_config() const +{ + return _state_config; +} + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, 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 d33d915d1..d0c3406b4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -121,6 +121,9 @@ class RobotContext /// Get the state of this robot const rmf_task::agv::State state() const; + /// Get the state config of this robot + const rmf_task::agv::StateConfig state_config() const; + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; From 0b2e9a73fa982175bba3cc7d93df80ae7155c4e7 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 11:01:20 +0800 Subject: [PATCH 17/50] Added dispatch_request_cb() --- rmf_battery/CMakeLists.txt | 4 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 17 ++++- .../src/rmf_fleet_adapter/TaskManager.hpp | 4 +- .../agv/FleetUpdateHandle.cpp | 71 ++++++++++++++++++- .../agv/internal_FleetUpdateHandle.hpp | 8 ++- rmf_task/CMakeLists.txt | 4 +- rmf_task/src/rmf_task/agv/State.cpp | 4 +- 7 files changed, 100 insertions(+), 12 deletions(-) diff --git a/rmf_battery/CMakeLists.txt b/rmf_battery/CMakeLists.txt index 1ef124999..09f650ade 100644 --- a/rmf_battery/CMakeLists.txt +++ b/rmf_battery/CMakeLists.txt @@ -15,8 +15,8 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") include(GNUInstallDirs) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 8e6d82156..e9423a13f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -18,6 +18,7 @@ #include "TaskManager.hpp" #include +#include namespace rmf_fleet_adapter { @@ -99,9 +100,21 @@ agv::ConstRobotContextPtr TaskManager::context() const } //============================================================================== -void TaskManager::set_queue(TaskManager::Assignments assignments) +void TaskManager::set_queue( + const std::vector& assignments) { - // TODO(YV) + _queue.clear(); + _queue.resize(assignments.size()); + // 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 (const auto& a : assignments) + { + if (const auto request = + std::dynamic_pointer_cast(a.request())) + { + + } + } } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index c3b71ff0b..e39ace430 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -41,7 +41,7 @@ class TaskManager : public std::enable_shared_from_this using Start = rmf_traffic::agv::Plan::Start; using StartSet = rmf_traffic::agv::Plan::StartSet; - using Assignments = rmf_task::agv::TaskPlanner::Assignments; + using Assignment = rmf_task::agv::TaskPlanner::Assignment; /// Add a task to the queue of this manager. void queue_task(std::shared_ptr task, Start expected_finish); @@ -56,7 +56,7 @@ class TaskManager : public std::enable_shared_from_this /// Set the queue for this task manager with assignments generated from the /// task planner - void set_queue(Assignments assignments); + void set_queue(const std::vector& assignments); /// Get the non-charging requests for pending tasks const std::vector requests() const; 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 59543bffd..201340ded 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -84,6 +84,13 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( { if (task_managers.empty()) return; + + if (msg->task_profile.task_id.empty()) + return; + + if (bid_notice_assignments.find(msg->task_profile.task_id) + != bid_notice_assignments.end()) + return; if (!accept_task) { @@ -270,7 +277,69 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( pending_requests, nullptr); - // Store results in internal map and publish BidProposal + const double cost = task_planner->compute_cost(assignments); + + // 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; + // TODO populate finish_time and robot_name + + bid_proposal_pub->publish(bid_proposal); + RCLCPP_INFO( + node->get_logger(), + "Submitted BidProposal to accommodate task [%s] with new cost [%f]", + id.c_str(), cost); + + // Store assignments in internal map + bid_notice_assignments.insert({id, assignments}); + +} + + +void FleetUpdateHandle::Implementation::dispatch_request_cb( + const DispatchRequest::SharedPtr msg) +{ + if (msg->fleet_name != name) + return; + + const std::string id = msg->task_profile.task_id; + const auto task_it = bid_notice_assignments.find(id); + + if (task_it == bid_notice_assignments.end()) + return; + + // We currently only support adding tasks + if (msg->method != DispatchRequest::ADD) + return; + + const auto& assignments = task_it->second; + + if (assignments.size() != task_managers.size()) + { + RCLCPP_ERROR( + node->get_logger(), + "The number of available robots do not match that in the assignments " + "for task_id:[%s]. This request will be ignored", id.c_str()); + + return; + } + + 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); + + RCLCPP_INFO( + node->get_logger(), + "Assignments updated for robots in fleet [%s]", + name.c_str()); } //============================================================================== 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 507d51cb3..32e3b6e6e 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 @@ -166,6 +166,10 @@ class FleetUpdateHandle::Implementation // We assume each robot has a designated charging waypoint std::unordered_set available_charging_waypoints; + double current_assignment_cost = 0.0; + // Map to store task id with assignments for BidNotice + std::unordered_map bid_notice_assignments = {}; + AcceptTaskRequest accept_task = nullptr; using BidNotice = rmf_task_msgs::msg::BidNotice; @@ -217,7 +221,7 @@ class FleetUpdateHandle::Implementation default_qos, [p = handle._pimpl.get()](const DispatchRequest::SharedPtr msg) { - // TODO(YV) + p->dispatch_request_cb(msg); }); // Subscribe DockSummary @@ -263,6 +267,8 @@ class FleetUpdateHandle::Implementation void bid_notice_cb(const BidNotice::SharedPtr msg); + void dispatch_request_cb(const DispatchRequest::SharedPtr msg); + std::size_t get_nearest_charger( const rmf_traffic::agv::Planner::Start& start, const std::unordered_set& charging_waypoints); diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index c16af6827..4af624e8a 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -18,8 +18,8 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address") include(GNUInstallDirs) diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp index c8ec09077..1562128bd 100644 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -35,9 +35,9 @@ class State::Implementation _charging_waypoint(charging_waypoint), _battery_soc(battery_soc) { - if (_battery_soc < 0.0 || _battery_soc > 1.0) + if (_battery_soc > 1.0) throw std::invalid_argument( - "Battery State of Charge needs be between 0.0 and 1.0."); + "Battery State of Charge needs to be between 0.0 and 1.0."); } rmf_traffic::agv::Plan::Start _location; From eb5ed0d582e26a3fa6ab86e42a0c38c57e51c58f Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 14:14:31 +0800 Subject: [PATCH 18/50] Updated set_queue() and make_clean() functions. Switching to using ConstRequestPtr everywhere --- .../src/rmf_fleet_adapter/Task.cpp | 6 ++-- .../src/rmf_fleet_adapter/Task.hpp | 8 ++--- .../src/rmf_fleet_adapter/TaskManager.cpp | 36 +++++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 2 +- .../agv/FleetUpdateHandle.cpp | 4 +-- .../rmf_fleet_adapter/agv/RobotContext.hpp | 2 +- .../src/rmf_fleet_adapter/tasks/Clean.cpp | 8 ++--- .../src/rmf_fleet_adapter/tasks/Clean.hpp | 7 ++-- rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 8 ++--- .../rmf_task/requests/ChargeBattery.hpp | 2 +- rmf_task/include/rmf_task/requests/Clean.hpp | 9 +++-- .../include/rmf_task/requests/Delivery.hpp | 2 +- rmf_task/src/rmf_task/agv/State.cpp | 2 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 24 ++++++------- .../src/rmf_task/requests/ChargeBattery.cpp | 2 +- rmf_task/src/rmf_task/requests/Clean.cpp | 7 +++- rmf_task/src/rmf_task/requests/Delivery.cpp | 2 +- rmf_task/test/unit/agv/test_TaskPlanner.cpp | 4 +-- 18 files changed, 87 insertions(+), 48 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index 9540eeb43..bc288423b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -31,7 +31,7 @@ std::shared_ptr Task::make( std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, - rmf_task::RequestPtr request) + rmf_task::ConstRequestPtr request) { return std::make_shared( Task(std::move(id), @@ -85,7 +85,7 @@ const std::string& Task::id() const } //============================================================================== -const rmf_task::RequestPtr Task::request() const +const rmf_task::ConstRequestPtr Task::request() const { return _request; } @@ -95,7 +95,7 @@ Task::Task( std::string id, std::vector> phases, rxcpp::schedulers::worker worker, - rmf_task::RequestPtr request) + rmf_task::ConstRequestPtr request) : _id(std::move(id)), _pending_phases(std::move(phases)), _worker(std::move(worker)), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index e7abbba7e..d751aa087 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -101,7 +101,7 @@ class Task : public std::enable_shared_from_this std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, - rmf_task::RequestPtr request = nullptr); + rmf_task::ConstRequestPtr request = nullptr); void begin(); @@ -123,7 +123,7 @@ class Task : public std::enable_shared_from_this const std::string& id() const; /// Get the request used to generate this task - const rmf_task::RequestPtr request() const; + const rmf_task::ConstRequestPtr request() const; private: @@ -131,7 +131,7 @@ class Task : public std::enable_shared_from_this std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, - rmf_task::RequestPtr request); + rmf_task::ConstRequestPtr request); std::string _id; @@ -149,7 +149,7 @@ class Task : public std::enable_shared_from_this rmf_utils::optional _initial_time; - rmf_task::RequestPtr _request; + rmf_task::ConstRequestPtr _request; void _start_next_phase(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index e9423a13f..3af52a027 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -19,6 +19,9 @@ #include #include +#include + +#include "tasks/Clean.hpp" namespace rmf_fleet_adapter { @@ -107,20 +110,47 @@ void TaskManager::set_queue( _queue.resize(assignments.size()); // 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 (const auto& a : assignments) + for (std::size_t i = 0; i < assignments.size(); ++i) { + const auto& a = assignments[i]; + auto start = _context->state().location(); + if (i != 0) + start = assignments[i-1].state().location(); + if (const auto request = std::dynamic_pointer_cast(a.request())) { + const auto task = rmf_fleet_adapter::tasks::make_clean( + request, + _context, + start); + } + + else if (const auto request = + std::dynamic_pointer_cast( + a.request())) + { + // const auto task = tasks::make_charge_battery() + } + else if (const auto request = + std::dynamic_pointer_cast( + a.request())) + { + // const auto task = tasks::make_delivery() + } + + else + { + continue; } } } //============================================================================== -const std::vector TaskManager::requests() const +const std::vector TaskManager::requests() const { - std::vector requests; + std::vector requests; requests.reserve(_queue.size()); for (const auto& task : _queue) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index e39ace430..bc57a2740 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -59,7 +59,7 @@ class TaskManager : public std::enable_shared_from_this void set_queue(const std::vector& assignments); /// Get the non-charging requests for pending tasks - const std::vector requests() const; + const std::vector requests() const; private: 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 201340ded..af11700d4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -127,7 +127,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } // Determine task type and convert to request pointer - rmf_task::RequestPtr new_request = nullptr; + rmf_task::ConstRequestPtr new_request = nullptr; const auto& task_profile = msg->task_profile; const auto& task_type = task_profile.type; const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.start_time); @@ -252,7 +252,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // non-charging tasks in task manager queues std::vector states; std::vector state_configs; - std::vector pending_requests; + std::vector pending_requests; pending_requests.push_back(new_request); for (const auto& t : task_managers) { 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 d0c3406b4..5212af6bb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -118,7 +118,7 @@ class RobotContext /// Set the state of this robot RobotContext& state(const rmf_task::agv::State& state); - /// Get the state of this robot + /// Get the state of this robot const rmf_task::agv::State state() const; /// Get the state config of this robot 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 8c1782a30..e9fa6dd8d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -24,17 +24,17 @@ namespace tasks { //============================================================================== std::shared_ptr make_clean( - const rmf_task::RequestPtr request, + const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start clean_start, - const rmf_traffic::agv::Plan::Goal clean_goal) + const rmf_traffic::agv::Plan::Start clean_start) { + rmf_traffic::agv::Planner::Goal clean_goal{request->start_waypoint()}; Task::PendingPhases phases; phases.push_back( phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); return Task::make( - std::to_string(request->id()), std::move(phases), context->worker()); + std::to_string(request->id()), std::move(phases), context->worker(), request); } } // namespace task 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 fca81468b..24cbf1243 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -23,17 +23,16 @@ #include -#include +#include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== std::shared_ptr make_clean( - const rmf_task::RequestPtr request, + const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start clean_start, - const rmf_traffic::agv::Plan::Goal clean_goal); + const rmf_traffic::agv::Plan::Start clean_start); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 0232c65ab..4b38ffd71 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -122,13 +122,13 @@ class TaskPlanner /// \param[in] earliest_start_time /// The earliest time the agent will begin exececuting this task Assignment( - rmf_task::RequestPtr request, + rmf_task::ConstRequestPtr request, State state, rmf_traffic::Time deployment_time); // Get the request of this task - rmf_task::RequestPtr request() const; + rmf_task::ConstRequestPtr request() const; // Get a const reference to the state const State& state() const; @@ -158,7 +158,7 @@ class TaskPlanner rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests); + std::vector requests); /// Get the optimal planner based assignments for a set of initial states and /// requests @@ -170,7 +170,7 @@ class TaskPlanner rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, std::function interrupter); double compute_cost(const Assignments& assignments); diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 959cae7f6..a5500dd26 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -38,7 +38,7 @@ class ChargeBattery : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index 98d148e25..a59dd920e 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -40,7 +40,7 @@ class Clean : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -62,13 +62,18 @@ class Clean : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + const std::size_t start_waypoint() const; + class Implementation; private: Clean(); rmf_utils::impl_ptr _pimpl; }; -} // namespace tasks +using CleanRequestPtr = std::shared_ptr; +using ConstCleanRequestPtr = std::shared_ptr; + +} // namespace requests } // namespace rmf_task #endif // INCLUDE__RMF_TASK__REQUESTS__CLEAN_HPP diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 062840ed0..c81e2b732 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -39,7 +39,7 @@ class Delivery : public rmf_task::Request { public: - static rmf_task::Request::SharedPtr make( + static ConstRequestPtr make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, diff --git a/rmf_task/src/rmf_task/agv/State.cpp b/rmf_task/src/rmf_task/agv/State.cpp index 1562128bd..82d002d49 100644 --- a/rmf_task/src/rmf_task/agv/State.cpp +++ b/rmf_task/src/rmf_task/agv/State.cpp @@ -35,7 +35,7 @@ class State::Implementation _charging_waypoint(charging_waypoint), _battery_soc(battery_soc) { - if (_battery_soc > 1.0) + if (_battery_soc < 0.0 || _battery_soc > 1.0) throw std::invalid_argument( "Battery State of Charge needs to be between 0.0 and 1.0."); } diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index d882648e0..0a8873686 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -117,14 +117,14 @@ class TaskPlanner::Assignment::Implementation { public: - rmf_task::RequestPtr request; + rmf_task::ConstRequestPtr request; State state; rmf_traffic::Time deployment_time; }; //============================================================================== TaskPlanner::Assignment::Assignment( - rmf_task::RequestPtr request, + rmf_task::ConstRequestPtr request, State state, rmf_traffic::Time deployment_time) : _pimpl(rmf_utils::make_impl( @@ -138,7 +138,7 @@ TaskPlanner::Assignment::Assignment( } //============================================================================== -rmf_task::RequestPtr TaskPlanner::Assignment::request() const +rmf_task::ConstRequestPtr TaskPlanner::Assignment::request() const { return _pimpl->request; } @@ -327,8 +327,8 @@ struct PendingTask PendingTask( std::vector& initial_states, std::vector& state_configs, - rmf_task::Request::SharedPtr request_, - rmf_task::Request::SharedPtr charge_battery_request) + rmf_task::ConstRequestPtr request_, + rmf_task::ConstRequestPtr charge_battery_request) : request(std::move(request_)), candidates(Candidates::make( initial_states, state_configs, *request, *charge_battery_request)), @@ -337,7 +337,7 @@ struct PendingTask // Do nothing } - rmf_task::Request::SharedPtr request; + rmf_task::ConstRequestPtr request; Candidates candidates; rmf_traffic::Time earliest_start_time; }; @@ -610,7 +610,7 @@ class TaskPlanner::Implementation std::shared_ptr config; - RequestPtr make_charging_request(rmf_traffic::Time start_time) + ConstRequestPtr make_charging_request(rmf_traffic::Time start_time) { return rmf_task::requests::ChargeBattery::make( config->battery_system(), @@ -658,7 +658,7 @@ class TaskPlanner::Implementation rmf_traffic::Time time_now, std::vector& initial_states, const std::vector& state_configs, - const std::vector& requests, + const std::vector& requests, const std::function interrupter, bool greedy) { @@ -694,7 +694,7 @@ class TaskPlanner::Implementation if (node->unassigned_tasks.empty()) return prune_assignments(complete_assignments); - std::vector new_tasks; + std::vector new_tasks; for (const auto& u : node->unassigned_tasks) new_tasks.push_back(u.second.request); @@ -788,7 +788,7 @@ class TaskPlanner::Implementation ConstNodePtr make_initial_node( std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, rmf_traffic::Time time_now) { auto initial_node = std::make_shared(); @@ -1198,7 +1198,7 @@ auto TaskPlanner::greedy_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests) -> Assignments + std::vector requests) -> Assignments { return _pimpl->complete_solve( time_now, @@ -1213,7 +1213,7 @@ auto TaskPlanner::optimal_plan( rmf_traffic::Time time_now, std::vector initial_states, std::vector state_configs, - std::vector requests, + std::vector requests, std::function interrupter) -> Assignments { return _pimpl->complete_solve( diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 1d34fb9e2..94913bf2c 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -43,7 +43,7 @@ class ChargeBattery::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr ChargeBattery::make( +rmf_task::ConstRequestPtr ChargeBattery::make( rmf_battery::agv::BatterySystem battery_system, std::shared_ptr motion_sink, std::shared_ptr device_sink, diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 931e53f49..03fec418d 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -46,7 +46,7 @@ class Clean::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr Clean::make( +rmf_task::ConstRequestPtr Clean::make( std::size_t id, std::size_t start_waypoint, std::size_t end_waypoint, @@ -220,6 +220,11 @@ rmf_traffic::Time Clean::earliest_start_time() const return _pimpl->start_time; } +const std::size_t Clean::start_waypoint() const +{ + return _pimpl->start_waypoint; +} + //============================================================================== } // namespace requests } // namespace rmf_task diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 980b3160e..230a5e0c1 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -44,7 +44,7 @@ class Delivery::Implementation }; //============================================================================== -rmf_task::Request::SharedPtr Delivery::make( +rmf_task::ConstRequestPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, std::size_t dropoff_waypoint, diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index f6460e8e9..b28970437 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -150,7 +150,7 @@ SCENARIO("Grid World") rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { rmf_task::requests::Delivery::make( 1, @@ -225,7 +225,7 @@ SCENARIO("Grid World") rmf_task::agv::StateConfig{0.2} }; - std::vector requests = + std::vector requests = { rmf_task::requests::Delivery::make( 1, From 0b95994e0a94ac5bf40daae6dfc932293f6cdfa4 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 15:17:17 +0800 Subject: [PATCH 19/50] Broke Task constructor API --- .../src/rmf_fleet_adapter/Task.cpp | 8 ++ .../src/rmf_fleet_adapter/Task.hpp | 10 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 6 +- .../src/rmf_fleet_adapter/TaskManager.hpp | 2 + .../src/rmf_fleet_adapter/tasks/Clean.cpp | 11 +- .../src/rmf_fleet_adapter/tasks/Clean.hpp | 6 +- .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 132 +++++++++--------- .../src/rmf_fleet_adapter/tasks/Loop.cpp | 106 +++++++------- 8 files changed, 157 insertions(+), 124 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index bc288423b..489c5bd7b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -31,12 +31,16 @@ std::shared_ptr Task::make( std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, rmf_task::ConstRequestPtr request) { return std::make_shared( Task(std::move(id), std::move(phases), std::move(worker), + deployment_time, + finish_state, std::move(request))); } @@ -95,10 +99,14 @@ Task::Task( std::string id, std::vector> phases, rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, rmf_task::ConstRequestPtr request) : _id(std::move(id)), _pending_phases(std::move(phases)), _worker(std::move(worker)), + _deployment_time(deployment_time), + _finish_state(finish_state), _request(std::move(request)) { _status_obs = _status_publisher.get_observable(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index d751aa087..bd726adaa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -22,10 +22,12 @@ #include #include +#include #include #include +#include #include #include @@ -101,7 +103,9 @@ class Task : public std::enable_shared_from_this std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, - rmf_task::ConstRequestPtr request = nullptr); + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, + rmf_task::ConstRequestPtr request); void begin(); @@ -131,6 +135,8 @@ class Task : public std::enable_shared_from_this std::string id, PendingPhases phases, rxcpp::schedulers::worker worker, + rmf_traffic::Time deployment_time, + rmf_task::agv::State finish_state, rmf_task::ConstRequestPtr request); std::string _id; @@ -149,6 +155,8 @@ 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::ConstRequestPtr _request; void _start_next_phase(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3af52a027..aba945be8 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -123,7 +123,11 @@ void TaskManager::set_queue( const auto task = rmf_fleet_adapter::tasks::make_clean( request, _context, - start); + start, + a.deployment_time(), + a.state()); + + _queue.push_back(std::move(task)); } else if (const auto request = diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index bc57a2740..00c9be84c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -72,6 +72,8 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; + rclcpp::TimerBase::SharedPtr _timer; + void _begin_next_task(); void clear_queue(); 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 e9fa6dd8d..3523fc2b9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -26,7 +26,9 @@ namespace tasks { std::shared_ptr make_clean( const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start clean_start) + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) { rmf_traffic::agv::Planner::Goal clean_goal{request->start_waypoint()}; Task::PendingPhases phases; @@ -34,7 +36,12 @@ std::shared_ptr make_clean( phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); return Task::make( - std::to_string(request->id()), std::move(phases), context->worker(), request); + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); } } // namespace task 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 24cbf1243..6c5d67ab5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.hpp @@ -22,7 +22,9 @@ #include "../agv/RobotContext.hpp" #include +#include +#include #include namespace rmf_fleet_adapter { @@ -32,7 +34,9 @@ namespace tasks { std::shared_ptr make_clean( const rmf_task::requests::ConstCleanRequestPtr request, const agv::RobotContextPtr& context, - const rmf_traffic::agv::Plan::Start clean_start); + const rmf_traffic::agv::Plan::Start clean_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); } // namespace tasks } // namespace rmf_fleet_adapter 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 09130b4e0..edf93b4d3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -1,79 +1,79 @@ -/* - * 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. - * -*/ +// /* +// * 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 "../phases/DispenseItem.hpp" -#include "../phases/IngestItem.hpp" -#include "../phases/GoToPlace.hpp" +// #include "../phases/DispenseItem.hpp" +// #include "../phases/IngestItem.hpp" +// #include "../phases/GoToPlace.hpp" -#include "Delivery.hpp" +// #include "Delivery.hpp" -namespace rmf_fleet_adapter { -namespace tasks { +// namespace rmf_fleet_adapter { +// namespace tasks { -//============================================================================== -std::shared_ptr make_delivery( - const rmf_task_msgs::msg::Delivery& request, - const agv::RobotContextPtr& context, - rmf_traffic::agv::Plan::Start pickup_start, - rmf_traffic::agv::Plan::Start dropoff_start) -{ - const auto& graph = context->navigation_graph(); +// //============================================================================== +// std::shared_ptr make_delivery( +// const rmf_task_msgs::msg::Delivery& request, +// const agv::RobotContextPtr& context, +// rmf_traffic::agv::Plan::Start pickup_start, +// rmf_traffic::agv::Plan::Start dropoff_start) +// { +// const auto& graph = context->navigation_graph(); - const auto pickup_wp = - graph.find_waypoint(request.pickup_place_name)->index(); +// const auto pickup_wp = +// graph.find_waypoint(request.pickup_place_name)->index(); - Task::PendingPhases phases; - phases.push_back( - phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); +// Task::PendingPhases phases; +// phases.push_back( +// phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); - phases.push_back( - std::make_unique( - context, - request.task_id, - request.pickup_dispenser, - context->itinerary().description().owner(), - request.items)); +// phases.push_back( +// std::make_unique( +// context, +// request.task_id, +// request.pickup_dispenser, +// context->itinerary().description().owner(), +// request.items)); - const auto dropoff_wp = - graph.find_waypoint(request.dropoff_place_name)->index(); +// const auto dropoff_wp = +// graph.find_waypoint(request.dropoff_place_name)->index(); - phases.push_back( - phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, std::move(dropoff_start), dropoff_wp)); - std::vector ingestor_items; - ingestor_items.reserve(request.items.size()); - for(auto& dispenser_item : request.items){ - rmf_ingestor_msgs::msg::IngestorRequestItem item{}; - item.type_guid = dispenser_item.type_guid; - item.quantity = dispenser_item.quantity; - item.compartment_name = dispenser_item.compartment_name; - ingestor_items.push_back(std::move(item)); - } +// std::vector ingestor_items; +// ingestor_items.reserve(request.items.size()); +// for(auto& dispenser_item : request.items){ +// rmf_ingestor_msgs::msg::IngestorRequestItem item{}; +// item.type_guid = dispenser_item.type_guid; +// item.quantity = dispenser_item.quantity; +// item.compartment_name = dispenser_item.compartment_name; +// ingestor_items.push_back(std::move(item)); +// } - phases.push_back( - std::make_unique( - context, - request.task_id, - request.dropoff_ingestor, - context->itinerary().description().owner(), - ingestor_items)); +// phases.push_back( +// std::make_unique( +// context, +// request.task_id, +// request.dropoff_ingestor, +// context->itinerary().description().owner(), +// ingestor_items)); - return Task::make(request.task_id, std::move(phases), context->worker()); -} +// return Task::make(request.task_id, std::move(phases), context->worker()); +// } -} // namespace task -} // namespace rmf_fleet_adapter +// } // namespace task +// } // 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 8593261fc..b5f9397cb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -1,68 +1,68 @@ -/* - * 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. - * -*/ +// /* +// * 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 "Loop.hpp" +// #include "Loop.hpp" -#include "../phases/GoToPlace.hpp" +// #include "../phases/GoToPlace.hpp" -namespace rmf_fleet_adapter { -namespace tasks { +// namespace rmf_fleet_adapter { +// namespace tasks { -//============================================================================== -std::shared_ptr make_loop( - const rmf_task_msgs::msg::Loop& request, - const agv::RobotContextPtr& context, - rmf_utils::optional init_start, - rmf_traffic::agv::Plan::Start loop_start, - rmf_utils::optional loop_end) -{ - const auto& graph = context->navigation_graph(); +// //============================================================================== +// std::shared_ptr make_loop( +// const rmf_task_msgs::msg::Loop& request, +// const agv::RobotContextPtr& context, +// rmf_utils::optional init_start, +// rmf_traffic::agv::Plan::Start loop_start, +// rmf_utils::optional loop_end) +// { +// const auto& graph = context->navigation_graph(); - Task::PendingPhases phases; +// Task::PendingPhases phases; - const auto start_wp = - graph.find_waypoint(request.start_name)->index(); +// const auto start_wp = +// graph.find_waypoint(request.start_name)->index(); - const auto end_wp = - graph.find_waypoint(request.finish_name)->index(); +// const auto end_wp = +// graph.find_waypoint(request.finish_name)->index(); - if (init_start) - { - phases.push_back( - phases::GoToPlace::make(context, *init_start, start_wp)); - } +// if (init_start) +// { +// phases.push_back( +// phases::GoToPlace::make(context, *init_start, start_wp)); +// } - phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, loop_start, end_wp)); - for (std::size_t i=1; i < request.num_loops; ++i) - { - assert(loop_end); +// for (std::size_t i=1; i < request.num_loops; ++i) +// { +// assert(loop_end); - phases.push_back( - phases::GoToPlace::make(context, *loop_end, start_wp)); +// phases.push_back( +// phases::GoToPlace::make(context, *loop_end, start_wp)); - phases.push_back( - phases::GoToPlace::make(context, loop_start, end_wp)); - } +// phases.push_back( +// phases::GoToPlace::make(context, loop_start, end_wp)); +// } - return Task::make(request.task_id, std::move(phases), context->worker()); -} +// return Task::make(request.task_id, std::move(phases), context->worker()); +// } -} // namespace tasks -} // naemspace rmf_fleet_adapter +// } // namespace tasks +// } // naemspace rmf_fleet_adapter From 4c4570a1cc434b6dbecb815c2457fd4e761f3f9a Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 17:14:13 +0800 Subject: [PATCH 20/50] Added timer to TaskManager --- rmf_fleet_adapter/src/full_control/main.cpp | 2 +- .../src/rmf_fleet_adapter/Task.cpp | 12 ++ .../src/rmf_fleet_adapter/Task.hpp | 6 + .../src/rmf_fleet_adapter/TaskManager.cpp | 113 +++++++++++------- .../src/rmf_fleet_adapter/TaskManager.hpp | 5 +- .../rmf_fleet_adapter/phases/GoToPlace.cpp | 12 +- rmf_fleet_adapter/test/test_Task.cpp | 19 ++- 7 files changed, 117 insertions(+), 52 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 88047e332..72a8a4246 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -487,7 +487,7 @@ std::shared_ptr make_fleet( // Parameters required for task planner // Battery system auto battery_system = std::make_shared( - rmf_fleet_adapter::get_battery_system(*node, 24.0, 40.0, 8.8)); + rmf_fleet_adapter::get_battery_system(*node, 24.0, 80.0, 8.8)); if (!battery_system->valid()) { RCLCPP_ERROR( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp index 489c5bd7b..f2b31919a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.cpp @@ -94,6 +94,18 @@ const rmf_task::ConstRequestPtr Task::request() const return _request; } +//============================================================================== +const rmf_traffic::Time Task::deployment_time() const +{ + return _deployment_time; +} + +//============================================================================== +const rmf_task::agv::State Task::finish_state() const +{ + return _finish_state; +} + //============================================================================== Task::Task( std::string id, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp index bd726adaa..36a34be6d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/Task.hpp @@ -129,6 +129,12 @@ 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 + const rmf_traffic::Time deployment_time() const; + + /// Get the finish state of this Task + const rmf_task::agv::State finish_state() const; + private: Task( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index aba945be8..ebf16f481 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include "tasks/Clean.hpp" namespace rmf_fleet_adapter { @@ -44,6 +46,15 @@ TaskManagerPtr TaskManager::make(agv::RobotContextPtr context) } }); + mgr->_timer = mgr->context()->node()->create_wall_timer( + std::chrono::seconds(1), + [w = mgr->weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->_begin_next_task(); + } + }); return mgr; } @@ -128,6 +139,11 @@ void TaskManager::set_queue( a.state()); _queue.push_back(std::move(task)); + + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + this->_context->node()->task_summary()->publish(msg); } else if (const auto request = @@ -172,64 +188,73 @@ const std::vector TaskManager::requests() const //============================================================================== void TaskManager::_begin_next_task() { + if (_active_task) + return; + if (_queue.empty()) { _task_sub.unsubscribe(); _expected_finish_location = rmf_utils::nullopt; - RCLCPP_INFO( - _context->node()->get_logger(), - "Finished all remaining tasks for [%s]", - _context->requester_id().c_str()); + // RCLCPP_INFO( + // _context->node()->get_logger(), + // "Finished all remaining tasks for [%s]", + // _context->requester_id().c_str()); return; } - _active_task = std::move(_queue.front()); - _queue.erase(_queue.begin()); - - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning new task [%s] for [%s]. Remaining queue size: %d", - _active_task->id().c_str(), _context->requester_id().c_str(), - _queue.size()); - - _task_sub = _active_task->observe() - .observe_on(rxcpp::identity_same_worker(_context->worker())) - .subscribe( - [this, id = _active_task->id()](Task::StatusMsg msg) + if (rmf_traffic_ros2::convert(_context->node()->now()) > + _queue.front()->deployment_time()) { - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - }, - [this, id = _active_task->id()](std::exception_ptr e) - { - rmf_task_msgs::msg::TaskSummary msg; - msg.state = msg.STATE_FAILED; + // Update state in RobotContext and Assign active task + _context->state(_queue.front()->finish_state()); + _active_task = std::move(_queue.front()); + _queue.erase(_queue.begin()); - try { - std::rethrow_exception(e); - } - catch(const std::exception& e) { - msg.status = e.what(); - } + RCLCPP_INFO( + _context->node()->get_logger(), + "Beginning new task [%s] for [%s]. Remaining queue size: %d", + _active_task->id().c_str(), _context->requester_id().c_str(), + _queue.size()); + + _task_sub = _active_task->observe() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [this, id = _active_task->id()](Task::StatusMsg msg) + { + msg.task_id = id; + _context->node()->task_summary()->publish(msg); + }, + [this, id = _active_task->id()](std::exception_ptr e) + { + rmf_task_msgs::msg::TaskSummary msg; + msg.state = msg.STATE_FAILED; - msg.task_id = id; - _context->node()->task_summary()->publish(msg); - _begin_next_task(); - }, - [this, id = _active_task->id()]() - { - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = id; - msg.state = msg.STATE_COMPLETED; - this->_context->node()->task_summary()->publish(msg); + try { + std::rethrow_exception(e); + } + catch(const std::exception& e) { + msg.status = e.what(); + } - _active_task = nullptr; - _begin_next_task(); - }); + msg.task_id = id; + _context->node()->task_summary()->publish(msg); + // _begin_next_task(); + }, + [this, id = _active_task->id()]() + { + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = id; + msg.state = msg.STATE_COMPLETED; + this->_context->node()->task_summary()->publish(msg); + + _active_task = nullptr; + // _begin_next_task(); + }); - _active_task->begin(); + _active_task->begin(); + } } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 00c9be84c..68e31d041 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -61,6 +61,9 @@ class TaskManager : public std::enable_shared_from_this /// Get the non-charging requests for pending tasks const std::vector requests() const; + // Callback for timer which begins next task if its deployment time has passed + void _begin_next_task(); + private: TaskManager(agv::RobotContextPtr context); @@ -74,8 +77,6 @@ class TaskManager : public std::enable_shared_from_this rclcpp::TimerBase::SharedPtr _timer; - void _begin_next_task(); - void clear_queue(); }; 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 176144766..281cff7d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -533,9 +533,17 @@ 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( - _description, std::move(sub_phases), _context->worker()); + _description, + std::move(sub_phases), + _context->worker(), + dummy_time, + dummy_state, + nullptr); _status_subscription = _subtasks->observe() .observe_on(rxcpp::identity_same_worker(_context->worker())) diff --git a/rmf_fleet_adapter/test/test_Task.cpp b/rmf_fleet_adapter/test/test_Task.cpp index 83f14cbb7..2378174f6 100644 --- a/rmf_fleet_adapter/test/test_Task.cpp +++ b/rmf_fleet_adapter/test/test_Task.cpp @@ -180,7 +180,10 @@ class MockSubtaskPhase : _subtasks( rmf_fleet_adapter::Task::make( "subtasks", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker())) + rxcpp::schedulers::make_event_loop().create_worker(), + std::chrono::steady_clock::now(), + {{std::chrono::steady_clock::now(), 0, 0.0}, 0, 1.0}, + nullptr)) { _desc = "subtasks"; _status_obs = _status_publisher.get_observable(); @@ -290,10 +293,15 @@ SCENARIO("Test simple task") 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}; + std::shared_ptr task = rmf_fleet_adapter::Task::make( "test_Task", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker()); + rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, + finish_state, nullptr); std::promise completed_promise; auto completed_future = completed_promise.get_future(); @@ -378,9 +386,14 @@ SCENARIO("Test nested task") phases.push_back( std::make_unique(std::move(c_phases))); + // Dummy parameters + rmf_traffic::Time deployment_time; + rmf_task::agv::State finish_state{{deployment_time, 0, 0.0}, 0, 1.0}; + const auto task = rmf_fleet_adapter::Task::make( "test_NestedTask", std::move(phases), - rxcpp::schedulers::make_event_loop().create_worker()); + rxcpp::schedulers::make_event_loop().create_worker(), deployment_time, + finish_state, nullptr); std::promise completed_promise; auto completed_future = completed_promise.get_future(); From fdcbfc95c5a272ed83fe94dbc509e6dd78b67700 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 17:41:05 +0800 Subject: [PATCH 21/50] Locking mutex when modifying task queue --- .../src/rmf_fleet_adapter/TaskManager.cpp | 16 ++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 3 +++ .../agv/internal_FleetUpdateHandle.hpp | 2 +- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index ebf16f481..3cb453d67 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -117,6 +117,7 @@ agv::ConstRobotContextPtr TaskManager::context() const void TaskManager::set_queue( const std::vector& assignments) { + std::lock_guard guard(_mutex); _queue.clear(); _queue.resize(assignments.size()); // We use dynamic cast to determine the type of request and then call the @@ -204,9 +205,20 @@ void TaskManager::_begin_next_task() return; } - if (rmf_traffic_ros2::convert(_context->node()->now()) > - _queue.front()->deployment_time()) + const rmf_traffic::Time now = rmf_traffic_ros2::convert( + _context->node()->now()); + + RCLCPP_INFO( + _context->node()->get_logger(), + "Time now:[%d], Next deployment time: [%d]", + now.time_since_epoch().count(), + _queue.front()->deployment_time()); + + return; + + if (now > _queue.front()->deployment_time()) { + std::lock_guard guard(_mutex); // Update state in RobotContext and Assign active task _context->state(_queue.front()->finish_state()); _active_task = std::move(_queue.front()); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 68e31d041..9976ede35 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -25,6 +25,8 @@ #include +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -75,6 +77,7 @@ class TaskManager : public std::enable_shared_from_this rxcpp::subscription _task_sub; rxcpp::subscription _emergency_sub; + std::mutex _mutex; rclcpp::TimerBase::SharedPtr _timer; void clear_queue(); 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 32e3b6e6e..55ee0f35b 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 @@ -197,7 +197,7 @@ class FleetUpdateHandle::Implementation // Create subs and pubs for bidding auto default_qos = rclcpp::SystemDefaultsQoS(); - auto transient_qos = default_qos; transient_qos.transient_local(); + auto transient_qos = rclcpp::QoS(10);; transient_qos.transient_local(); // Publish BidProposal handle._pimpl->bid_proposal_pub = From 13279695e7f9004b4098900419666da39376d1cf Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 18:48:47 +0800 Subject: [PATCH 22/50] Dispatcher pipeline works! --- rmf_fleet_adapter/src/full_control/main.cpp | 2 +- .../src/rmf_fleet_adapter/TaskManager.cpp | 38 +++++++++++++++---- .../agv/FleetUpdateHandle.cpp | 22 +++++++++++ rmf_task/include/rmf_task/agv/TaskPlanner.hpp | 4 +- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 2 +- 5 files changed, 57 insertions(+), 11 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 72a8a4246..88047e332 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -487,7 +487,7 @@ std::shared_ptr make_fleet( // Parameters required for task planner // Battery system auto battery_system = std::make_shared( - rmf_fleet_adapter::get_battery_system(*node, 24.0, 80.0, 8.8)); + rmf_fleet_adapter::get_battery_system(*node, 24.0, 40.0, 8.8)); if (!battery_system->valid()) { RCLCPP_ERROR( diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3cb453d67..d9a19a524 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -25,6 +25,8 @@ #include "tasks/Clean.hpp" +#include + namespace rmf_fleet_adapter { //============================================================================== @@ -119,7 +121,7 @@ void TaskManager::set_queue( { std::lock_guard guard(_mutex); _queue.clear(); - _queue.resize(assignments.size()); + RCLCPP_ERROR(_context->node()->get_logger(), "Here 1"); // 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) @@ -132,25 +134,31 @@ void TaskManager::set_queue( if (const auto request = std::dynamic_pointer_cast(a.request())) { - const auto task = rmf_fleet_adapter::tasks::make_clean( + RCLCPP_ERROR(_context->node()->get_logger(), "Here 2"); + + auto task = rmf_fleet_adapter::tasks::make_clean( request, _context, start, a.deployment_time(), a.state()); - _queue.push_back(std::move(task)); + _queue.push_back(task); rmf_task_msgs::msg::TaskSummary msg; msg.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; this->_context->node()->task_summary()->publish(msg); + + RCLCPP_ERROR(_context->node()->get_logger(), "Here 3"); + } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + RCLCPP_ERROR(_context->node()->get_logger(), "Here 4"); // const auto task = tasks::make_charge_battery() } @@ -166,6 +174,9 @@ void TaskManager::set_queue( continue; } } + + RCLCPP_ERROR(_context->node()->get_logger(), "Here 5"); + } //============================================================================== @@ -194,8 +205,8 @@ void TaskManager::_begin_next_task() if (_queue.empty()) { - _task_sub.unsubscribe(); - _expected_finish_location = rmf_utils::nullopt; + // _task_sub.unsubscribe(); + // _expected_finish_location = rmf_utils::nullopt; // RCLCPP_INFO( // _context->node()->get_logger(), @@ -205,19 +216,32 @@ void TaskManager::_begin_next_task() return; } + RCLCPP_ERROR(_context->node()->get_logger(), "Here 6"); + const rmf_traffic::Time now = rmf_traffic_ros2::convert( _context->node()->now()); + RCLCPP_ERROR(_context->node()->get_logger(), "Here 7"); + + const auto next_task = _queue.front(); + + RCLCPP_ERROR(_context->node()->get_logger(), "Here 8"); + + const auto deployment_time = next_task->deployment_time(); + + RCLCPP_ERROR(_context->node()->get_logger(), "Here 9"); + RCLCPP_INFO( _context->node()->get_logger(), "Time now:[%d], Next deployment time: [%d]", now.time_since_epoch().count(), - _queue.front()->deployment_time()); + deployment_time.time_since_epoch().count()); - return; + RCLCPP_ERROR(_context->node()->get_logger(), "Here 10"); if (now > _queue.front()->deployment_time()) { + RCLCPP_ERROR(_context->node()->get_logger(), "Here 11"); std::lock_guard guard(_mutex); // Update state in RobotContext and Assign active task _context->state(_queue.front()->finish_state()); 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 af11700d4..dde110e2d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -22,6 +22,8 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include + namespace rmf_fleet_adapter { namespace agv { @@ -279,6 +281,26 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( const double cost = task_planner->compute_cost(assignments); + // Display assignments for debugging + std::cout << "Cost: " << cost << std::endl; + for (std::size_t i = 0; i < assignments.size(); ++i) + { + std:: cout << "--Agent: " << i << std::endl; + for (const auto& a : assignments[i]) + { + const auto& s = a.state(); + const double request_seconds = a.request()->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 double finish_seconds = finish_time.time_since_epoch().count()/1e9; + std::cout << " <" << a.request()->id() << ": " << request_seconds + << ", " << start_seconds + << ", "<< finish_seconds << ", " << 100* s.battery_soc() + << "%>" << std::endl; + } + } + std::cout << " ----------------------" << std::endl; + // Publish BidProposal rmf_task_msgs::msg::BidProposal bid_proposal; bid_proposal.fleet_name = name; diff --git a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp index 4b38ffd71..a6e820fb1 100644 --- a/rmf_task/include/rmf_task/agv/TaskPlanner.hpp +++ b/rmf_task/include/rmf_task/agv/TaskPlanner.hpp @@ -133,9 +133,9 @@ class TaskPlanner // Get a const reference to the state const State& state() const; - // Get a const reference to the time when the robot begins executing + // Get the time when the robot begins executing // this assignment - const rmf_traffic::Time& deployment_time() const; + const rmf_traffic::Time deployment_time() const; class Implementation; diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 0a8873686..cf8a3b8d0 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -150,7 +150,7 @@ const State& TaskPlanner::Assignment::state() const } //============================================================================== -const rmf_traffic::Time& TaskPlanner::Assignment::deployment_time() const +const rmf_traffic::Time TaskPlanner::Assignment::deployment_time() const { return _pimpl->deployment_time; } From db3c32c87a5a6acd7eb0c175a52f924002732cfc Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 21:02:15 +0800 Subject: [PATCH 23/50] Cleaned up printouts --- .../src/rmf_fleet_adapter/TaskManager.cpp | 34 ++----------------- 1 file changed, 3 insertions(+), 31 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index d9a19a524..3b10e2454 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -121,7 +121,6 @@ void TaskManager::set_queue( { std::lock_guard guard(_mutex); _queue.clear(); - RCLCPP_ERROR(_context->node()->get_logger(), "Here 1"); // 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) @@ -134,8 +133,6 @@ void TaskManager::set_queue( if (const auto request = std::dynamic_pointer_cast(a.request())) { - RCLCPP_ERROR(_context->node()->get_logger(), "Here 2"); - auto task = rmf_fleet_adapter::tasks::make_clean( request, _context, @@ -149,16 +146,12 @@ void TaskManager::set_queue( msg.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; this->_context->node()->task_summary()->publish(msg); - - RCLCPP_ERROR(_context->node()->get_logger(), "Here 3"); - } else if (const auto request = std::dynamic_pointer_cast( a.request())) { - RCLCPP_ERROR(_context->node()->get_logger(), "Here 4"); // const auto task = tasks::make_charge_battery() } @@ -174,9 +167,6 @@ void TaskManager::set_queue( continue; } } - - RCLCPP_ERROR(_context->node()->get_logger(), "Here 5"); - } //============================================================================== @@ -216,33 +206,14 @@ void TaskManager::_begin_next_task() return; } - RCLCPP_ERROR(_context->node()->get_logger(), "Here 6"); - + std::lock_guard guard(_mutex); const rmf_traffic::Time now = rmf_traffic_ros2::convert( _context->node()->now()); - - RCLCPP_ERROR(_context->node()->get_logger(), "Here 7"); - const auto next_task = _queue.front(); - - RCLCPP_ERROR(_context->node()->get_logger(), "Here 8"); - const auto deployment_time = next_task->deployment_time(); - RCLCPP_ERROR(_context->node()->get_logger(), "Here 9"); - - RCLCPP_INFO( - _context->node()->get_logger(), - "Time now:[%d], Next deployment time: [%d]", - now.time_since_epoch().count(), - deployment_time.time_since_epoch().count()); - - RCLCPP_ERROR(_context->node()->get_logger(), "Here 10"); - - if (now > _queue.front()->deployment_time()) + if (now > deployment_time) { - RCLCPP_ERROR(_context->node()->get_logger(), "Here 11"); - std::lock_guard guard(_mutex); // Update state in RobotContext and Assign active task _context->state(_queue.front()->finish_state()); _active_task = std::move(_queue.front()); @@ -286,6 +257,7 @@ void TaskManager::_begin_next_task() this->_context->node()->task_summary()->publish(msg); _active_task = nullptr; + // _begin_next_task(); }); From 9ab34a5543d91a25b8bbeee54b432ab28a6c5e4d Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Mon, 26 Oct 2020 21:33:37 +0800 Subject: [PATCH 24/50] Update finish time after robot has completed task --- .../src/rmf_fleet_adapter/TaskManager.cpp | 12 +++++++++--- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 2 +- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 4 ++-- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3b10e2454..4596ae77e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -119,7 +119,6 @@ agv::ConstRobotContextPtr TaskManager::context() const void TaskManager::set_queue( const std::vector& assignments) { - 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 @@ -140,6 +139,8 @@ void TaskManager::set_queue( a.deployment_time(), a.state()); + std::lock_guard guard(_mutex); + _queue.push_back(task); rmf_task_msgs::msg::TaskSummary msg; @@ -257,8 +258,12 @@ void TaskManager::_begin_next_task() this->_context->node()->task_summary()->publish(msg); _active_task = nullptr; - - // _begin_next_task(); + // Update the location and battery sock in the state in RobotContext + // TODO(YV) update the battery soc + auto& finish_state = _context->state(); + auto location = finish_state.location(); + location.time(rmf_traffic_ros2::convert(_context->node()->now())); + finish_state.location(location); }); _active_task->begin(); @@ -268,6 +273,7 @@ void TaskManager::_begin_next_task() //============================================================================== void TaskManager::clear_queue() { + std::lock_guard guard(_mutex); _queue.clear(); } 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 217ecfe66..b12a1e859 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -214,7 +214,7 @@ RobotContext& RobotContext::maximum_delay( } //============================================================================== -const rmf_task::agv::State RobotContext::state() const +rmf_task::agv::State& RobotContext::state() { return _state; } 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 5212af6bb..98c5231d6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -118,8 +118,8 @@ class RobotContext /// Set the state of this robot RobotContext& state(const rmf_task::agv::State& state); - /// Get the state of this robot - const rmf_task::agv::State state() const; + /// Get a mutable reference to the state of this robot + rmf_task::agv::State& state(); /// Get the state config of this robot const rmf_task::agv::StateConfig state_config() const; From 4b284ba2fdc38d683e32b55d6eb3a15d07be3490 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 10:16:27 +0800 Subject: [PATCH 25/50] Get latest state of robots before running task planner. TODO: Update battery soc --- .../src/rmf_fleet_adapter/TaskManager.cpp | 21 +++++++++++++------ .../src/rmf_fleet_adapter/TaskManager.hpp | 6 +++++- .../agv/FleetUpdateHandle.cpp | 2 +- 3 files changed, 21 insertions(+), 8 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 4596ae77e..6bc66ee50 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -103,6 +103,21 @@ auto TaskManager::expected_finish_location() const -> StartSet return _context->location(); } +//============================================================================== +auto TaskManager::expected_finish_state() const -> State +{ + if (_active_task) + return _context->state(); + + // Update battery soc and finish time in the current state + auto& finish_state = _context->state(); + auto location = finish_state.location(); + location.time(rmf_traffic_ros2::convert(_context->node()->now())); + finish_state.location(location); + // TODO(YV): Update battery soc + return finish_state; +} + //============================================================================== const agv::RobotContextPtr& TaskManager::context() { @@ -258,12 +273,6 @@ void TaskManager::_begin_next_task() this->_context->node()->task_summary()->publish(msg); _active_task = nullptr; - // Update the location and battery sock in the state in RobotContext - // TODO(YV) update the battery soc - auto& finish_state = _context->state(); - auto location = finish_state.location(); - location.time(rmf_traffic_ros2::convert(_context->node()->now())); - finish_state.location(location); }); _active_task->begin(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 9976ede35..672b8af2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -44,12 +44,13 @@ class TaskManager : public std::enable_shared_from_this 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; /// Add a task to the queue of this manager. void queue_task(std::shared_ptr task, Start expected_finish); /// The location where we expect this robot to be at the end of its current - /// task queue. + /// task queue StartSet expected_finish_location() const; const agv::RobotContextPtr& context(); @@ -66,6 +67,9 @@ class TaskManager : public std::enable_shared_from_this // Callback for timer which begins next task if its deployment time has passed void _begin_next_task(); + // The state of the robot. + State expected_finish_state() const; + private: TaskManager(agv::RobotContextPtr context); 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 dde110e2d..045e529c0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -258,7 +258,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( pending_requests.push_back(new_request); for (const auto& t : task_managers) { - states.push_back(t.first->state()); + states.push_back(t.second->expected_finish_state()); state_configs.push_back(t.first->state_config()); const auto requests = t.second->requests(); pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); From da560f7e778a0478aff882fa1eddfeae0620c3ff Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 13:39:46 +0800 Subject: [PATCH 26/50] Fixed bug in compute_change_of_charge() --- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 4 +- .../test/unit/agv/test_battery_drain.cpp | 54 +++++++++++++++++++ .../src/rmf_task/requests/ChargeBattery.cpp | 1 + rmf_task/src/rmf_task/requests/Clean.cpp | 12 ++--- 4 files changed, 60 insertions(+), 11 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 1a1ad3aa6..1aa8d239c 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -105,11 +105,11 @@ double SimpleMotionPowerSink::compute_change_in_charge( { const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - const double w = velocity[2]; + const double w = std::abs(velocity[2]); const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); - const double alpha = acceleration[2]; + const double alpha = std::abs(acceleration[2]); // Loss through acceleration const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 928037d71..ed1d99eed 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -241,3 +241,57 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") CHECK(ok); } } + + +SCENARIO("Testing Cleaning Request") +{ + using BatterySystem = rmf_battery::agv::BatterySystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + using namespace std::chrono_literals; + + // Initializing system traits + BatterySystem battery_system{24, 40, 8.8}; + REQUIRE(battery_system.valid()); + MechanicalSystem mechanical_system{70, 40, 0.22}; + REQUIRE(mechanical_system.valid()); + PowerSystem power_system_1{"processor", 20}; + REQUIRE(power_system_1.valid()); + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.4, 1.0}, {nullptr, nullptr}); + + const double initial_soc = 1.0; + + WHEN("Computing invariant drain for zone_3") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{104.0, -46.92, -M_PI/2.0}, + Eigen::Vector3d{104.0, -48.55, 0.0}, + Eigen::Vector3d{159.8, -48.38, M_PI/2.0}, + Eigen::Vector3d{159.8, -46.73, M_PI}, + Eigen::Vector3d{105.4, -47.04, M_PI/2.0}, + Eigen::Vector3d{105.5, -45.37, 0.0}, + Eigen::Vector3d{159.8, -45.25, 0.0}, + Eigen::Vector3d{155.0, -46.79, -M_PI/2.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); + + std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl; + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + REQUIRE(remaining_soc <= 1.0); + } +} \ No newline at end of file diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 94913bf2c..dca5c0881 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -143,6 +143,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( state.finish_time( wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); + state.battery_soc(_pimpl->_charge_soc); return Estimate(state, wait_until); diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 03fec418d..18b8bff25 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -90,7 +90,7 @@ rmf_task::ConstRequestPtr Clean::make( clean->_pimpl->cleaning_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(clean->_pimpl->invariant_duration)); clean->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_ambient + - dSOC_cleaning; + dSOC_cleaning; } return clean; @@ -130,15 +130,10 @@ rmf_utils::optional Clean::estimate_finish( if (initial_state.waypoint() != _pimpl->start_waypoint) { - // Compute plan to cleaning start waypoint along with battery drain - rmf_traffic::agv::Planner::Start start{ - start_time, - initial_state.waypoint(), - 0.0}; - rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; - const auto result_to_start = _pimpl->planner->plan(start, goal); + const auto result_to_start = _pimpl->planner->plan( + initial_state.location(), goal); // We assume we can always compute a plan const auto& trajectory = result_to_start->get_itinerary().back().trajectory(); @@ -153,7 +148,6 @@ rmf_utils::optional Clean::estimate_finish( _pimpl->ambient_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_ambient; - if (battery_soc <= state_config.threshold_soc()) return rmf_utils::nullopt; } From a4c2182476ed8c841980a73e7acb7585ef250d21 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 13:45:18 +0800 Subject: [PATCH 27/50] Using magnitude of w and alpha in compute_change_in_charge() --- .../rmf_battery/agv/SimpleMotionPowerSink.cpp | 4 +- .../test/unit/agv/test_battery_drain.cpp | 53 +++++++++++++++++++ 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp index 1a1ad3aa6..1aa8d239c 100644 --- a/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp +++ b/rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp @@ -105,11 +105,11 @@ double SimpleMotionPowerSink::compute_change_in_charge( { const Eigen::Vector3d velocity = motion->compute_velocity(sim_time); const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2)); - const double w = velocity[2]; + const double w = std::abs(velocity[2]); const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time); const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2)); - const double alpha = acceleration[2]; + const double alpha = std::abs(acceleration[2]); // Loss through acceleration const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step; diff --git a/rmf_battery/test/unit/agv/test_battery_drain.cpp b/rmf_battery/test/unit/agv/test_battery_drain.cpp index 928037d71..65ca402f3 100644 --- a/rmf_battery/test/unit/agv/test_battery_drain.cpp +++ b/rmf_battery/test/unit/agv/test_battery_drain.cpp @@ -241,3 +241,56 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB") CHECK(ok); } } + +SCENARIO("Testing Cleaning Request") +{ + using BatterySystem = rmf_battery::agv::BatterySystem; + using MechanicalSystem = rmf_battery::agv::MechanicalSystem; + using PowerSystem = rmf_battery::agv::PowerSystem; + using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink; + using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink; + using namespace std::chrono_literals; + + // Initializing system traits + BatterySystem battery_system{24, 40, 8.8}; + REQUIRE(battery_system.valid()); + MechanicalSystem mechanical_system{70, 40, 0.22}; + REQUIRE(mechanical_system.valid()); + PowerSystem power_system_1{"processor", 20}; + REQUIRE(power_system_1.valid()); + SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system}; + SimpleDevicePowerSink device_power_sink{battery_system, power_system_1}; + + // Initializing vehicle traits + const rmf_traffic::agv::VehicleTraits traits( + {0.7, 0.5}, {0.4, 1.0}, {nullptr, nullptr}); + + const double initial_soc = 1.0; + + WHEN("Computing invariant drain for zone_3") + { + const auto start_time = std::chrono::steady_clock::now(); + const std::vector positions = { + Eigen::Vector3d{104.0, -46.92, -M_PI/2.0}, + Eigen::Vector3d{104.0, -48.55, 0.0}, + Eigen::Vector3d{159.8, -48.38, M_PI/2.0}, + Eigen::Vector3d{159.8, -46.73, M_PI}, + Eigen::Vector3d{105.4, -47.04, M_PI/2.0}, + Eigen::Vector3d{105.5, -45.37, 0.0}, + Eigen::Vector3d{159.8, -45.25, 0.0}, + Eigen::Vector3d{155.0, -46.79, -M_PI/2.0}, + }; + rmf_traffic::Trajectory trajectory = + rmf_traffic::agv::Interpolate::positions(traits, start_time, positions); + + const double dSOC_motion = motion_power_sink.compute_change_in_charge( + trajectory); + const double dSOC_device = device_power_sink.compute_change_in_charge( + rmf_traffic::time::to_seconds(trajectory.duration())); + + // std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl; + + const double remaining_soc = initial_soc - dSOC_motion - dSOC_device; + REQUIRE(remaining_soc <= 1.0); + } +} \ No newline at end of file From 6430c228be7fccda1e97f3141870635ca45693b9 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 15:07:17 +0800 Subject: [PATCH 28/50] Added update_battery_soc() to RobotUpdateHandle which updates the current_battery_soc in RobotContext. Initial states for task planning use this updated battery_soc --- .../rmf_fleet_adapter/agv/RobotUpdateHandle.hpp | 4 ++++ rmf_fleet_adapter/src/full_control/main.cpp | 5 +++++ .../src/rmf_fleet_adapter/TaskManager.cpp | 7 ++++++- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++++++ .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 3 +++ .../rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 16 ++++++++++++++++ 6 files changed, 40 insertions(+), 1 deletion(-) 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 f0ad0937d..9008ebd3f 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -88,6 +88,10 @@ class RobotUpdateHandle const double max_merge_lane_distance = 1.0, const double min_lane_length = 1e-8); + /// Update the current battery level of the robot by specifying its state of + /// charge as a fraction of its total charge capacity + void update_battery_soc(const double battery_soc); + /// Specify how high the delay of the current itinerary can become before it /// gets interrupted and replanned. A nullopt value will allow for an /// arbitrarily long delay to build up without being interrupted. diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 88047e332..a22c18dad 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -331,6 +331,11 @@ class FleetDriverRobotCommandHandle // command estimate_state(_node, state.location, _travel_info); } + + // Update battery soc + const double battery_soc = state.battery_percent / 100.0; + if (battery_soc >= 0.0 && battery_soc <= 1.0) + _travel_info.updater->update_battery_soc(battery_soc); } void set_updater(rmf_fleet_adapter::agv::RobotUpdateHandlePtr updater) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 6bc66ee50..7e844dde6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -106,6 +106,8 @@ auto TaskManager::expected_finish_location() const -> StartSet //============================================================================== auto TaskManager::expected_finish_state() const -> State { + // 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->state(); @@ -114,7 +116,10 @@ auto TaskManager::expected_finish_state() const -> State auto location = finish_state.location(); location.time(rmf_traffic_ros2::convert(_context->node()->now())); finish_state.location(location); - // TODO(YV): Update battery soc + + const double current_battery_soc = _context->current_battery_soc(); + finish_state.battery_soc(current_battery_soc); + return finish_state; } 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 b12a1e859..8ed899582 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -233,6 +233,12 @@ const rmf_task::agv::StateConfig RobotContext::state_config() const return _state_config; } +//============================================================================== +const double RobotContext::current_battery_soc() const +{ + return _current_battery_soc; +} + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, 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 98c5231d6..495ae4d28 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -124,6 +124,8 @@ class RobotContext /// Get the state config of this robot const rmf_task::agv::StateConfig state_config() const; + const double current_battery_soc() const; + private: friend class FleetUpdateHandle; friend class RobotUpdateHandle; @@ -159,6 +161,7 @@ class RobotContext rmf_traffic::schedule::Negotiator* _negotiator = nullptr; + double _current_battery_soc; rmf_task::agv::State _state; rmf_task::agv::StateConfig _state_config; }; 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 228bfb6cf..9d5f4ca8b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -162,6 +162,22 @@ void RobotUpdateHandle::update_position( } } +//============================================================================== +void RobotUpdateHandle::update_battery_soc(const double battery_soc) +{ + if (battery_soc < 0.0 || battery_soc > 1.0) + return; + + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [context, battery_soc](const auto&) + { + context->_current_battery_soc = battery_soc; + }); + } +} + //============================================================================== RobotUpdateHandle& RobotUpdateHandle::maximum_delay( rmf_utils::optional value) From a422007a3075871347706e89a2a0f54e5de3fe4e Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 27 Oct 2020 16:17:14 +0800 Subject: [PATCH 29/50] Added ChargeBattery task factory --- .../agv/FleetUpdateHandle.cpp | 7 +-- .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 48 +++++++++++++++++++ .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 44 +++++++++++++++++ .../rmf_task/requests/ChargeBattery.hpp | 3 +- 4 files changed, 95 insertions(+), 7 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp 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 045e529c0..02e283fc3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -137,11 +137,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( std::string id = msg->task_profile.task_id; const auto& graph = planner->get_configuration().graph(); - // RCLCPP_INFO( - // node->get_logger(), - // "Fleet [%s] is processing BidNotice with task_id:[%s] and type:[%d]...", - // name.c_str(), id.c_str(), task_type.value); - // Process Cleaning task if (task_type.value == rmf_task_msgs::msg::TaskType::CLEANING_TASK) { @@ -250,7 +245,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( if (!new_request) return; - // Update robot states and combine new requestptr with requestptr of + // Collate robot states and combine new requestptr with requestptr of // non-charging tasks in task manager queues std::vector states; std::vector state_configs; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp new file mode 100644 index 000000000..a4822a0ad --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -0,0 +1,48 @@ +/* + * 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 "../phases/GoToPlace.hpp" + +#include "ChargeBattery.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_charge_battery( + const rmf_task::requests::ConstChargeBatteryRequest 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) +{ + rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint())}; + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make(context, std::move(start), goal)); + + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} + +} // 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 new file mode 100644 index 000000000..d6c4264cc --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -0,0 +1,44 @@ +/* + * 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_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP +#define SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include +#include + +#include +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_charge_battery( + const rmf_task::requests::ConstChargeBatteryRequest 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); + +} // namespace tasks +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__TASKS__CHARGEBATTERY_HPP diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index a5500dd26..98ba910c1 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -62,7 +62,8 @@ class ChargeBattery : public rmf_task::Request rmf_utils::impl_ptr _pimpl; }; -using ChargeBatteryPtr = std::shared_ptr; +using ChargeBatteryRequestPtr = std::shared_ptr; +using ConstChargeBatteryRequestPtr = std::shared_ptr; } // namespace requests } // namespace rmf_task From 2fa920e5a7525648cb30b45f124dafaf80e60d37 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 27 Oct 2020 19:09:24 +0800 Subject: [PATCH 30/50] Added skeleton for WaitForCharge --- .../phases/WaitForCharge.cpp | 138 ++++++++++++++++++ .../phases/WaitForCharge.hpp | 109 ++++++++++++++ .../rmf_fleet_adapter/tasks/ChargeBattery.cpp | 11 +- .../rmf_fleet_adapter/tasks/ChargeBattery.hpp | 2 +- .../rmf_task/requests/ChargeBattery.hpp | 2 + .../src/rmf_task/requests/ChargeBattery.cpp | 5 + 6 files changed, 263 insertions(+), 4 deletions(-) create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp create mode 100644 rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp new file mode 100644 index 000000000..dd60eb12f --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -0,0 +1,138 @@ +/* + * 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 "WaitForCharge.hpp" + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +auto WaitForCharge::Active::observe() const -> const rxcpp::observable& +{ + return _status_obs; +} + +//============================================================================== +rmf_traffic::Duration WaitForCharge::Active::estimate_remaining_time() const +{ + const double capacity = _battery_system.capacity(); + const double charging_current = _battery_system.charging_current(); + const double time_estimate = + 3600.0 * capacity * (_charge_to_soc - _context->current_battery_soc()) / charging_current; + + return rmf_traffic::time::from_seconds(time_estimate); +} + +//============================================================================== +void WaitForCharge::Active::emergency_alarm(const bool value) +{ + // Assume charging station is a holding point +} + +//============================================================================== +void WaitForCharge::Active::cancel() +{ + // TODO +} + +//============================================================================== +const std::string& WaitForCharge::Active::description() const +{ + return _description; +} + +//============================================================================== +WaitForCharge::Active::Active( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc) +: _context(std::move(context)), + _battery_system(battery_system), + _charge_to_soc(charge_to_soc) +{ + _description = "Charging [" + _context->requester_id() + "] to [" + + std::to_string(100.0 * _charge_to_soc) + "]"; + + + StatusMsg initial_msg; + initial_msg.status = _description; + _status_publisher.get_subscriber().on_next(initial_msg); + const auto now = _context->node()->now(); + initial_msg.start_time = now; + initial_msg.end_time = now + estimate_remaining_time(); + + _status_obs = _status_publisher + .get_observable() + .start_with(initial_msg); + +} + +//============================================================================== +std::shared_ptr WaitForCharge::Pending::begin() +{ + auto active = + std::shared_ptr(new Active( + _context, _battery_system, _charge_to_soc)); + + return active; +} + +//============================================================================== +rmf_traffic::Duration WaitForCharge::Pending::estimate_phase_duration() const +{ + return rmf_traffic::time::from_seconds(_time_estimate); +} + +//============================================================================== +const std::string& WaitForCharge::Pending::description() const +{ + return _description; +} + +//============================================================================== +WaitForCharge::Pending::Pending( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc, + double time_estimate) +: _context(std::move(context)), + _battery_system(battery_system), + _charge_to_soc(charge_to_soc), + _time_estimate(time_estimate) +{ + _description = + "Charging robot to [" + std::to_string(100.0 * charge_to_soc) + "%]"; +} + +//============================================================================== +auto WaitForCharge::make( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc) -> std::unique_ptr +{ + + const double capacity = battery_system.capacity(); + const double charging_current = battery_system.charging_current(); + const double time_estimate = + 3600.0 * capacity * (charge_to_soc - context->current_battery_soc()) / charging_current; + + return std::unique_ptr( + new Pending(context, battery_system, charge_to_soc, time_estimate)); +} + +} // namespace phases +} // namespace rmf_fleet_adapter \ No newline at end of file diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp new file mode 100644 index 000000000..0664ec179 --- /dev/null +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -0,0 +1,109 @@ +/* + * 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_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP +#define SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP + +#include "../Task.hpp" +#include "../agv/RobotContext.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace phases { + +//============================================================================== +class WaitForCharge +{ +public: + + using StatusMsg = Task::StatusMsg; + class Pending; + + class Active : public Task::ActivePhase + { + public: + + // Documentation inherited from ActivePhase + const rxcpp::observable& observe() const final; + + // Documentation inherited from ActivePhase + rmf_traffic::Duration estimate_remaining_time() const final; + + // Documentation inherited from ActivePhase + void emergency_alarm(bool on) final; + + // Documentation inherited from ActivePhase + void cancel() final; + + // Documentation inherited from ActivePhase + const std::string & description() const final; + + private: + friend class Pending; + Active( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc); + + agv::RobotContextPtr _context; + rmf_battery::agv::BatterySystem _battery_system; + double _charge_to_soc; + std::string _description; + rxcpp::observable _status_obs; + rxcpp::subjects::subject _status_publisher; + + }; + + class Pending : public Task::PendingPhase + { + public: + // Documentation inherited + std::shared_ptr begin() final; + + // Documentation inherited + rmf_traffic::Duration estimate_phase_duration() const final; + + // Documentation inherited + const std::string& description() const final; + + private: + friend class WaitForCharge; + Pending( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc, + double time_estimate); + + agv::RobotContextPtr _context; + rmf_battery::agv::BatterySystem _battery_system; + double _charge_to_soc; + std::string _description; + double _time_estimate; + }; + + static std::unique_ptr make( + agv::RobotContextPtr context, + rmf_battery::agv::BatterySystem battery_system, + double charge_to_soc); + +}; + +} // namespace phases +} // namespace rmf_fleet_adapter + +#endif // SRC__RMF_FLEET_ADAPTER__PHASES__WAITFORCHARGE_HPP \ No newline at end of file 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 a4822a0ad..d8fe0549f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -17,6 +17,8 @@ #include "../phases/GoToPlace.hpp" +#include "../phases/WaitForCharge.hpp" + #include "ChargeBattery.hpp" namespace rmf_fleet_adapter { @@ -24,16 +26,19 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( - const rmf_task::requests::ConstChargeBatteryRequest request, + const rmf_task::requests::ConstChargeBatteryRequestPtr 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) { - rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint())}; + rmf_traffic::agv::Planner::Goal goal{finish_state.charging_waypoint()}; + Task::PendingPhases phases; phases.push_back( - phases::GoToPlace::make(context, std::move(start), goal)); + phases::GoToPlace::make(context, std::move(start), goal)); + // phases.push_back( + // phases::WaitForCharge::make(context, request->battery_system(), 1.0)); return Task::make( std::to_string(request->id()), 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 d6c4264cc..66ffef52d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.hpp @@ -32,7 +32,7 @@ namespace tasks { //============================================================================== std::shared_ptr make_charge_battery( - const rmf_task::requests::ConstChargeBatteryRequest request, + const rmf_task::requests::ConstChargeBatteryRequestPtr request, const agv::RobotContextPtr& context, const rmf_traffic::agv::Plan::Start start, const rmf_traffic::Time deployment_time, diff --git a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp index 98ba910c1..0435bfea4 100644 --- a/rmf_task/include/rmf_task/requests/ChargeBattery.hpp +++ b/rmf_task/include/rmf_task/requests/ChargeBattery.hpp @@ -56,6 +56,8 @@ class ChargeBattery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + const rmf_battery::agv::BatterySystem& battery_system() const; + class Implementation; private: ChargeBattery(); diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index dca5c0881..acf4c63a6 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -162,5 +162,10 @@ rmf_traffic::Time ChargeBattery::earliest_start_time() const } //============================================================================== +const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const +{ + return _pimpl->_battery_system; +} + } // namespace requests } // namespace rmf_task From 9cb4a3184bc3998ba0c40295f0f093f2c522dffc Mon Sep 17 00:00:00 2001 From: Rushyendra Maganty Date: Tue, 27 Oct 2020 19:13:06 +0800 Subject: [PATCH 31/50] Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. --- rmf_task/src/rmf_task/requests/ChargeBattery.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 1d34fb9e2..fa8fdb680 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -116,7 +116,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( const auto result = _pimpl->_planner->plan(start, goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - const rmf_traffic::Duration variant_duration = finish_time - start_time; + variant_duration = finish_time - start_time; if (_pimpl->_drain_battery) { From 77d4ac139d1b300403a3078562f0e256632d84e4 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Tue, 27 Oct 2020 19:30:42 +0800 Subject: [PATCH 32/50] Fixed return value of battery_system() in ChargeBattery --- .../src/rmf_fleet_adapter/tasks/ChargeBattery.cpp | 4 ++-- rmf_task/src/rmf_task/requests/ChargeBattery.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 d8fe0549f..0414a38a2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/ChargeBattery.cpp @@ -37,8 +37,8 @@ std::shared_ptr make_charge_battery( Task::PendingPhases phases; phases.push_back( phases::GoToPlace::make(context, std::move(start), goal)); - // phases.push_back( - // phases::WaitForCharge::make(context, request->battery_system(), 1.0)); + phases.push_back( + phases::WaitForCharge::make(context, request->battery_system(), 1.0)); return Task::make( std::to_string(request->id()), diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index ce9f6202f..855976a2c 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -164,7 +164,7 @@ rmf_traffic::Time ChargeBattery::earliest_start_time() const //============================================================================== const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const { - return _pimpl->_battery_system; + return *_pimpl->_battery_system; } } // namespace requests From 5dba5ed739a9723c0656a57f5b4293dc1295fbce Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 28 Oct 2020 11:14:59 +0800 Subject: [PATCH 33/50] Catch exception for invalid cleaning trajectories --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 13 +++++++++++-- rmf_task/src/rmf_task/requests/Clean.cpp | 1 + 2 files changed, 12 insertions(+), 2 deletions(-) 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 02e283fc3..57af0e508 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -195,12 +195,21 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( 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); + 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()); + + return; + } // TODO(YV) get rid of id field in RequestPtr std::stringstream id_stream(id); diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 18b8bff25..7327ecfcb 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -89,6 +89,7 @@ rmf_task::ConstRequestPtr Clean::make( const double dSOC_cleaning = clean->_pimpl->cleaning_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(clean->_pimpl->invariant_duration)); + clean->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_ambient + dSOC_cleaning; } From bee2fa8233cf3b82692bfc2123ff7e206ce7ca05 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 28 Oct 2020 14:20:24 +0800 Subject: [PATCH 34/50] Populate finish_time in BidProposal --- .../agv/FleetUpdateHandle.cpp | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) 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 57af0e508..fd0681a9e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -283,6 +283,15 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( pending_requests, nullptr); + if (assignments.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Failed to compute assignments for task_id:[%s]", id.c_str()); + + return; + } + const double cost = task_planner->compute_cost(assignments); // Display assignments for debugging @@ -311,8 +320,16 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( bid_proposal.task_profile = task_profile; bid_proposal.prev_cost = current_assignment_cost; bid_proposal.new_cost = cost; - // TODO populate finish_time and robot_name - + // TODO robot_name + for (const auto& agent : assignments) + { + for (const auto& assignment : agent) + { + if (std::to_string(assignment.request()->id()) == id) + bid_proposal.finish_time = rmf_traffic_ros2::convert( + assignment.state().finish_time()); + } + } bid_proposal_pub->publish(bid_proposal); RCLCPP_INFO( node->get_logger(), @@ -337,6 +354,11 @@ void FleetUpdateHandle::Implementation::dispatch_request_cb( if (task_it == bid_notice_assignments.end()) return; + RCLCPP_INFO( + node->get_logger(), + "Bid for task_id:[%s] awarded to fleet [%s]", + id.c_str(), name.c_str()); + // We currently only support adding tasks if (msg->method != DispatchRequest::ADD) return; From ec399d98621c385f5dd88a72b274373e0020a0d1 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 28 Oct 2020 17:18:11 +0800 Subject: [PATCH 35/50] Added observable and publisher for current_battery_soc --- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 17 ++++++++++++++++- .../src/rmf_fleet_adapter/agv/RobotContext.hpp | 13 ++++++++++++- .../rmf_fleet_adapter/agv/RobotUpdateHandle.cpp | 2 +- 3 files changed, 29 insertions(+), 3 deletions(-) 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 8ed899582..ba6441fdd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -234,11 +234,24 @@ const rmf_task::agv::StateConfig RobotContext::state_config() const } //============================================================================== -const double RobotContext::current_battery_soc() const +double RobotContext::current_battery_soc() const { return _current_battery_soc; } +RobotContext& RobotContext::current_battery_soc(const double battery_soc) +{ + _current_battery_soc = battery_soc; + _battery_soc_publisher.get_subscriber().on_next(battery_soc); +} + +//============================================================================== +const rxcpp::observable& RobotContext::observe_battery_soc() const +{ + return _battery_soc_obs; +} + + //============================================================================== void RobotContext::respond( const TableViewerPtr& table_viewer, @@ -287,6 +300,8 @@ RobotContext::RobotContext( _itinerary.description().profile()); _interrupt_obs = _interrupt_publisher.get_observable(); + + _battery_soc_obs = _battery_soc_publisher.get_observable(); } } // 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 495ae4d28..95d274960 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -124,7 +124,15 @@ class RobotContext /// Get the state config of this robot const rmf_task::agv::StateConfig state_config() const; - const double current_battery_soc() const; + /// Get the current battery state of charge + double current_battery_soc() const; + + /// Set the current battery state of charge. Note: This function also + /// publishes the battery soc via _battery_soc_publisher. + RobotContext& current_battery_soc(const double battery_soc); + + // Get a reference to the battery soc observer of this robot. + const rxcpp::observable& observe_battery_soc() const; private: friend class FleetUpdateHandle; @@ -161,7 +169,10 @@ class RobotContext rmf_traffic::schedule::Negotiator* _negotiator = nullptr; + /// Always call the current_battery_soc() setter to set a new value double _current_battery_soc; + rxcpp::subjects::subject _battery_soc_publisher; + rxcpp::observable _battery_soc_obs; rmf_task::agv::State _state; rmf_task::agv::StateConfig _state_config; }; 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 9d5f4ca8b..866992d87 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 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc) context->worker().schedule( [context, battery_soc](const auto&) { - context->_current_battery_soc = battery_soc; + context->current_battery_soc(battery_soc); }); } } From b0486c6346cd021a7106455ef4f9fad341734458 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 28 Oct 2020 17:53:07 +0800 Subject: [PATCH 36/50] Updated begin() for WaitForCharge phase --- .../rmf_fleet_adapter/phases/WaitForCharge.cpp | 16 ++++++++++++++++ .../rmf_fleet_adapter/phases/WaitForCharge.hpp | 5 ++++- 2 files changed, 20 insertions(+), 1 deletion(-) 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 dd60eb12f..fcbed0eee 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -88,6 +88,22 @@ std::shared_ptr WaitForCharge::Pending::begin() std::shared_ptr(new Active( _context, _battery_system, _charge_to_soc)); + active->_battery_soc_subscription = _context->observe_battery_soc() + .observe_on(rxcpp::identity_same_worker(_context->worker())) + .subscribe( + [a = active->weak_from_this()](const auto&) + { + const auto active = a.lock(); + + if(std::abs(active->_context->current_battery_soc() - + active->_charge_to_soc) < 1e-3) + { + active->_status_publisher.get_subscriber().on_completed(); + } + // TODO Publish warning message to alert user if battery is not + // charging at expected rate + }); + return active; } 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 0664ec179..1bb2faa73 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -34,7 +34,9 @@ class WaitForCharge using StatusMsg = Task::StatusMsg; class Pending; - class Active : public Task::ActivePhase + class Active + : public Task::ActivePhase, + public std::enable_shared_from_this { public: @@ -66,6 +68,7 @@ class WaitForCharge std::string _description; rxcpp::observable _status_obs; rxcpp::subjects::subject _status_publisher; + rmf_rxcpp::subscription_guard _battery_soc_subscription; }; From 20262eea3440475e394b8a584b62a987016c39b6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 29 Oct 2020 10:45:47 +0800 Subject: [PATCH 37/50] Fixed completion logic in WaitForCharge --- rmf_fleet_adapter/src/full_control/main.cpp | 4 +++- .../src/rmf_fleet_adapter/phases/WaitForCharge.cpp | 8 +++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index a22c18dad..5e6b96061 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -315,7 +315,9 @@ class FleetDriverRobotCommandHandle *_travel_info.traits, rmf_traffic_ros2::convert(state.location.t), positions); - assert(trajectory.size() > 1); + + if (trajetory.size() < 2) + return; if (auto participant = _travel_info.updater->get_participant()) { 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 fcbed0eee..15a2454f9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -91,12 +91,14 @@ std::shared_ptr WaitForCharge::Pending::begin() active->_battery_soc_subscription = _context->observe_battery_soc() .observe_on(rxcpp::identity_same_worker(_context->worker())) .subscribe( - [a = active->weak_from_this()](const auto&) + [a = active->weak_from_this()](const double battery_soc) { const auto active = a.lock(); - if(std::abs(active->_context->current_battery_soc() - - active->_charge_to_soc) < 1e-3) + if (!active) + return; + + if(active->_charge_to_soc <= battery_soc) { active->_status_publisher.get_subscriber().on_completed(); } From 060c65cb873e7fc8d659147084c719daa471d500 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 29 Oct 2020 10:47:35 +0800 Subject: [PATCH 38/50] Fixed typo --- rmf_fleet_adapter/src/full_control/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 5e6b96061..9262fd8d7 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -316,7 +316,7 @@ class FleetDriverRobotCommandHandle rmf_traffic_ros2::convert(state.location.t), positions); - if (trajetory.size() < 2) + if (trajectory.size() < 2) return; if (auto participant = _travel_info.updater->get_participant()) From 136e8bcbb844ef8d2139c949a6246d08b1af1dd6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 29 Oct 2020 13:01:40 +0800 Subject: [PATCH 39/50] Updated fleet adapter launch params. Populate task id in TaskProfile in Status msg --- .../launch/fleet_adapter.launch.xml | 25 +++++++++++++++++++ rmf_fleet_adapter/src/full_control/main.cpp | 4 +-- .../src/rmf_fleet_adapter/TaskManager.cpp | 5 ++++ 3 files changed, 32 insertions(+), 2 deletions(-) diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml index 002b4c9c4..2a375c15e 100644 --- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml +++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml @@ -24,6 +24,19 @@ + + + + + + + + + + + + + + + @@ -50,6 +65,16 @@ + + + + + + + + + + diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index 9262fd8d7..d3d9e4c2e 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -569,7 +569,7 @@ std::shared_ptr make_fleet( } std::unordered_set task_types; - if (node->declare_parameter("perform_loop", true)) + if (node->declare_parameter("perform_loop", false)) { task_types.insert(rmf_task_msgs::msg::TaskType::LOOP_TASK); } @@ -583,7 +583,7 @@ std::shared_ptr make_fleet( [](const rmf_task_msgs::msg::Delivery&){ return true; }); } - if (node->declare_parameter("perform_cleaning", true)) + if (node->declare_parameter("perform_cleaning", false)) { task_types.insert(rmf_task_msgs::msg::TaskType::CLEANING_TASK); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 7e844dde6..3608035fa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -89,6 +89,7 @@ void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) { rmf_task_msgs::msg::TaskSummary msg; msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; this->_context->node()->task_summary()->publish(msg); } @@ -165,6 +166,7 @@ void TaskManager::set_queue( rmf_task_msgs::msg::TaskSummary msg; msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; this->_context->node()->task_summary()->publish(msg); } @@ -252,6 +254,7 @@ void TaskManager::_begin_next_task() [this, id = _active_task->id()](Task::StatusMsg msg) { msg.task_id = id; + msg.task_profile.task_id = id; _context->node()->task_summary()->publish(msg); }, [this, id = _active_task->id()](std::exception_ptr e) @@ -267,6 +270,7 @@ void TaskManager::_begin_next_task() } msg.task_id = id; + msg.task_profile.task_id = id; _context->node()->task_summary()->publish(msg); // _begin_next_task(); }, @@ -274,6 +278,7 @@ void TaskManager::_begin_next_task() { rmf_task_msgs::msg::TaskSummary msg; msg.task_id = id; + msg.task_profile.task_id = id; msg.state = msg.STATE_COMPLETED; this->_context->node()->task_summary()->publish(msg); From 9378ed0a3d4c400275df1601226fc81e988662c2 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Thu, 29 Oct 2020 14:57:00 +0800 Subject: [PATCH 40/50] Become stubborn while docking --- .../src/rmf_fleet_adapter/phases/GoToPlace.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 281cff7d1..83bb0bb2b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/GoToPlace.cpp @@ -22,6 +22,8 @@ #include "RequestLift.hpp" #include "DockRobot.hpp" +#include + namespace rmf_fleet_adapter { namespace phases { @@ -89,6 +91,16 @@ void GoToPlace::Active::respond( const TableViewerPtr& table_viewer, const ResponderPtr& responder) { + if (_subtasks) + { + if (dynamic_cast(_subtasks->current_phase().get())) + { + rmf_traffic::schedule::StubbornNegotiator(_context->itinerary()) + .respond(table_viewer, responder); + return; + } + } + auto approval_cb = [w = weak_from_this()]( const rmf_traffic::agv::Plan& plan) -> rmf_utils::optional From 1c2e149fbd65b3fa7817c256807540884dbbe463 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 30 Oct 2020 14:43:47 +0800 Subject: [PATCH 41/50] Added recharge_threshold parameter to fleet adapter launch --- .../include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp | 8 ++++---- rmf_fleet_adapter/launch/fleet_adapter.launch.xml | 4 +++- rmf_fleet_adapter/src/full_control/main.cpp | 8 +++++++- 3 files changed, 14 insertions(+), 6 deletions(-) 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 d556544b8..a12628e08 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -96,10 +96,10 @@ class FleetUpdateHandle : public std::enable_shared_from_this const bool drain_battery); - /// Set the threshold below which the robot should automatically head back to - /// its charging dock. The user is responsible to set this value such that the - /// robot is capable of reaching its nearest charging station from anywhere - /// on the map. Default value is 0.2. + /// Set the threshold state of charge below which the robot should + /// automatically head back to its charging dock. The user is responsible to + /// set this value such that the robot is capable of reaching its nearest + /// charging station from anywhere on the map. Default value is 0.2. /// /// \param[in] threshold /// The fraction of the total battery capacity diff --git a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml index 2a375c15e..d086b66ed 100644 --- a/rmf_fleet_adapter/launch/fleet_adapter.launch.xml +++ b/rmf_fleet_adapter/launch/fleet_adapter.launch.xml @@ -35,7 +35,8 @@ - + + + diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index d3d9e4c2e..d996d7d80 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -556,7 +556,13 @@ std::shared_ptr make_fleet( // Drain battery const bool drain_battery = rmf_fleet_adapter::get_parameter_or_default( - *node, "drain_battery", true); + *node, "drain_battery", false); + + // Recharge threshold + const double recharge_threshold = rmf_fleet_adapter::get_parameter_or_default( + *node, "recharge_threshold", 0.2); + + connections->fleet->set_recharge_threshold(recharge_threshold); if (!connections->fleet->set_task_planner_params( battery_system, motion_sink, ambient_sink, tool_sink, drain_battery)) From 4a6081fb17c74f7c9e559048ce4ee3e427bc6784 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Fri, 30 Oct 2020 16:18:11 +0800 Subject: [PATCH 42/50] Fix missing return value in current_battery_soc() --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 ba6441fdd..4b322710c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -243,6 +243,8 @@ RobotContext& RobotContext::current_battery_soc(const double battery_soc) { _current_battery_soc = battery_soc; _battery_soc_publisher.get_subscriber().on_next(battery_soc); + + return *this; } //============================================================================== From 21fe3d5092789cbe87b33537e208e2d7849404cf Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 3 Nov 2020 11:03:38 +0800 Subject: [PATCH 43/50] Populate robot_name in BidProposal msg --- .../agv/FleetUpdateHandle.cpp | 19 +++++++++++++++++-- .../agv/internal_FleetUpdateHandle.hpp | 2 +- 2 files changed, 18 insertions(+), 3 deletions(-) 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 fd0681a9e..bba9a36ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -23,6 +23,7 @@ #include "../tasks/Loop.hpp" #include +#include namespace rmf_fleet_adapter { namespace agv { @@ -260,12 +261,18 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( std::vector state_configs; std::vector pending_requests; pending_requests.push_back(new_request); + // Map robot index to name for BidProposal + std::unordered_map robot_name_map; + std::size_t index = 0; for (const auto& t : task_managers) { states.push_back(t.second->expected_finish_state()); state_configs.push_back(t.first->state_config()); const auto requests = t.second->requests(); pending_requests.insert(pending_requests.end(), requests.begin(), requests.end()); + + robot_name_map.insert({index, t.first->name()}); + ++index; } RCLCPP_INFO( @@ -320,15 +327,21 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( bid_proposal.task_profile = task_profile; bid_proposal.prev_cost = current_assignment_cost; bid_proposal.new_cost = cost; - // TODO robot_name + index = 0; for (const auto& agent : assignments) { for (const auto& assignment : agent) { if (std::to_string(assignment.request()->id()) == id) + { bid_proposal.finish_time = rmf_traffic_ros2::convert( assignment.state().finish_time()); + if (robot_name_map.find(index) != robot_name_map.end()) + bid_proposal.robot_name = robot_name_map[index]; + break; + } } + ++index; } bid_proposal_pub->publish(bid_proposal); RCLCPP_INFO( @@ -617,7 +630,7 @@ std::size_t FleetUpdateHandle::Implementation::get_nearest_charger( p = *start.location(); double min_dist = std::numeric_limits::max(); - std::size_t nearest_charger; + std::size_t nearest_charger = 0; for (const auto& wp : charging_waypoints) { const auto loc = graph.get_waypoint(wp).get_location(); @@ -773,6 +786,8 @@ bool FleetUpdateHandle::set_task_planner_params( _pimpl->task_planner = std::make_shared( task_config); + + _pimpl->drain_battery = drain_battery; _pimpl->initialized_task_planner = true; 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 55ee0f35b..1b1be2859 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 @@ -139,7 +139,7 @@ class FleetUpdateHandle::Implementation std::shared_ptr motion_sink = nullptr; std::shared_ptr ambient_sink = nullptr; std::shared_ptr tool_sink = nullptr; - const bool drain_battery = true; + bool drain_battery = true; std::shared_ptr task_planner = nullptr; bool initialized_task_planner = false; From f21883ac51d6336985518c8972c6f6847cfaff44 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 3 Nov 2020 11:19:32 +0800 Subject: [PATCH 44/50] TaskManager queues charging tasks --- .../src/rmf_fleet_adapter/TaskManager.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 3608035fa..6b8f3d3ad 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -24,6 +24,7 @@ #include #include "tasks/Clean.hpp" +#include "tasks/ChargeBattery.hpp" #include @@ -175,14 +176,28 @@ void TaskManager::set_queue( std::dynamic_pointer_cast( a.request())) { - // const auto task = tasks::make_charge_battery() - } + const auto task = tasks::make_charge_battery( + request, + _context, + start, + a.deployment_time(), + a.state()); + + std::lock_guard guard(_mutex); + + _queue.push_back(task); + + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + this->_context->node()->task_summary()->publish(msg); } else if (const auto request = std::dynamic_pointer_cast( a.request())) { - // const auto task = tasks::make_delivery() + } else From 6ef20670b2d6bf2806a397537b952c1b0c966f45 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 3 Nov 2020 16:06:17 +0800 Subject: [PATCH 45/50] Modify start in set_queue() --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 1 + rmf_task/include/rmf_task/requests/Delivery.hpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 6b8f3d3ad..bd17beff6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -150,6 +150,7 @@ void TaskManager::set_queue( auto start = _context->state().location(); if (i != 0) start = assignments[i-1].state().location(); + start.time(a.deployment_time()); if (const auto request = std::dynamic_pointer_cast(a.request())) diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index c81e2b732..bc796f07a 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -65,6 +65,9 @@ class Delivery : public rmf_task::Request rmf_utils::impl_ptr _pimpl; }; +using DeliveryRequestPtr = std::shared_ptr; +using ConstDeliveryRequestPtr = std::shared_ptr; + } // namespace tasks } // namespace rmf_task From 63dc33515f3fcb73384b2313f175aaac4c6d925d Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 4 Nov 2020 18:21:07 +0800 Subject: [PATCH 46/50] Added second GoToPlace phase for Clean task --- .../src/rmf_fleet_adapter/tasks/Clean.cpp | 4 +++ rmf_task/include/rmf_task/requests/Clean.hpp | 5 +++ rmf_task/src/rmf_task/requests/Clean.cpp | 32 +++++++++++++++++++ 3 files changed, 41 insertions(+) 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 3523fc2b9..2620082eb 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Clean.cpp @@ -31,9 +31,13 @@ std::shared_ptr make_clean( const rmf_task::agv::State finish_state) { rmf_traffic::agv::Planner::Goal clean_goal{request->start_waypoint()}; + auto end_start = request->location_after_clean(clean_start); + rmf_traffic::agv::Planner::Goal end_goal{request->end_waypoint()}; Task::PendingPhases phases; phases.push_back( phases::GoToPlace::make(context, std::move(clean_start), clean_goal)); + phases.push_back( + phases::GoToPlace::make(context, std::move(end_start), end_goal)); return Task::make( std::to_string(request->id()), diff --git a/rmf_task/include/rmf_task/requests/Clean.hpp b/rmf_task/include/rmf_task/requests/Clean.hpp index a59dd920e..3a1c22743 100644 --- a/rmf_task/include/rmf_task/requests/Clean.hpp +++ b/rmf_task/include/rmf_task/requests/Clean.hpp @@ -64,6 +64,11 @@ class Clean : public rmf_task::Request const std::size_t start_waypoint() const; + const std::size_t end_waypoint() const; + + rmf_traffic::agv::Planner::Start location_after_clean( + rmf_traffic::agv::Planner::Start start) const; + class Implementation; private: Clean(); diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 7327ecfcb..6e01a5d4a 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -215,11 +215,43 @@ rmf_traffic::Time Clean::earliest_start_time() const return _pimpl->start_time; } +//============================================================================== const std::size_t Clean::start_waypoint() const { return _pimpl->start_waypoint; } +//============================================================================== +const std::size_t Clean::end_waypoint() const +{ + return _pimpl->end_waypoint; +} + +//============================================================================== +rmf_traffic::agv::Planner::Start Clean::location_after_clean( + rmf_traffic::agv::Planner::Start start) const +{ + if (start.waypoint() == _pimpl->start_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->start_waypoint}; + + const auto result = _pimpl->planner->plan(start, goal); + // We assume we can always compute a plan + const auto& trajectory = + result->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const double orientation = trajectory.back().position()[2]; + + rmf_traffic::agv::Planner::Start location_after_clean{ + finish_time + _pimpl->invariant_duration, + _pimpl->start_waypoint, + orientation}; + + return location_after_clean; + +} + //============================================================================== } // namespace requests } // namespace rmf_task From b013e5a1ced3e7c77ab3e6486ee31bb75b8db4f6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 4 Nov 2020 20:12:23 +0800 Subject: [PATCH 47/50] Added TODO to account for travel from end of cleaning trajectory to end_waypoint --- rmf_task/src/rmf_task/requests/Clean.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index 6e01a5d4a..f292d00e8 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -123,6 +123,7 @@ rmf_utils::optional Clean::estimate_finish( initial_state.battery_soc()}; rmf_traffic::Duration variant_duration(0); + rmf_traffic::Duration end_duration(0); const rmf_traffic::Time start_time = initial_state.finish_time(); double battery_soc = initial_state.battery_soc(); @@ -154,6 +155,13 @@ rmf_utils::optional Clean::estimate_finish( } } + if (_pimpl->start_waypoint != _pimpl->end_waypoint) + { + // TODO(YV) Account for battery drain and duration when robot moves from + // end of cleaning trajectory to its end_waypoint. We currently define the + // end_waypoint near the start_waypoint in the nav graph for minimum error + } + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; const rmf_traffic::Time wait_until = initial_state.finish_time() > ideal_start ? @@ -161,7 +169,7 @@ rmf_utils::optional Clean::estimate_finish( // Factor in invariants state.finish_time( - wait_until + variant_duration + _pimpl->invariant_duration); + wait_until + variant_duration + _pimpl->invariant_duration + end_duration); if (_pimpl->drain_battery) { From 45ceb289e27a8ccbda6525436da737ef9737a6cc Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 4 Nov 2020 20:23:36 +0800 Subject: [PATCH 48/50] Populate robot name in TaskSummary msg --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 5 +++++ rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index bd17beff6..71abf5a79 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -92,6 +92,7 @@ void TaskManager::queue_task(std::shared_ptr task, Start expected_finish) msg.task_id = _queue.back()->id(); msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); this->_context->node()->task_summary()->publish(msg); } } @@ -170,6 +171,7 @@ void TaskManager::set_queue( msg.task_id = _queue.back()->id(); msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); this->_context->node()->task_summary()->publish(msg); } @@ -192,6 +194,7 @@ void TaskManager::set_queue( msg.task_id = _queue.back()->id(); msg.task_profile.task_id = _queue.back()->id(); msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); this->_context->node()->task_summary()->publish(msg); } else if (const auto request = @@ -287,6 +290,7 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; + msg.robot_name = _context->name(); _context->node()->task_summary()->publish(msg); // _begin_next_task(); }, @@ -296,6 +300,7 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.state = msg.STATE_COMPLETED; + msg.robot_name = _context->name(); this->_context->node()->task_summary()->publish(msg); _active_task = nullptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 672b8af2c..b5c1bd224 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -61,7 +61,7 @@ class TaskManager : public std::enable_shared_from_this /// task planner void set_queue(const std::vector& assignments); - /// Get the non-charging requests for pending tasks + /// Get the non-charging requests among pending tasks const std::vector requests() const; // Callback for timer which begins next task if its deployment time has passed From bffe5aacaf34299c024b3d0dbb2cdb9e17310996 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 4 Nov 2020 21:07:17 +0800 Subject: [PATCH 49/50] Populate TaskSummary --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 71abf5a79..26f46916d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -274,6 +274,7 @@ void TaskManager::_begin_next_task() { msg.task_id = id; msg.task_profile.task_id = id; + msg.robot_name = _context->name(); _context->node()->task_summary()->publish(msg); }, [this, id = _active_task->id()](std::exception_ptr e) From 59ffd217ee5bf667815d014231b385af3eeb9b87 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 5 Nov 2020 15:13:18 +0800 Subject: [PATCH 50/50] Changed TaskType enum names and updated FleetUpdateHandle to work with new TaskProfile msg --- rmf_fleet_adapter/src/full_control/main.cpp | 8 ++++---- .../rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 16 ++++++++-------- rmf_task_msgs/msg/TaskType.msg | 4 ++-- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/rmf_fleet_adapter/src/full_control/main.cpp b/rmf_fleet_adapter/src/full_control/main.cpp index d996d7d80..5596792a0 100644 --- a/rmf_fleet_adapter/src/full_control/main.cpp +++ b/rmf_fleet_adapter/src/full_control/main.cpp @@ -577,27 +577,27 @@ std::shared_ptr make_fleet( std::unordered_set task_types; if (node->declare_parameter("perform_loop", false)) { - task_types.insert(rmf_task_msgs::msg::TaskType::LOOP_TASK); + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_LOOP); } // If the perform_deliveries parameter is true, then we just blindly accept // all delivery requests. if (node->declare_parameter("perform_deliveries", false)) { - task_types.insert(rmf_task_msgs::msg::TaskType::DELIVERY_TASK); + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_DELIVERY); connections->fleet->accept_delivery_requests( [](const rmf_task_msgs::msg::Delivery&){ return true; }); } if (node->declare_parameter("perform_cleaning", false)) { - task_types.insert(rmf_task_msgs::msg::TaskType::CLEANING_TASK); + task_types.insert(rmf_task_msgs::msg::TaskType::TYPE_CLEAN); } connections->fleet->accept_task_requests( [task_types](const rmf_task_msgs::msg::TaskProfile& msg) { - if (task_types.find(msg.type.value) != task_types.end()) + if (task_types.find(msg.task_type.type) != task_types.end()) return true; return false; 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 bba9a36ef..da85355f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -132,27 +132,27 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( // 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.type; + const auto& task_type = task_profile.task_type; const rmf_traffic::Time start_time = rmf_traffic_ros2::convert(task_profile.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(); // Process Cleaning task - if (task_type.value == rmf_task_msgs::msg::TaskType::CLEANING_TASK) + if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_CLEAN) { - if (task_profile.params.empty()) + if (task_profile.clean.start_waypoint.empty()) { RCLCPP_INFO( node->get_logger(), - "Required param [zone] missing in TaskProfile. Rejecting BidNotice " - " with task_id:[%s]" , id.c_str()); + "Required param [clean.start_waypoint] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); return; } // Check for valid start waypoint - const std::string start_wp_name = task_profile.params[0].value; + const std::string start_wp_name = task_profile.clean.start_waypoint; const auto start_wp = graph.find_waypoint(start_wp_name); if (!start_wp) { @@ -234,11 +234,11 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( "Generated Clean request"); } - else if (task_type.value == rmf_task_msgs::msg::TaskType::DELIVERY_TASK) + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { // TODO(YV) } - else if (task_type.value == rmf_task_msgs::msg::TaskType::LOOP_TASK) + else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { // TODO(YV) } diff --git a/rmf_task_msgs/msg/TaskType.msg b/rmf_task_msgs/msg/TaskType.msg index ac3733bbe..a180229b5 100644 --- a/rmf_task_msgs/msg/TaskType.msg +++ b/rmf_task_msgs/msg/TaskType.msg @@ -2,7 +2,7 @@ uint32 type uint32 TYPE_STATION=0 uint32 TYPE_LOOP=1 uint32 TYPE_DELIVERY=2 -uint32 TYPE_CHARGING=3 -uint32 TYPE_CLEANING=4 +uint32 TYPE_CHARGE_BATTERY=3 +uint32 TYPE_CLEAN=4 uint32 TYPE_PATROL=5