This repository was archived by the owner on Jul 22, 2021. It is now read-only.
Feature/task dispatcher#228
Merged
youliangtan merged 67 commits intodevelop/dispatcher_demofrom Jan 5, 2021
Merged
Conversation
This was referenced Dec 22, 2020
Merged
mxgrey
reviewed
Dec 23, 2020
youliangtan
force-pushed
the
feature/task-dispatcher
branch
from
f7168bb to
01dc181
Compare
youliangtan
force-pushed
the
feature/task-dispatcher
branch
from
582fcfc to
98b379f
Compare
Contributor
Author
|
Will merge this experimental feature into |
Yadunund
added a commit
that referenced
this pull request
Jan 14, 2021
* Initial commit Signed-off-by: Yadunund <yadunund@openrobotics.org> * Added unit tests for PowerSystem * Completed implementation of SystemTraits Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Fixed test Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Refactored MechanicalSystem * Added EstimateBattery Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Moved soc implementation to SimpleBatteryEstimator * Debugging soc * Debuggin messages * More debugging * Initial signs of working discharge model * Added more tests * Fixed tests * Removed message printouts * Replaced KE with energy for acceleration * Added documentation and linted * Added CleaningTaskPlanner * Added CleaningTaskPlanner.cpp and corrected unit test * Added CleaningTaskPlanner Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Added virtual destructor Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Debugging Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Fixed test for CleaningTaskPlanner Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * More tests Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Restructured CMakeLists and fixed warnings * Refactored * Setting up rmf_tasks package Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Basic setup * AGV state defined and implemented * Request abstract API and derived API for delivery Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Delivery request estimation implemented * ChargeBattery request implemented * Using finish_duration instead of finish_time * Switching back to using Time instead of Duration for finish_time Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Migration to using this library is done * Increased sim_step to 0.5 * Cleaned up unused source files and cmake commands * Initial commit Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Started implementation * Completed migration and added toy ptoblem as unit test Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Moved threshold_soc into StateConfig class * Fixed UB in pop_unassigned * Fixed UB in Candidates::make() * Updated expand_candidate and implemented correct_assingments() to add missing charging tasks * Fixed UB in greedy_solve() * estimate_finish() of Delivery checks if robot can return to its charger after reaching dropoff waypoint * Added Clean request * Cleanup * Using rmf_traffic::agv::Plan::Start for holding waypoint and time information (#188) * Using rmf_traffic::agv::Plan::Start for holding waypoint and time information Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added tests for State and StateConfig exceptions Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Using passed relative_start_time instead of getting current time using chrono Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Changing name from plan_start to location Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Removed unused efficiency from PowerSystem and added estimate_voltage() method to BatterySystem * Switched to new BatterySystem API * Renamed rmf_tasks to rmf_task * Update schedule with docking itinerary (#190) * Update schedule with docking itinerary * Use optional of reference wrapper (#191) Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Set docking itinerary every 1s Co-authored-by: Aaron Chong <aaronchongth@gmail.com> * Replaced task_id with request in Assignment * Renamed earliest_start_time to deployment_time in assignment. Modified expand_candidate. * TaskPlanner::Configuration no longer takes in a charging request. The factory is used to construct a charging request. * update msgs for dispatcher (#193) * Fixed dynamic cast * Using magnitude of w and alpha in compute_change_in_charge() * Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. * Added new Clean task, changed TaskProfile and SubmitTask to use explicit message definitions (#200) * Added new Clean task, changed TaskProfile and SubmitTask to use enum for type and explicit message definitions * Reverted to use TaskType, modified value type to uint32 * add eval enum to submit task srv (#201) * Develop/task planner integration (#199) * Added set_queue and requests methods to TaskManager * Added make_clean to convert from request to task * Commented out Delivery and Loop request subs in Adapter * Commented out Delivery and Loop related code in FleetUpdateHandle * Updated Adapter::add_fleet() * Undid changes to add_fleet which broke API/ABI. Added set_task_planner_params() to FleetUpdateHandle * Added accept_task_request to FleetUpdateHandle * Added BidNotice callback * Updated bidding topic names and started processing clean task request * Added dock related messages to rmf_fleet_msgs. Finished processing clean request. * Moved BidNotice callback into FleetUpdateHandle.cpp * RobotContext constructor taskes in State and StateConfig which are used for task planning * Added is_charger() and set_charger() methods to Waypoint API * add_robot uses get_nearest_charger() * parse_graph parses is_charger option * Added state_config() to RobotContext * Added dispatch_request_cb() * Updated set_queue() and make_clean() functions. Switching to using ConstRequestPtr everywhere * Broke Task constructor API * Added timer to TaskManager * Locking mutex when modifying task queue * Dispatcher pipeline works! * Cleaned up printouts * Update finish time after robot has completed task * Get latest state of robots before running task planner. TODO: Update battery soc * Fixed bug in compute_change_of_charge() * Using magnitude of w and alpha in compute_change_in_charge() * Added update_battery_soc() to RobotUpdateHandle which updates the current_battery_soc in RobotContext. Initial states for task planning use this updated battery_soc * Added ChargeBattery task factory * Added skeleton for WaitForCharge * Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. * Fixed return value of battery_system() in ChargeBattery * Catch exception for invalid cleaning trajectories * Populate finish_time in BidProposal * Added observable and publisher for current_battery_soc * Updated begin() for WaitForCharge phase * Fixed completion logic in WaitForCharge * Fixed typo * Updated fleet adapter launch params. Populate task id in TaskProfile in Status msg * Become stubborn while docking * Added recharge_threshold parameter to fleet adapter launch * Fix missing return value in current_battery_soc() * Populate robot_name in BidProposal msg * TaskManager queues charging tasks * Modify start in set_queue() * Added second GoToPlace phase for Clean task * Added TODO to account for travel from end of cleaning trajectory to end_waypoint * Populate robot name in TaskSummary msg * Populate TaskSummary * Changed TaskType enum names and updated FleetUpdateHandle to work with new TaskProfile msg Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Co-authored-by: Michael X. Grey <grey@openrobotics.org> * ChargeBattery task creates WaitForCharge phase with 0.99 max threshold. Fixed valid() check in PowerSystem * Added equality constraint in PowerSystem::valid() * Add unit test to detect implicit charging task at the start of Assignments (#208) * Add unit test to detect implicit charging task in the beginning Test checks for discrepancy in state of charge to see if charging has been taken into account without being explicitly added to Assignments. * Fix false negative in unit test case for implicit charging task Checks if task is a charging task before returning an error. * Deal with implicit charging before first assignment (#209) * Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. * Loop and Delivery integration with dispatcher framework (#211) * Added creation of delivery request * Modified make and extended API for Delivery request * Added items() and dropoff_start() to Delivery request * Deleted old Delivery task implementation * Loop request skeleton * Implemented Loop request * Parse loop request in FleetUpdateHandle * Integrated Loop requests * Cleanup make_loop * Updated tests after merging in heuristic changes for task planner * Change id field of Request to string and modify TaskPlanner to use internal ids instead (#203) * Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. * Updated FleetUpdateHandle and Tasks to use string request id * Cache estimated values to speed up TaskPlanner (#212) * Squashed commit of the following: commit b69a5de Merge: 6206e41 005e88b Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 10 13:24:30 2020 +0800 Merge branch 'feature/task_planner' into refactor_id_w_map commit 005e88b Author: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Date: Tue Nov 10 12:08:34 2020 +0800 Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. commit 6206e41 Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 3 18:20:24 2020 +0800 Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. commit d00b3f3 Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 3 10:06:46 2020 +0800 Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Cache request estimate values in TaskPlanner Create new EstimateCache class and memoize the duration and charge consumed when travelling betwen any 2 waypoints on the graph. * Recreate TaskPlanner in tests for more accurate run time measurements Also adds other stylistic changes including renaming of CacheElem to CacheElement. * few liners to populate start and estimated finish time (#216) * Cleaned up commented code * Made ChargeBattery ids unique * add fleetname to task summary * Debug/task planner candidates (#218) * Added test for planning an impossible to complete task * TaskPlanner returns empty assignments if request is impossible to complete * Updated API documentation in FleetUpdateHandle * TaskPlanner returns an std::variant (#221) * TaskPlanner solve methods return variant * Cleanup * Updated fleet adapter implementation * Fixed typo * Fixed logging * Fixed logging * Changed logging level to Error for task planner failure messages * Automatic retreat for battery recharging (#222) * TaskManager initiates a ChargeBattery request when robot is idle and battery will fall below threshold * Adjusted retreat threshold * Take into account battery drain while idling in TaskPlanner estimates (#223) * Take into account charge depletion due to waiting to begin task * Add charge depletion for Loop and Clean tasks. Rename Delivery::Implementation Also modifies ChargeBattery requests to return a value even if charge is already at 100%, if a robot is not currently at a charger. * Remove leading underscores from ChargeBattery::Implementation * Removed commented code from Adapter.cpp * Powersystem does not require string name * Added documentation for MechanicalSystem * Removed BatteryProfile. Switched to make() pattern in rmf_battery * Addressed suggestions for rmf_battery * Added account_for_batery_drain() * Added unstable API extension to RobotUpdateHandle * Log error message when battery percentage in RobotState is out of range * Removed TODO in Task and iostream header * Renamed state() to current_task_end_state() in Robotcontext * TaskManager queue.clear() is protected by mutex * Protected read instances of queue with mutex * Added warning when robot has insufficient charge to automatically retreat to charger * Removed clear_queue() and moved begin_next_task into private * Added TODO to redesign TaskManager * Bumped printouts for missing fields to ERROR and other corrections to FleetUpdateHandle * retreat_to_charger() estimates battery drain over all trajectories in the plan * Battery drain is computed over all trajectories in a generated plan * Iterate over all trajectories in plan when computing invariant battery drain * Rename inertia to moment_of_inertia * Added const type specifier and comments for test expectations in rmf_battery * Use RCLCPP_DEBUG for printing assignments and fix const correctness of current_task_end_state() * Added TODO to fix request_delivery and request_loop functions in MockAdapter * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task/include/rmf_task/agv/TaskPlanner.hpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task_msgs/srv/GetTask.srv Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task_msgs/srv/CancelTask.srv Co-authored-by: Grey <grey@openrobotics.org> * Fixed CMakeLists.txt and header guards for rmf_task * Added setters for other parameters in TaskPlanner Configuration * Removed FilterType from public API. Planner uses Hash type always. * Updated note on friction_coefficient * Added documentation for requests and fixed comments * Renamed GetTask to GetTaskList * Renamed GetTask to GetTaskList * Added set_charger_waypoint() to RobotUpdateHandle * Hashing in EstimateCache uses sum not xor * Renamed StateConfig to Constraints * Reworked PairHash * Corrected shift in PairHash * Return task_planning_constraints by const ref * Updated Changelogs * Feature/task dispatcher (#228) * prototyping a minimal dispatcher with new task_msgs * add new exec * skeleton for bidder and nomination * further complete bidder and dispatcher * create simple test exec with bidding pipeline * first version restructuring of new bidding mechanism * preliminary test run of bidder and dispatcher nodes * update codes * redesign task action interfaces and test out task action interactions * restructure rmf tasks dir and enhance task tracking on dispatcher * add simple test on bidding * refactor and define dispatchernode api * halfway game thru restructuring of code * expose dispatcher api and change action msg fields * dispatcher lib api * continuation of work on restucturing dispatcher api * update msg definitions and apis * adding srv msg, basic integration test * cleanups and further complete dispatcher node * lint, use TaskSummary as TaskStatus * minor changes based on comments * update msgs for dispatcher * Update dispatcher api * extend impl for dispatcher, and add dockerfile * sequential bidding and further refactoring, then test * latest minor fix on UB * merge with new msgs * cleanup diff * add loop type * receiving stray task summarry msg * merge with dispatcher-demo and rm charging task from cache on every reassignment * merge latest master on traffic light * refactoring actino task templates * refactor and cleanups again * remove charge battery id gen * first review cleanup iteration * more cleanups * remove template action * further refactoring * fix compilations * con't refactor * create TaskDescription.Msg in TaskProfile * append task_description and update task_id gen * reorg apis and add internal_Auctioneer * fix issue on ignoring stray taskstatus * change default eval and update cancel task impl * switch to std::optional * remove max limit terminated tasks according to submission time * refactor headers * fix bug on unusual short bid duration * loop delivery tests for dispatcher (#236) * unit tests for loop and delivery requests with task dispatcher * minor cleanup * refactor dispatch_task() method * Implementation of EstimateCache uses std::mutex object and not shared pointer * add changelog and clean printout logs Co-authored-by: Aaron Chong <aaronchongth@gmail.com> Co-authored-by: youliang <tan_you_liang@hotmail.com> Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Co-authored-by: Michael X. Grey <grey@openrobotics.org>
marcoag
pushed a commit
that referenced
this pull request
Jan 15, 2021
* Improve planner performance -- Nav Graph Complexity (#243) * Use a spin lock for short-wait mutexes (#248) * removing fcl as its not a rosdep key (#234) * removing fcl as its not a rosdep key Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org> * Add a comment for users who want to use fcl-0.6 Co-authored-by: Michael X. Grey <grey@openrobotics.org> * version bump and changelog dates (#252) Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org> * Adds a parameter for reduced cpu usage during testing. (#253) * Messages for Chargers (#245) * Initial charger strawman messages * Stylistic fixes * Add prefix * update sertvices with "robot ids" * Add model and vendor * Switched to message only format as suggested by @codebot * fix minor issue * update with more of @codebot 's suggestions. * spelling mistake fix and more updates * uypdate charger status message * use single robot * update docs * docs update * docs updated * minor tweaks * whoops... forgot the state `uint16` * make naming more consistent * add time to fully charged field * incorporate review feedback Signed-off-by: Morgan Quigley <morgan@osrfoundation.org> Co-authored-by: Morgan Quigley <morgan@osrfoundation.org> * Add search node count API to Planner::Debug (#257) * Add request id parameter (#256) * add request id parameter * minor edits to comments Signed-off-by: Morgan Quigley <morgan@osrfoundation.org> Co-authored-by: Morgan Quigley <morgan@osrfoundation.org> * Fix an incorrect assertion (#259) * rclcpp requires const char * for logging macros Signed-off-by: Marco A. Gutierrez <marco@openrobotics.org> * Develop/dispatcher demo (#217) * Initial commit Signed-off-by: Yadunund <yadunund@openrobotics.org> * Added unit tests for PowerSystem * Completed implementation of SystemTraits Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Fixed test Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Refactored MechanicalSystem * Added EstimateBattery Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Moved soc implementation to SimpleBatteryEstimator * Debugging soc * Debuggin messages * More debugging * Initial signs of working discharge model * Added more tests * Fixed tests * Removed message printouts * Replaced KE with energy for acceleration * Added documentation and linted * Added CleaningTaskPlanner * Added CleaningTaskPlanner.cpp and corrected unit test * Added CleaningTaskPlanner Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Added virtual destructor Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Debugging Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Fixed test for CleaningTaskPlanner Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * More tests Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Restructured CMakeLists and fixed warnings * Refactored * Setting up rmf_tasks package Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Basic setup * AGV state defined and implemented * Request abstract API and derived API for delivery Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Delivery request estimation implemented * ChargeBattery request implemented * Using finish_duration instead of finish_time * Switching back to using Time instead of Duration for finish_time Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Migration to using this library is done * Increased sim_step to 0.5 * Cleaned up unused source files and cmake commands * Initial commit Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Started implementation * Completed migration and added toy ptoblem as unit test Signed-off-by: Yadunund Vijay <yadunund@openrobotics.org> * Moved threshold_soc into StateConfig class * Fixed UB in pop_unassigned * Fixed UB in Candidates::make() * Updated expand_candidate and implemented correct_assingments() to add missing charging tasks * Fixed UB in greedy_solve() * estimate_finish() of Delivery checks if robot can return to its charger after reaching dropoff waypoint * Added Clean request * Cleanup * Using rmf_traffic::agv::Plan::Start for holding waypoint and time information (#188) * Using rmf_traffic::agv::Plan::Start for holding waypoint and time information Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Added tests for State and StateConfig exceptions Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Using passed relative_start_time instead of getting current time using chrono Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Changing name from plan_start to location Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Removed unused efficiency from PowerSystem and added estimate_voltage() method to BatterySystem * Switched to new BatterySystem API * Renamed rmf_tasks to rmf_task * Update schedule with docking itinerary (#190) * Update schedule with docking itinerary * Use optional of reference wrapper (#191) Signed-off-by: Aaron Chong <aaronchongth@gmail.com> * Set docking itinerary every 1s Co-authored-by: Aaron Chong <aaronchongth@gmail.com> * Replaced task_id with request in Assignment * Renamed earliest_start_time to deployment_time in assignment. Modified expand_candidate. * TaskPlanner::Configuration no longer takes in a charging request. The factory is used to construct a charging request. * update msgs for dispatcher (#193) * Fixed dynamic cast * Using magnitude of w and alpha in compute_change_in_charge() * Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. * Added new Clean task, changed TaskProfile and SubmitTask to use explicit message definitions (#200) * Added new Clean task, changed TaskProfile and SubmitTask to use enum for type and explicit message definitions * Reverted to use TaskType, modified value type to uint32 * add eval enum to submit task srv (#201) * Develop/task planner integration (#199) * Added set_queue and requests methods to TaskManager * Added make_clean to convert from request to task * Commented out Delivery and Loop request subs in Adapter * Commented out Delivery and Loop related code in FleetUpdateHandle * Updated Adapter::add_fleet() * Undid changes to add_fleet which broke API/ABI. Added set_task_planner_params() to FleetUpdateHandle * Added accept_task_request to FleetUpdateHandle * Added BidNotice callback * Updated bidding topic names and started processing clean task request * Added dock related messages to rmf_fleet_msgs. Finished processing clean request. * Moved BidNotice callback into FleetUpdateHandle.cpp * RobotContext constructor taskes in State and StateConfig which are used for task planning * Added is_charger() and set_charger() methods to Waypoint API * add_robot uses get_nearest_charger() * parse_graph parses is_charger option * Added state_config() to RobotContext * Added dispatch_request_cb() * Updated set_queue() and make_clean() functions. Switching to using ConstRequestPtr everywhere * Broke Task constructor API * Added timer to TaskManager * Locking mutex when modifying task queue * Dispatcher pipeline works! * Cleaned up printouts * Update finish time after robot has completed task * Get latest state of robots before running task planner. TODO: Update battery soc * Fixed bug in compute_change_of_charge() * Using magnitude of w and alpha in compute_change_in_charge() * Added update_battery_soc() to RobotUpdateHandle which updates the current_battery_soc in RobotContext. Initial states for task planning use this updated battery_soc * Added ChargeBattery task factory * Added skeleton for WaitForCharge * Fix lifetime of variant_duration variable (#197) Fixes redeclaration of variant_duration inside if-block scope. * Fixed return value of battery_system() in ChargeBattery * Catch exception for invalid cleaning trajectories * Populate finish_time in BidProposal * Added observable and publisher for current_battery_soc * Updated begin() for WaitForCharge phase * Fixed completion logic in WaitForCharge * Fixed typo * Updated fleet adapter launch params. Populate task id in TaskProfile in Status msg * Become stubborn while docking * Added recharge_threshold parameter to fleet adapter launch * Fix missing return value in current_battery_soc() * Populate robot_name in BidProposal msg * TaskManager queues charging tasks * Modify start in set_queue() * Added second GoToPlace phase for Clean task * Added TODO to account for travel from end of cleaning trajectory to end_waypoint * Populate robot name in TaskSummary msg * Populate TaskSummary * Changed TaskType enum names and updated FleetUpdateHandle to work with new TaskProfile msg Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Co-authored-by: Michael X. Grey <grey@openrobotics.org> * ChargeBattery task creates WaitForCharge phase with 0.99 max threshold. Fixed valid() check in PowerSystem * Added equality constraint in PowerSystem::valid() * Add unit test to detect implicit charging task at the start of Assignments (#208) * Add unit test to detect implicit charging task in the beginning Test checks for discrepancy in state of charge to see if charging has been taken into account without being explicitly added to Assignments. * Fix false negative in unit test case for implicit charging task Checks if task is a charging task before returning an error. * Deal with implicit charging before first assignment (#209) * Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. * Loop and Delivery integration with dispatcher framework (#211) * Added creation of delivery request * Modified make and extended API for Delivery request * Added items() and dropoff_start() to Delivery request * Deleted old Delivery task implementation * Loop request skeleton * Implemented Loop request * Parse loop request in FleetUpdateHandle * Integrated Loop requests * Cleanup make_loop * Updated tests after merging in heuristic changes for task planner * Change id field of Request to string and modify TaskPlanner to use internal ids instead (#203) * Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. * Updated FleetUpdateHandle and Tasks to use string request id * Cache estimated values to speed up TaskPlanner (#212) * Squashed commit of the following: commit b69a5de Merge: 6206e41 005e88b Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 10 13:24:30 2020 +0800 Merge branch 'feature/task_planner' into refactor_id_w_map commit 005e88b Author: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Date: Tue Nov 10 12:08:34 2020 +0800 Reduce heuristic overestimation and ignore charging task costs in TaskPlanner (#207) * Improved heuristic to reduce overestimation Takes into account earliest start time when computing the cost of a task, more closely mimicing g(n). * Add field to Assignment determining whether to include in final cost Enables charging tasks to be ignored from final calculation. Adds miscellaneous comments explaining h(n) calculation. * Remove additional include_in_cost field from Assignment Use dynamic_pointer_cast to identify charging requests instead. * Add testcase illustrating difference in costs with new g(n) and h(n) * Include smallest variant cost in h(n) computation Include the smallest possible variant cost among the unasigned tasks for each candidate when computing the heuristic cost. commit 6206e41 Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 3 18:20:24 2020 +0800 Replace map with autogenerated IDs upon task insertion into Node Creates wrappers for Assignments, containing an internal_id field. This field is generated by the Node when inserting a new task. commit d00b3f3 Author: mrushyendra <mrushyendra@yahoo.com.sg> Date: Tue Nov 3 10:06:46 2020 +0800 Change request id field from size_t to string Modifies task planner, tests and requests code to use string based id instead of size_t. * Cache request estimate values in TaskPlanner Create new EstimateCache class and memoize the duration and charge consumed when travelling betwen any 2 waypoints on the graph. * Recreate TaskPlanner in tests for more accurate run time measurements Also adds other stylistic changes including renaming of CacheElem to CacheElement. * few liners to populate start and estimated finish time (#216) * Cleaned up commented code * Made ChargeBattery ids unique * add fleetname to task summary * Debug/task planner candidates (#218) * Added test for planning an impossible to complete task * TaskPlanner returns empty assignments if request is impossible to complete * Updated API documentation in FleetUpdateHandle * TaskPlanner returns an std::variant (#221) * TaskPlanner solve methods return variant * Cleanup * Updated fleet adapter implementation * Fixed typo * Fixed logging * Fixed logging * Changed logging level to Error for task planner failure messages * Automatic retreat for battery recharging (#222) * TaskManager initiates a ChargeBattery request when robot is idle and battery will fall below threshold * Adjusted retreat threshold * Take into account battery drain while idling in TaskPlanner estimates (#223) * Take into account charge depletion due to waiting to begin task * Add charge depletion for Loop and Clean tasks. Rename Delivery::Implementation Also modifies ChargeBattery requests to return a value even if charge is already at 100%, if a robot is not currently at a charger. * Remove leading underscores from ChargeBattery::Implementation * Removed commented code from Adapter.cpp * Powersystem does not require string name * Added documentation for MechanicalSystem * Removed BatteryProfile. Switched to make() pattern in rmf_battery * Addressed suggestions for rmf_battery * Added account_for_batery_drain() * Added unstable API extension to RobotUpdateHandle * Log error message when battery percentage in RobotState is out of range * Removed TODO in Task and iostream header * Renamed state() to current_task_end_state() in Robotcontext * TaskManager queue.clear() is protected by mutex * Protected read instances of queue with mutex * Added warning when robot has insufficient charge to automatically retreat to charger * Removed clear_queue() and moved begin_next_task into private * Added TODO to redesign TaskManager * Bumped printouts for missing fields to ERROR and other corrections to FleetUpdateHandle * retreat_to_charger() estimates battery drain over all trajectories in the plan * Battery drain is computed over all trajectories in a generated plan * Iterate over all trajectories in plan when computing invariant battery drain * Rename inertia to moment_of_inertia * Added const type specifier and comments for test expectations in rmf_battery * Use RCLCPP_DEBUG for printing assignments and fix const correctness of current_task_end_state() * Added TODO to fix request_delivery and request_loop functions in MockAdapter * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task/include/rmf_task/agv/TaskPlanner.hpp Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task_msgs/srv/GetTask.srv Co-authored-by: Grey <grey@openrobotics.org> * Update rmf_task_msgs/srv/CancelTask.srv Co-authored-by: Grey <grey@openrobotics.org> * Fixed CMakeLists.txt and header guards for rmf_task * Added setters for other parameters in TaskPlanner Configuration * Removed FilterType from public API. Planner uses Hash type always. * Updated note on friction_coefficient * Added documentation for requests and fixed comments * Renamed GetTask to GetTaskList * Renamed GetTask to GetTaskList * Added set_charger_waypoint() to RobotUpdateHandle * Hashing in EstimateCache uses sum not xor * Renamed StateConfig to Constraints * Reworked PairHash * Corrected shift in PairHash * Return task_planning_constraints by const ref * Updated Changelogs * Feature/task dispatcher (#228) * prototyping a minimal dispatcher with new task_msgs * add new exec * skeleton for bidder and nomination * further complete bidder and dispatcher * create simple test exec with bidding pipeline * first version restructuring of new bidding mechanism * preliminary test run of bidder and dispatcher nodes * update codes * redesign task action interfaces and test out task action interactions * restructure rmf tasks dir and enhance task tracking on dispatcher * add simple test on bidding * refactor and define dispatchernode api * halfway game thru restructuring of code * expose dispatcher api and change action msg fields * dispatcher lib api * continuation of work on restucturing dispatcher api * update msg definitions and apis * adding srv msg, basic integration test * cleanups and further complete dispatcher node * lint, use TaskSummary as TaskStatus * minor changes based on comments * update msgs for dispatcher * Update dispatcher api * extend impl for dispatcher, and add dockerfile * sequential bidding and further refactoring, then test * latest minor fix on UB * merge with new msgs * cleanup diff * add loop type * receiving stray task summarry msg * merge with dispatcher-demo and rm charging task from cache on every reassignment * merge latest master on traffic light * refactoring actino task templates * refactor and cleanups again * remove charge battery id gen * first review cleanup iteration * more cleanups * remove template action * further refactoring * fix compilations * con't refactor * create TaskDescription.Msg in TaskProfile * append task_description and update task_id gen * reorg apis and add internal_Auctioneer * fix issue on ignoring stray taskstatus * change default eval and update cancel task impl * switch to std::optional * remove max limit terminated tasks according to submission time * refactor headers * fix bug on unusual short bid duration * loop delivery tests for dispatcher (#236) * unit tests for loop and delivery requests with task dispatcher * minor cleanup * refactor dispatch_task() method * Implementation of EstimateCache uses std::mutex object and not shared pointer * add changelog and clean printout logs Co-authored-by: Aaron Chong <aaronchongth@gmail.com> Co-authored-by: youliang <tan_you_liang@hotmail.com> Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg> Co-authored-by: Michael X. Grey <grey@openrobotics.org> * Fix iterator invalidation (#261) * Fix the cmake of rmf_fleet_adapter (#262) Co-authored-by: Grey <grey@openrobotics.org> Co-authored-by: Arjo Chakravarty <arjo129@gmail.com> Co-authored-by: Morgan Quigley <morgan@osrfoundation.org> Co-authored-by: Yadu <yadunund@openrobotics.org> Co-authored-by: Aaron Chong <aaronchongth@gmail.com> Co-authored-by: youliang <tan_you_liang@hotmail.com> Co-authored-by: Rushyendra Maganty <mrushyendra@yahoo.com.sg>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to subscribe to this conversation on GitHub.
Already have an account?
Sign in.
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This PR introduces the rmf dispatcher node, an add-on to the task planner PR (#217). Note that this PR is in sync with the aforementioned
dispatcher_demoPR.Here, a dispatcher node is used to facilitate the bidding process on dispatching the best fleet adapter to work the task. Also the node will aggregate all robot task status within the RMF. As this is relevant to the dispatcher node, all development is placed within
rmf_task_ros2package.To run the dispatcher node
3 simple interfaces are used to assist external applications (e.g. Web GUI) to initiate and track tasks in RMF:
/submit_task: submit a task/cancel_task: cancel an ongoing task/get_tasks: get all active and terminated tasks statusHow to test it
To conduct a full test with
rmf_demos(branch:develop/rmf_demo_panel), we will be using the developingrmf_demo_panelto initiate and track tasks. Setup instruction is located in this README