From 9f533d722030e949d8c75bd1f64e6b0a6306b673 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 10 Nov 2020 11:48:43 +0800 Subject: [PATCH 1/9] Added creation of delivery request --- .../agv/FleetUpdateHandle.cpp | 101 +++++++++++++++++- 1 file changed, 100 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 da85355f3..dd1fc7a59 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,12 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include +#include + +#include +#include + #include #include @@ -236,7 +242,100 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_DELIVERY) { - // TODO(YV) + const auto& delivery = task_profile.delivery; + if (delivery.pickup_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.pickup_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.pickup_dispenser.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.pickup_dispenser] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_place_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_place_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (delivery.dropoff_ingestor.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [delivery.dropoff_ingestor] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + const auto pickup_wp = graph.find_waypoint(delivery.pickup_place_name); + if (!pickup_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), delivery.pickup_place_name.c_str(), id.c_str()); + + return; + } + + const auto dropoff_wp = graph.find_waypoint(delivery.dropoff_place_name); + if (!dropoff_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), delivery.dropoff_place_name.c_str(), id.c_str()); + + return; + } + + // TODO(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::Delivery::make( + request_id, + pickup_wp->index(), + dropoff_wp->index(), + motion_sink, + ambient_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Delivery request"); + } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { From 059ac4a6cb6134d0f5b9f2ef82c2a6389f2cd1ac Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 10 Nov 2020 13:55:34 +0800 Subject: [PATCH 2/9] Modified make and extended API for Delivery request --- .../agv/FleetUpdateHandle.cpp | 2 ++ .../include/rmf_task/requests/Delivery.hpp | 10 ++++++ rmf_task/src/rmf_task/requests/Delivery.cpp | 30 ++++++++++++++++ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 36 +++++++++++++++++++ 4 files changed, 78 insertions(+) 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 dd1fc7a59..753313041 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -325,7 +325,9 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( new_request = rmf_task::requests::Delivery::make( request_id, pickup_wp->index(), + delivery.pickup_dispenser, dropoff_wp->index(), + delivery.dropoff_ingestor, motion_sink, ambient_sink, planner, diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index bc796f07a..108295d8c 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -42,7 +42,9 @@ class Delivery : public rmf_task::Request static ConstRequestPtr make( std::size_t id, std::size_t pickup_waypoint, + std::string pickup_dispenser, std::size_t dropoff_waypoint, + std::string dropoff_ingestor, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -59,6 +61,14 @@ class Delivery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + const std::size_t pickup_waypoint() const; + + const std::string& pickup_dispenser() const; + + const std::size_t dropoff_waypoint() const; + + const std::string& dropoff_ingestor() const; + class Implementation; private: Delivery(); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 230a5e0c1..6ef3f073a 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -32,7 +32,9 @@ class Delivery::Implementation std::size_t _id; std::size_t _pickup_waypoint; + std::string _pickup_dispenser; std::size_t _dropoff_waypoint; + std::string _dropoff_ingestor; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; @@ -47,7 +49,9 @@ class Delivery::Implementation rmf_task::ConstRequestPtr Delivery::make( std::size_t id, std::size_t pickup_waypoint, + std::string pickup_dispenser, std::size_t dropoff_waypoint, + std::string dropoff_ingestor, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -57,7 +61,9 @@ rmf_task::ConstRequestPtr Delivery::make( std::shared_ptr delivery(new Delivery()); delivery->_pimpl->_id = id; delivery->_pimpl->_pickup_waypoint = pickup_waypoint; + delivery->_pimpl->_pickup_dispenser = std::move(pickup_dispenser); delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; + delivery->_pimpl->_dropoff_ingestor = std::move(dropoff_ingestor); delivery->_pimpl->_motion_sink = std::move(motion_sink); delivery->_pimpl->_device_sink = std::move(device_sink); delivery->_pimpl->_planner = std::move(planner); @@ -218,6 +224,30 @@ rmf_traffic::Time Delivery::earliest_start_time() const return _pimpl->_start_time; } +//============================================================================== +const std::size_t Delivery::pickup_waypoint() const +{ + return _pimpl->_pickup_waypoint; +} + +//============================================================================== +const std::string& Delivery::pickup_dispenser() const +{ + return _pimpl->_pickup_dispenser; +} + +//============================================================================== +const std::string& Delivery::dropoff_ingestor() const +{ + return _pimpl->_dropoff_ingestor; +} + +//============================================================================== +const std::size_t Delivery::dropoff_waypoint() const +{ + return _pimpl->_dropoff_waypoint; +} + //============================================================================== } // namespace requests } // namespace rmf_task diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 67b380428..b6b530494 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -187,7 +187,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", motion_sink, device_sink, planner, @@ -197,7 +199,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", motion_sink, device_sink, planner, @@ -207,7 +211,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", motion_sink, device_sink, planner, @@ -262,7 +268,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", motion_sink, device_sink, planner, @@ -272,7 +280,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", motion_sink, device_sink, planner, @@ -282,7 +292,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", motion_sink, device_sink, planner, @@ -292,7 +304,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", motion_sink, device_sink, planner, @@ -302,7 +316,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 5, 10, + "dispenser", 0, + "ingestor", motion_sink, device_sink, planner, @@ -312,7 +328,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 6, 4, + "dispenser", 8, + "ingestor", motion_sink, device_sink, planner, @@ -322,7 +340,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 7, 8, + "dispenser", 14, + "ingestor", motion_sink, device_sink, planner, @@ -332,7 +352,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 8, 5, + "dispenser", 11, + "ingestor", motion_sink, device_sink, planner, @@ -342,7 +364,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 9, 9, + "dispenser", 0, + "ingestor", motion_sink, device_sink, planner, @@ -352,7 +376,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 10, 1, + "dispenser", 3, + "ingestor", motion_sink, device_sink, planner, @@ -362,7 +388,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 11, 0, + "dispenser", 12, + "ingestor", motion_sink, device_sink, planner, @@ -418,7 +446,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", motion_sink, device_sink, planner, @@ -428,7 +458,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", motion_sink, device_sink, planner, @@ -438,7 +470,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 9, + "dispenser", 4, + "ingestor", motion_sink, device_sink, planner, @@ -448,7 +482,9 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", motion_sink, device_sink, planner, From 429087a865acd6cfe88074398f248936619266d9 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 10 Nov 2020 17:22:13 +0800 Subject: [PATCH 3/9] Added items() and dropoff_start() to Delivery request --- .../src/rmf_fleet_adapter/TaskManager.cpp | 20 +++- .../agv/FleetUpdateHandle.cpp | 1 + .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 112 +++++++++++++----- .../src/rmf_fleet_adapter/tasks/Delivery.hpp | 11 +- rmf_task/CMakeLists.txt | 7 ++ .../include/rmf_task/requests/Delivery.hpp | 10 ++ rmf_task/package.xml | 1 + rmf_task/src/rmf_task/requests/Clean.cpp | 8 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 33 ++++++ rmf_task/test/unit/agv/test_TaskPlanner.cpp | 18 +++ 10 files changed, 181 insertions(+), 40 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 26f46916d..e350a88a0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -25,6 +25,7 @@ #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" +#include "tasks/Delivery.hpp" #include @@ -195,12 +196,29 @@ void TaskManager::set_queue( 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); } + this->_context->node()->task_summary()->publish(msg); + + } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + const auto task = tasks::make_delivery( + request, + _context, + start, + a.deployment_time(), + a.state()); + + _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; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); } 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 753313041..3471cf2fd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -328,6 +328,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( delivery.pickup_dispenser, dropoff_wp->index(), delivery.dropoff_ingestor, + delivery.items, motion_sink, ambient_sink, planner, 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 edf93b4d3..6eb063562 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -1,30 +1,84 @@ -// /* -// * 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 "Delivery.hpp" - -// namespace rmf_fleet_adapter { -// namespace tasks { - -// //============================================================================== +/* + * 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 "Delivery.hpp" + +#include + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_delivery( + const rmf_task::requests::ConstDeliveryRequestPtr request, + const agv::RobotContextPtr& context, + const rmf_traffic::agv::Plan::Start pickup_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state) +{ + Task::PendingPhases phases; + phases.push_back( + phases::GoToPlace::make( + context, std::move(pickup_start), request->pickup_waypoint())); + + phases.push_back( + std::make_unique( + context, + std::to_string(request->id()), + request->pickup_dispenser(), + context->itinerary().description().owner(), + request->items())); + + auto dropoff_start = request->dropoff_start(pickup_start); + phases.push_back( + phases::GoToPlace::make( + context, std::move(dropoff_start), request->dropoff_waypoint())); + + + std::vector ingestor_items; + ingestor_items.reserve(request->items().size()); + for(const 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, + std::to_string(request->id()), + request->dropoff_ingestor(), + context->itinerary().description().owner(), + ingestor_items)); + + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} // std::shared_ptr make_delivery( // const rmf_task_msgs::msg::Delivery& request, // const agv::RobotContextPtr& context, @@ -75,5 +129,5 @@ // 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/Delivery.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp index 49dfe995a..22b03ecd2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.hpp @@ -21,19 +21,18 @@ #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_delivery( - const rmf_task_msgs::msg::Delivery& request, + const rmf_task::requests::ConstDeliveryRequestPtr request, const agv::RobotContextPtr& context, - rmf_traffic::agv::Plan::Start pickup_start, - rmf_traffic::agv::Plan::Start dropoff_start); + const rmf_traffic::agv::Plan::Start pickup_start, + const rmf_traffic::Time deployment_time, + const rmf_task::agv::State finish_state); } // namespace tasks } // namespace rmf_fleet_adapter diff --git a/rmf_task/CMakeLists.txt b/rmf_task/CMakeLists.txt index 4af624e8a..f7e1f2138 100644 --- a/rmf_task/CMakeLists.txt +++ b/rmf_task/CMakeLists.txt @@ -27,6 +27,7 @@ include(GNUInstallDirs) find_package(rmf_utils REQUIRED) find_package(rmf_traffic REQUIRED) find_package(rmf_battery REQUIRED) +find_package(rmf_dispenser_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(ament_cmake_catch2 QUIET) @@ -46,6 +47,8 @@ add_library(rmf_task SHARED target_link_libraries(rmf_task PUBLIC rmf_battery::rmf_battery + PRIVATE + ${rmf_dispenser_msgs_LIBRARIES} ) target_include_directories(rmf_task @@ -53,6 +56,8 @@ target_include_directories(rmf_task $ $ ${EIGEN3_INCLUDE_DIRS} + PRIVATE + ${rmf_dispenser_msgs_INCLUDE_DIRS} ) if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) @@ -65,11 +70,13 @@ if(BUILD_TESTING AND ament_cmake_catch2_FOUND AND rmf_cmake_uncrustify_FOUND) rmf_task rmf_utils::rmf_utils rmf_traffic::rmf_traffic + ${rmf_dispenser_msgs_LIBRARIES} ) target_include_directories(test_rmf_task PRIVATE $ + ${rmf_dispenser_msgs_INCLUDE_DIRS} ) find_file(uncrustify_config_file NAMES "share/format/rmf_code_style.cfg") diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 108295d8c..29818203b 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -32,6 +32,8 @@ #include #include +#include + namespace rmf_task { namespace requests { @@ -39,12 +41,16 @@ class Delivery : public rmf_task::Request { public: + using DispenserRequestItem = rmf_dispenser_msgs::msg::DispenserRequestItem; + using Start = rmf_traffic::agv::Planner::Start; + static ConstRequestPtr make( std::size_t id, std::size_t pickup_waypoint, std::string pickup_dispenser, std::size_t dropoff_waypoint, std::string dropoff_ingestor, + std::vector items, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -69,6 +75,10 @@ class Delivery : public rmf_task::Request const std::string& dropoff_ingestor() const; + const std::vector& items() const; + + Start dropoff_start(const Start& start) const; + class Implementation; private: Delivery(); diff --git a/rmf_task/package.xml b/rmf_task/package.xml index 658107312..d9deef5a9 100644 --- a/rmf_task/package.xml +++ b/rmf_task/package.xml @@ -10,6 +10,7 @@ rmf_battery rmf_utils + rmf_dispenser_msgs eigen diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index f292d00e8..66ca57f14 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -245,10 +245,10 @@ rmf_traffic::agv::Planner::Start Clean::location_after_clean( 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(); + // 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{ diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 6ef3f073a..022b40d8d 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -35,6 +35,7 @@ class Delivery::Implementation std::string _pickup_dispenser; std::size_t _dropoff_waypoint; std::string _dropoff_ingestor; + std::vector _items; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; @@ -52,6 +53,7 @@ rmf_task::ConstRequestPtr Delivery::make( std::string pickup_dispenser, std::size_t dropoff_waypoint, std::string dropoff_ingestor, + std::vector items, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -64,6 +66,7 @@ rmf_task::ConstRequestPtr Delivery::make( delivery->_pimpl->_pickup_dispenser = std::move(pickup_dispenser); delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; delivery->_pimpl->_dropoff_ingestor = std::move(dropoff_ingestor); + delivery->_pimpl->_items = std::move(items); delivery->_pimpl->_motion_sink = std::move(motion_sink); delivery->_pimpl->_device_sink = std::move(device_sink); delivery->_pimpl->_planner = std::move(planner); @@ -248,6 +251,36 @@ const std::size_t Delivery::dropoff_waypoint() const return _pimpl->_dropoff_waypoint; } +//============================================================================== +const std::vector& Delivery::items() const +{ + return _pimpl->_items; +} + +//============================================================================== +Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const +{ + if (start.waypoint() == _pimpl->_pickup_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_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 dropoff_start{ + finish_time, + _pimpl->_pickup_waypoint, + orientation}; + + return dropoff_start; + +} + //============================================================================== } // namespace requests } // namespace rmf_task diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index b6b530494..f27c7d069 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -190,6 +190,7 @@ SCENARIO("Grid World") "dispenser", 3, "ingestor", + {}, motion_sink, device_sink, planner, @@ -202,6 +203,7 @@ SCENARIO("Grid World") "dispenser", 2, "ingestor", + {}, motion_sink, device_sink, planner, @@ -214,6 +216,7 @@ SCENARIO("Grid World") "dispenser", 9, "ingestor", + {}, motion_sink, device_sink, planner, @@ -271,6 +274,7 @@ SCENARIO("Grid World") "dispenser", 3, "ingestor", + {}, motion_sink, device_sink, planner, @@ -283,6 +287,7 @@ SCENARIO("Grid World") "dispenser", 2, "ingestor", + {}, motion_sink, device_sink, planner, @@ -295,6 +300,7 @@ SCENARIO("Grid World") "dispenser", 9, "ingestor", + {}, motion_sink, device_sink, planner, @@ -307,6 +313,7 @@ SCENARIO("Grid World") "dispenser", 11, "ingestor", + {}, motion_sink, device_sink, planner, @@ -319,6 +326,7 @@ SCENARIO("Grid World") "dispenser", 0, "ingestor", + {}, motion_sink, device_sink, planner, @@ -331,6 +339,7 @@ SCENARIO("Grid World") "dispenser", 8, "ingestor", + {}, motion_sink, device_sink, planner, @@ -343,6 +352,7 @@ SCENARIO("Grid World") "dispenser", 14, "ingestor", + {}, motion_sink, device_sink, planner, @@ -355,6 +365,7 @@ SCENARIO("Grid World") "dispenser", 11, "ingestor", + {}, motion_sink, device_sink, planner, @@ -367,6 +378,7 @@ SCENARIO("Grid World") "dispenser", 0, "ingestor", + {}, motion_sink, device_sink, planner, @@ -379,6 +391,7 @@ SCENARIO("Grid World") "dispenser", 3, "ingestor", + {}, motion_sink, device_sink, planner, @@ -391,6 +404,7 @@ SCENARIO("Grid World") "dispenser", 12, "ingestor", + {}, motion_sink, device_sink, planner, @@ -449,6 +463,7 @@ SCENARIO("Grid World") "dispenser", 3, "ingestor", + {}, motion_sink, device_sink, planner, @@ -461,6 +476,7 @@ SCENARIO("Grid World") "dispenser", 2, "ingestor", + {}, motion_sink, device_sink, planner, @@ -473,6 +489,7 @@ SCENARIO("Grid World") "dispenser", 4, "ingestor", + {}, motion_sink, device_sink, planner, @@ -485,6 +502,7 @@ SCENARIO("Grid World") "dispenser", 11, "ingestor", + {}, motion_sink, device_sink, planner, From c564e7dd55a54a54471f018480d77bac0b7d5acf Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 10 Nov 2020 18:37:49 +0800 Subject: [PATCH 4/9] Deleted old Delivery task implementation --- .../src/rmf_fleet_adapter/tasks/Delivery.cpp | 49 ------------------- 1 file changed, 49 deletions(-) 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 6eb063562..a25cce7f3 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp @@ -79,55 +79,6 @@ std::shared_ptr make_delivery( finish_state, request); } -// 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(); - -// 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)); - -// 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)); - -// 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)); - -// return Task::make(request.task_id, std::move(phases), context->worker()); -// } } // namespace task } // namespace rmf_fleet_adapter From bce91564e8c73b974a1ca6300370780c1a68957a Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Tue, 10 Nov 2020 18:55:34 +0800 Subject: [PATCH 5/9] Loop request skeleton --- rmf_task/include/rmf_task/requests/Loop.hpp | 87 +++++++++++++++++++++ rmf_task/src/rmf_task/requests/Loop.cpp | 48 ++++++++++++ 2 files changed, 135 insertions(+) create mode 100644 rmf_task/include/rmf_task/requests/Loop.hpp create mode 100644 rmf_task/src/rmf_task/requests/Loop.cpp diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp new file mode 100644 index 000000000..81da1e217 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -0,0 +1,87 @@ +/* + * 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 INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP +#define INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP + +#include + +#include +#include + +#include +#include + +#include + +#include +#include +#include + +namespace rmf_task { +namespace requests { + +class Loop : public rmf_task::Request +{ +public: + + using Start = rmf_traffic::agv::Planner::Start; + + static ConstRequestPtr make( + std::size_t id, + std::size_t start_waypoint, + std::size_t finish_waypoint, + std::size_t num_loops, + std::shared_ptr motion_sink, + std::shared_ptr device_sink, + std::shared_ptr planner, + rmf_traffic::Time start_time, + bool drain_battery = true); + + std::size_t id() const final; + + rmf_utils::optional estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) const final; + + rmf_traffic::Duration invariant_duration() const final; + + rmf_traffic::Time earliest_start_time() const final; + + const std::size_t start_waypoint() const; + + const std::size_t finish_waypoint() const; + + const std::size_t num_loops() const; + + Start loop_start(const Start& start) const; + + Start loop_end(const Start& start) const; + + class Implementation; +private: + Delivery(); + rmf_utils::impl_ptr _pimpl; +}; + +using LoopRequestPtr = std::shared_ptr; +using ConstLoopRequestPtr = std::shared_ptr; + +} // namespace tasks +} // namespace rmf_task + +#endif // INCLUDE__RMF_TASK__REQUESTS__LOOP_HPP diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp new file mode 100644 index 000000000..66a4eeb68 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Loop.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 + +namespace rmf_task { +namespace requests { + +//============================================================================== +class Loop::Implementation +{ +public: + + Implementation() + {} + + std::size_t id; + std::size_t start_waypoint; + std::size_t finish_waypoint; + std::size_t num_loops; + std::shared_ptr motion_sink; + std::shared_ptr device_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + double invariant_battery_drain; +}; + + + +} // namespace requests +} // namespace rmf_task \ No newline at end of file From baabd1709ec019f2341177e71158cd05bd501d0a Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 11 Nov 2020 17:21:34 +0800 Subject: [PATCH 6/9] Implemented Loop request --- .../include/rmf_task/requests/Delivery.hpp | 4 +- rmf_task/include/rmf_task/requests/Loop.hpp | 11 +- rmf_task/src/rmf_task/requests/Delivery.cpp | 53 ++-- rmf_task/src/rmf_task/requests/Loop.cpp | 254 +++++++++++++++++- 4 files changed, 290 insertions(+), 32 deletions(-) diff --git a/rmf_task/include/rmf_task/requests/Delivery.hpp b/rmf_task/include/rmf_task/requests/Delivery.hpp index 29818203b..0b89f5a0f 100644 --- a/rmf_task/include/rmf_task/requests/Delivery.hpp +++ b/rmf_task/include/rmf_task/requests/Delivery.hpp @@ -67,11 +67,11 @@ class Delivery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; - const std::size_t pickup_waypoint() const; + std::size_t pickup_waypoint() const; const std::string& pickup_dispenser() const; - const std::size_t dropoff_waypoint() const; + std::size_t dropoff_waypoint() const; const std::string& dropoff_ingestor() const; diff --git a/rmf_task/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp index 81da1e217..1c1860ba5 100644 --- a/rmf_task/include/rmf_task/requests/Loop.hpp +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -47,7 +48,7 @@ class Loop : public rmf_task::Request std::size_t finish_waypoint, std::size_t num_loops, std::shared_ptr motion_sink, - std::shared_ptr device_sink, + std::shared_ptr ambient_sink, std::shared_ptr planner, rmf_traffic::Time start_time, bool drain_battery = true); @@ -62,11 +63,11 @@ class Loop : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; - const std::size_t start_waypoint() const; + std::size_t start_waypoint() const; - const std::size_t finish_waypoint() const; + std::size_t finish_waypoint() const; - const std::size_t num_loops() const; + std::size_t num_loops() const; Start loop_start(const Start& start) const; @@ -74,7 +75,7 @@ class Loop : public rmf_task::Request class Implementation; private: - Delivery(); + Loop(); rmf_utils::impl_ptr _pimpl; }; diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 022b40d8d..fb15278f5 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -74,30 +74,35 @@ rmf_task::ConstRequestPtr Delivery::make( delivery->_pimpl->_start_time = start_time; // Calculate duration of invariant component of task - const auto plan_start_time = std::chrono::steady_clock::now(); - rmf_traffic::agv::Planner::Start start{ - plan_start_time, - delivery->_pimpl->_pickup_waypoint, - 0.0}; - - rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; - const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); - - const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); - const auto& finish_time = *trajectory.finish_time(); - - delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + delivery->_pimpl->_invariant_duration = rmf_traffic::Duration{0}; delivery->_pimpl->_invariant_battery_drain = 0.0; - if (delivery->_pimpl->_drain_battery) + if (delivery->_pimpl->_pickup_waypoint != delivery->_pimpl->_dropoff_waypoint) { - // Compute battery drain - const double dSOC_motion = - delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); - const double dSOC_device = - delivery->_pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); - delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + const auto plan_start_time = std::chrono::steady_clock::now(); + rmf_traffic::agv::Planner::Start start{ + plan_start_time, + delivery->_pimpl->_pickup_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; + const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); + + const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + + delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + + if (delivery->_pimpl->_drain_battery) + { + // Compute battery drain + const double dSOC_motion = + delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + delivery->_pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); + delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + } } return delivery; @@ -152,7 +157,7 @@ rmf_utils::optional Delivery::estimate_finish( const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if(_pimpl->_drain_battery) + if (_pimpl->_drain_battery) { // Compute battery drain dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); @@ -228,7 +233,7 @@ rmf_traffic::Time Delivery::earliest_start_time() const } //============================================================================== -const std::size_t Delivery::pickup_waypoint() const +std::size_t Delivery::pickup_waypoint() const { return _pimpl->_pickup_waypoint; } @@ -246,7 +251,7 @@ const std::string& Delivery::dropoff_ingestor() const } //============================================================================== -const std::size_t Delivery::dropoff_waypoint() const +std::size_t Delivery::dropoff_waypoint() const { return _pimpl->_dropoff_waypoint; } diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 66a4eeb68..6b0ee4be5 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -33,7 +33,7 @@ class Loop::Implementation std::size_t finish_waypoint; std::size_t num_loops; std::shared_ptr motion_sink; - std::shared_ptr device_sink; + std::shared_ptr ambient_sink; std::shared_ptr planner; bool drain_battery; rmf_traffic::Time start_time; @@ -42,7 +42,259 @@ class Loop::Implementation double invariant_battery_drain; }; +//============================================================================== +ConstRequestPtr Loop::make( + std::size_t id, + std::size_t start_waypoint, + std::size_t finish_waypoint, + std::size_t num_loops, + std::shared_ptr motion_sink, + std::shared_ptr ambient_sink, + std::shared_ptr planner, + rmf_traffic::Time start_time, + bool drain_battery) +{ + std::shared_ptr loop(new Loop()); + loop->_pimpl->id = id; + loop->_pimpl->start_waypoint = start_waypoint; + loop->_pimpl->finish_waypoint = finish_waypoint; + loop->_pimpl->num_loops = num_loops; + loop->_pimpl->motion_sink = std::move(motion_sink); + loop->_pimpl->ambient_sink = std::move(ambient_sink); + loop->_pimpl->planner = std::move(planner); + loop->_pimpl->drain_battery = drain_battery; + loop->_pimpl->start_time = start_time; + + // Calculate the invariant duration and battery drain for this task + loop->_pimpl->invariant_duration = rmf_traffic::Duration{0}; + loop->_pimpl->invariant_battery_drain = 0.0; + + if (loop->_pimpl->start_waypoint != loop->_pimpl->finish_waypoint) + { + rmf_traffic::agv::Planner::Start loop_start{ + start_time, + loop->_pimpl->start_waypoint, + 0.0}; + rmf_traffic::agv::Planner::Goal loop_end_goal{loop->_pimpl->start_waypoint}; + + const auto forward_loop_plan = loop->_pimpl->planner->plan( + loop_start, loop_end_goal); + + const auto trajectory = + forward_loop_plan->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const auto forward_duration = finish_time - start_time; + + loop->_pimpl->invariant_duration = (2 * num_loops - 1) * forward_duration; + + if (loop->_pimpl->drain_battery) + { + // Compute battery drain + const double dSOC_motion = + loop->_pimpl->motion_sink->compute_change_in_charge(trajectory); + const double dSOC_device = + loop->_pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(forward_duration)); + const double forward_battery_drain = dSOC_motion + dSOC_device; + loop->_pimpl->invariant_battery_drain = + (2 * num_loops - 1) * forward_battery_drain; + } + } + + return loop; + +} + +//============================================================================== +Loop::Loop() +: _pimpl(rmf_utils::make_impl(Implementation())) +{ + // Do nothing +} + +//============================================================================== +std::size_t Loop::id() const +{ + return _pimpl->id; +} + +//============================================================================== +rmf_utils::optional Loop::estimate_finish( + const agv::State& initial_state, + const agv::StateConfig& state_config) const +{ + + rmf_traffic::Duration variant_duration(0); + + const rmf_traffic::Time start_time = initial_state.finish_time(); + double battery_soc = initial_state.battery_soc(); + double dSOC_motion = 0.0; + double dSOC_device = 0.0; + + // Check if a plan has to be generated from finish location to start_waypoint + if (initial_state.waypoint() != _pimpl->start_waypoint) + { + // Compute plan to start_waypoint along with battery drain + rmf_traffic::agv::Planner::Start init_start{ + start_time, + initial_state.waypoint(), + 0.0}; + + rmf_traffic::agv::Planner::Goal loop_start_goal{_pimpl->start_waypoint}; + const auto plan_to_start = _pimpl->planner->plan(init_start, loop_start_goal); + // We assume we can always compute a plan + const auto& trajectory = plan_to_start->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + variant_duration = finish_time - start_time; + + if (_pimpl->drain_battery) + { + // Compute battery drain + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = + _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(variant_duration)); + battery_soc = battery_soc - dSOC_motion - dSOC_device; + } + + if (battery_soc <= state_config.threshold_soc()) + return rmf_utils::nullopt; + } + + // Compute wait_until + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; + const rmf_traffic::Time wait_until = + initial_state.finish_time() > ideal_start ? + initial_state.finish_time() : ideal_start; + + // Compute finish time + const rmf_traffic::Time state_finish_time = + wait_until + variant_duration + _pimpl->invariant_duration; + + // Subtract invariant battery drain and check if robot can return to its charger + if (_pimpl->drain_battery) + { + battery_soc -= _pimpl->invariant_battery_drain; + if (battery_soc <= state_config.threshold_soc()) + return rmf_utils::nullopt; + + if ( _pimpl->finish_waypoint != initial_state.charging_waypoint()) + { + rmf_traffic::agv::Planner::Start retreat_start{ + state_finish_time, + _pimpl->finish_waypoint, + 0.0}; + + rmf_traffic::agv::Planner::Goal charger_goal{ + initial_state.charging_waypoint()}; + + const auto result_to_charger = _pimpl->planner->plan( + retreat_start, charger_goal); + // We assume we can always compute a plan + const auto& trajectory = + result_to_charger->get_itinerary().back().trajectory(); + const auto& finish_time = *trajectory.finish_time(); + const rmf_traffic::Duration retreat_duration = + finish_time - state_finish_time; + + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(retreat_duration)); + const double retreat_battery_drain = dSOC_motion + dSOC_device; + + if (battery_soc - retreat_battery_drain <= state_config.threshold_soc()) + return rmf_utils::nullopt; + } + } + + // Return Estimate + rmf_traffic::agv::Planner::Start location{ + state_finish_time, + _pimpl->finish_waypoint, + initial_state.location().orientation()}; + agv::State state{ + std::move(location), + initial_state.charging_waypoint(), + battery_soc}; + + return Estimate(state, wait_until); +} + +//============================================================================== +rmf_traffic::Duration Loop::invariant_duration() const +{ + return _pimpl->invariant_duration; +} + +//============================================================================== +rmf_traffic::Time Loop::earliest_start_time() const +{ + return _pimpl->start_time; +} + +//============================================================================== +std::size_t Loop::start_waypoint() const +{ + return _pimpl->start_waypoint; +} + +//============================================================================== +std::size_t Loop::finish_waypoint() const +{ + return _pimpl->finish_waypoint; +} + +//============================================================================== +std::size_t Loop::num_loops() const +{ + return _pimpl->num_loops; +} + +//============================================================================== +Loop::Start Loop::loop_start(const Loop::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 loop_start{ + finish_time, + _pimpl->start_waypoint, + orientation}; + + return loop_start; +} + +//============================================================================== +Loop::Start Loop::loop_end(const Loop::Start& start) const +{ + if (start.waypoint() == _pimpl->finish_waypoint) + return start; + + rmf_traffic::agv::Planner::Goal goal{_pimpl->finish_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 loop_end{ + finish_time, + _pimpl->finish_waypoint, + orientation}; + return loop_end; +} } // namespace requests } // namespace rmf_task \ No newline at end of file From e5dc26e0be63512dde3172080fb59d535f985f6c Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 11 Nov 2020 18:13:13 +0800 Subject: [PATCH 7/9] Parse loop request in FleetUpdateHandle --- .../agv/FleetUpdateHandle.cpp | 77 ++++++++++++++++++- rmf_task/src/rmf_task/requests/Loop.cpp | 2 +- 2 files changed, 77 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 3471cf2fd..97620254d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -24,9 +24,11 @@ #include #include +#include #include #include +#include #include #include @@ -342,7 +344,80 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } else if (task_type.type == rmf_task_msgs::msg::TaskType::TYPE_LOOP) { - // TODO(YV) + const auto& loop = task_profile.loop; + if (loop.start_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.start_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (loop.finish_name.empty()) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.finish_name] missing in TaskProfile." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + if (loop.num_loops < 1) + { + RCLCPP_INFO( + node->get_logger(), + "Required param [loop.num_loops] in TaskProfile is invalid." + "Rejecting BidNotice with task_id:[%s]" , id.c_str()); + + return; + } + + const auto start_wp = graph.find_waypoint(loop.start_name); + if (!start_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), loop.start_name.c_str(), id.c_str()); + + return; + } + + const auto finish_wp = graph.find_waypoint(loop.finish_name); + if (!finish_wp) + { + RCLCPP_INFO( + node->get_logger(), + "Fleet [%s] does not have a named waypoint [%s] configured in its " + "nav graph. Rejecting BidNotice with task_id:[%s]", + name.c_str(), loop.finish_name.c_str(), id.c_str()); + + return; + } + + // 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::Loop::make( + request_id, + start_wp->index(), + finish_wp->index(), + loop.num_loops, + motion_sink, + ambient_sink, + planner, + start_time, + drain_battery); + + RCLCPP_INFO( + node->get_logger(), + "Generated Loop request"); } else { diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 6b0ee4be5..f30650131 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -297,4 +297,4 @@ Loop::Start Loop::loop_end(const Loop::Start& start) const } } // namespace requests -} // namespace rmf_task \ No newline at end of file +} // namespace rmf_task From d6968cbdb70eebbe376f30a1ee68f91275d254e5 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 12 Nov 2020 13:36:51 +0800 Subject: [PATCH 8/9] Integrated Loop requests --- .../src/rmf_fleet_adapter/TaskManager.cpp | 22 +++++ .../agv/FleetUpdateHandle.cpp | 1 - .../src/rmf_fleet_adapter/tasks/Loop.cpp | 91 ++++++++++++++----- .../src/rmf_fleet_adapter/tasks/Loop.hpp | 11 ++- rmf_task/src/rmf_task/requests/Loop.cpp | 5 +- 5 files changed, 96 insertions(+), 34 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index e350a88a0..67539f596 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -20,12 +20,14 @@ #include #include #include +#include #include #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" #include "tasks/Delivery.hpp" +#include "tasks/Loop.hpp" #include @@ -222,6 +224,26 @@ void TaskManager::set_queue( } + else if (const auto request = + std::dynamic_pointer_cast(a.request())) + { + const auto task = tasks::make_loop( + request, + _context, + start, + a.deployment_time(), + a.state()); + + _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; + msg.robot_name = _context->name(); + this->_context->node()->task_summary()->publish(msg); + } + else { continue; 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 97620254d..adcb882d9 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -531,7 +531,6 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( } - void FleetUpdateHandle::Implementation::dispatch_request_cb( const DispatchRequest::SharedPtr msg) { 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 b5f9397cb..9c1d43db7 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -1,26 +1,67 @@ -// /* -// * Copyright (C) 2020 Open Source Robotics Foundation -// * -// * Licensed under the Apache License, Version 2.0 (the "License"); -// * you may not use this file except in compliance with the License. -// * You may obtain a copy of the License at -// * -// * http://www.apache.org/licenses/LICENSE-2.0 -// * -// * Unless required by applicable law or agreed to in writing, software -// * distributed under the License is distributed on an "AS IS" BASIS, -// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// * See the License for the specific language governing permissions and -// * limitations under the License. -// * -// */ - -// #include "Loop.hpp" - -// #include "../phases/GoToPlace.hpp" - -// namespace rmf_fleet_adapter { -// namespace tasks { +/* + * 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 "../phases/GoToPlace.hpp" + +namespace rmf_fleet_adapter { +namespace tasks { + +//============================================================================== +std::shared_ptr make_loop( + const rmf_task::requests::ConstLoopRequestPtr 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) +{ + + Task::PendingPhases phases; + const auto loop_start = request->loop_start(start); + const auto loop_end = request->loop_end(loop_start); + + phases.push_back( + phases::GoToPlace::make( + context, std::move(start), request->start_waypoint())); + + phases.push_back( + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); + + for (std::size_t i = 1; i < request->num_loops(); ++i) + { + phases.push_back( + phases::GoToPlace::make( + context, loop_end, request->start_waypoint())); + + phases.push_back( + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); + } + + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} // //============================================================================== // std::shared_ptr make_loop( @@ -64,5 +105,5 @@ // return Task::make(request.task_id, std::move(phases), context->worker()); // } -// } // namespace tasks -// } // naemspace rmf_fleet_adapter +} // namespace tasks +} // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp index 6ba1df91c..19c3bf17d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.hpp @@ -21,17 +21,18 @@ #include "../Task.hpp" #include "../agv/RobotContext.hpp" -#include +#include namespace rmf_fleet_adapter { namespace tasks { //============================================================================== -std::shared_ptr make_loop(const rmf_task_msgs::msg::Loop& request, +std::shared_ptr make_loop( + const rmf_task::requests::ConstLoopRequestPtr request, const agv::RobotContextPtr& context, - rmf_utils::optional init_start, - rmf_traffic::agv::Plan::Start loop_start, - rmf_utils::optional loop_end); + 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 diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index f30650131..877673965 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -68,14 +68,14 @@ ConstRequestPtr Loop::make( // Calculate the invariant duration and battery drain for this task loop->_pimpl->invariant_duration = rmf_traffic::Duration{0}; loop->_pimpl->invariant_battery_drain = 0.0; - if (loop->_pimpl->start_waypoint != loop->_pimpl->finish_waypoint) { rmf_traffic::agv::Planner::Start loop_start{ start_time, loop->_pimpl->start_waypoint, 0.0}; - rmf_traffic::agv::Planner::Goal loop_end_goal{loop->_pimpl->start_waypoint}; + rmf_traffic::agv::Planner::Goal loop_end_goal{ + loop->_pimpl->finish_waypoint}; const auto forward_loop_plan = loop->_pimpl->planner->plan( loop_start, loop_end_goal); @@ -84,7 +84,6 @@ ConstRequestPtr Loop::make( forward_loop_plan->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); const auto forward_duration = finish_time - start_time; - loop->_pimpl->invariant_duration = (2 * num_loops - 1) * forward_duration; if (loop->_pimpl->drain_battery) From 9ea2b0e5002f31c59e7f5b41df53137218827ec6 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Thu, 12 Nov 2020 13:46:06 +0800 Subject: [PATCH 9/9] Cleanup make_loop --- .../src/rmf_fleet_adapter/tasks/Loop.cpp | 42 ------------------- 1 file changed, 42 deletions(-) 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 9c1d43db7..0ff429944 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp @@ -63,47 +63,5 @@ std::shared_ptr make_loop( request); } -// //============================================================================== -// 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; - -// const auto start_wp = -// graph.find_waypoint(request.start_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)); -// } - - -// 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); - -// phases.push_back( -// phases::GoToPlace::make(context, *loop_end, start_wp)); - -// phases.push_back( -// phases::GoToPlace::make(context, loop_start, end_wp)); -// } - -// return Task::make(request.task_id, std::move(phases), context->worker()); -// } - } // namespace tasks } // namespace rmf_fleet_adapter