diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 26f46916d..67539f596 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -20,11 +20,14 @@ #include #include #include +#include #include #include "tasks/Clean.hpp" #include "tasks/ChargeBattery.hpp" +#include "tasks/Delivery.hpp" +#include "tasks/Loop.hpp" #include @@ -195,13 +198,50 @@ 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); + + } + + 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 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..adcb882d9 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,14 @@ #include "../tasks/Delivery.hpp" #include "../tasks/Loop.hpp" +#include +#include +#include + +#include +#include +#include + #include #include @@ -236,11 +244,180 @@ 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(), + delivery.pickup_dispenser, + dropoff_wp->index(), + delivery.dropoff_ingestor, + delivery.items, + 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) { - // 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 { @@ -354,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/Delivery.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Delivery.cpp index edf93b4d3..a25cce7f3 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,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. -// * -// */ +/* + * 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 { +#include -// //============================================================================== -// 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(); +namespace rmf_fleet_adapter { +namespace tasks { -// const auto pickup_wp = -// graph.find_waypoint(request.pickup_place_name)->index(); +//============================================================================== +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())); -// Task::PendingPhases phases; -// phases.push_back( -// phases::GoToPlace::make(context, std::move(pickup_start), pickup_wp)); + phases.push_back( + std::make_unique( + context, + std::to_string(request->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)); + auto dropoff_start = request->dropoff_start(pickup_start); + phases.push_back( + phases::GoToPlace::make( + context, std::move(dropoff_start), request->dropoff_waypoint())); -// 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(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)); + } -// 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, + std::to_string(request->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( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} -// 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_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Loop.cpp index b5f9397cb..0ff429944 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,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. -// * -// */ +/* + * 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::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; + Task::PendingPhases phases; + const auto loop_start = request->loop_start(start); + const auto loop_end = request->loop_end(loop_start); -// const auto start_wp = -// graph.find_waypoint(request.start_name)->index(); + phases.push_back( + phases::GoToPlace::make( + context, std::move(start), request->start_waypoint())); -// const auto end_wp = -// graph.find_waypoint(request.finish_name)->index(); + phases.push_back( + phases::GoToPlace::make( + context, loop_start, request->finish_waypoint())); -// if (init_start) -// { -// phases.push_back( -// phases::GoToPlace::make(context, *init_start, start_wp)); -// } + 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())); + } -// phases.push_back( -// phases::GoToPlace::make(context, loop_start, end_wp)); + return Task::make( + std::to_string(request->id()), + std::move(phases), + context->worker(), + deployment_time, + finish_state, + request); +} -// 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 -// } // 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/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 bc796f07a..0b89f5a0f 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,10 +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, @@ -59,6 +67,18 @@ class Delivery : public rmf_task::Request rmf_traffic::Time earliest_start_time() const final; + std::size_t pickup_waypoint() const; + + const std::string& pickup_dispenser() const; + + std::size_t dropoff_waypoint() const; + + 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/include/rmf_task/requests/Loop.hpp b/rmf_task/include/rmf_task/requests/Loop.hpp new file mode 100644 index 000000000..1c1860ba5 --- /dev/null +++ b/rmf_task/include/rmf_task/requests/Loop.hpp @@ -0,0 +1,88 @@ +/* + * 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 +#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 ambient_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; + + std::size_t start_waypoint() const; + + std::size_t finish_waypoint() const; + + std::size_t num_loops() const; + + Start loop_start(const Start& start) const; + + Start loop_end(const Start& start) const; + + class Implementation; +private: + Loop(); + 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/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 230a5e0c1..fb15278f5 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -32,7 +32,10 @@ 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::vector _items; std::shared_ptr _motion_sink; std::shared_ptr _device_sink; std::shared_ptr _planner; @@ -47,7 +50,10 @@ 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::vector items, std::shared_ptr motion_sink, std::shared_ptr device_sink, std::shared_ptr planner, @@ -57,7 +63,10 @@ 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->_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); @@ -65,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; @@ -143,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); @@ -218,6 +232,60 @@ rmf_traffic::Time Delivery::earliest_start_time() const return _pimpl->_start_time; } +//============================================================================== +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; +} + +//============================================================================== +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/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp new file mode 100644 index 000000000..877673965 --- /dev/null +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -0,0 +1,299 @@ +/* + * 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 ambient_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + 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->finish_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 diff --git a/rmf_task/test/unit/agv/test_TaskPlanner.cpp b/rmf_task/test/unit/agv/test_TaskPlanner.cpp index 67b380428..f27c7d069 100644 --- a/rmf_task/test/unit/agv/test_TaskPlanner.cpp +++ b/rmf_task/test/unit/agv/test_TaskPlanner.cpp @@ -187,7 +187,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -197,7 +200,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -207,7 +213,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -262,7 +271,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -272,7 +284,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -282,7 +297,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 7, + "dispenser", 9, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -292,7 +310,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -302,7 +323,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 5, 10, + "dispenser", 0, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -312,7 +336,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 6, 4, + "dispenser", 8, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -322,7 +349,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 7, 8, + "dispenser", 14, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -332,7 +362,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 8, 5, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -342,7 +375,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 9, 9, + "dispenser", 0, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -352,7 +388,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 10, 1, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -362,7 +401,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 11, 0, + "dispenser", 12, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -418,7 +460,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 1, 0, + "dispenser", 3, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -428,7 +473,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 2, 15, + "dispenser", 2, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -438,7 +486,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 3, 9, + "dispenser", 4, + "ingestor", + {}, motion_sink, device_sink, planner, @@ -448,7 +499,10 @@ SCENARIO("Grid World") rmf_task::requests::Delivery::make( 4, 8, + "dispenser", 11, + "ingestor", + {}, motion_sink, device_sink, planner,