From 74f661a27c6f7e9b2661c6f5d5d6f5868ea91bbb Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 21 Dec 2023 21:07:46 +0800 Subject: [PATCH] Update `GoToPlace` to allow finding nearest spot (#308) Signed-off-by: Arjo Chakravarty Signed-off-by: Michael X. Grey Co-authored-by: Michael X. Grey --- .../event_description__go_to_place.json | 34 ++- .../rmf_fleet_adapter/events/GoToPlace.cpp | 210 +++++++++++++++--- .../rmf_fleet_adapter/events/GoToPlace.hpp | 22 +- .../events/ResponsiveWait.cpp | 3 +- .../src/rmf_fleet_adapter/tasks/Patrol.cpp | 36 +++ 5 files changed, 257 insertions(+), 48 deletions(-) diff --git a/rmf_fleet_adapter/schemas/event_description__go_to_place.json b/rmf_fleet_adapter/schemas/event_description__go_to_place.json index 8d932d01f..11bff60d1 100644 --- a/rmf_fleet_adapter/schemas/event_description__go_to_place.json +++ b/rmf_fleet_adapter/schemas/event_description__go_to_place.json @@ -18,6 +18,38 @@ } }, "required": ["place"] + }, + { + "type": "object", + "properties" : { + "one_of" : { + "type": "array", + "description": "A list of places to choose from. Will try to go to the nearest one.", + "items": { "$ref": "place.json" }, + "minItems": 1 + }, + "constraints" : { + "type": "array", + "description": "list of constraints to use", + "items": {"$ref": "#/$defs/constraint"} + } + }, + "required": ["one_of"] + } + ], + "$defs": { + "constraint": { + "type": "object", + "properties": { + "category": { + "type": "string", + "description": "The type of constraint, e.g. prefer_same_map" + }, + "description": { + "description": "A complete description of the constraint" + } + }, + "required": ["category"] } - ] + } } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp index 671e62b99..362962346 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.cpp @@ -19,6 +19,7 @@ #include "../project_itinerary.hpp" #include +#include namespace rmf_fleet_adapter { namespace events { @@ -67,8 +68,7 @@ auto GoToPlace::Standby::make( const auto context = state.get()->value; const auto header = description.generate_header(state, *parameters); - auto standby = std::make_shared(Standby{description.destination()}); - standby->_followed_by = description.expected_next_destinations(); + auto standby = std::make_shared(Standby{description}); standby->_assign_id = id; standby->_context = context; standby->_time_estimate = header.original_duration_estimate(); @@ -86,8 +86,8 @@ auto GoToPlace::Standby::make( } //============================================================================== -GoToPlace::Standby::Standby(rmf_traffic::agv::Plan::Goal goal) -: _goal(std::move(goal)) +GoToPlace::Standby::Standby(Description description) +: _description(std::move(description)) { // Do nothin } @@ -111,17 +111,10 @@ auto GoToPlace::Standby::begin( { if (!_active) { - RCLCPP_INFO( - _context->node()->get_logger(), - "Beginning a new go_to_place [%lu] for robot [%s]", - _goal.waypoint(), - _context->requester_id().c_str()); - _active = Active::make( _assign_id, _context, - _goal, - _followed_by, + _description, _tail_period, _state, _update, @@ -135,21 +128,41 @@ auto GoToPlace::Standby::begin( auto GoToPlace::Active::make( const AssignIDPtr& id, agv::RobotContextPtr context, - rmf_traffic::agv::Plan::Goal goal, - std::vector followed_by, + Description description, std::optional tail_period, rmf_task::events::SimpleEventStatePtr state, std::function update, std::function finished) -> std::shared_ptr { - auto active = std::make_shared(Active(std::move(goal))); - active->_followed_by = std::move(followed_by); + auto active = std::make_shared(Active(std::move(description))); active->_assign_id = id; active->_context = std::move(context); active->_tail_period = tail_period; active->_update = std::move(update); active->_finished = std::move(finished); active->_state = std::move(state); + + if (active->_description.one_of().empty()) + { + active->_state->update_status(Status::Error); + active->_state->update_log().error( + "No destination option was provided to go_to_place. There is nowhere to " + "go, so we will proceed to the next step in the task."); + + RCLCPP_ERROR( + active->_context->node()->get_logger(), + "No destination option was provided for a go_to_place for [%s]. There is " + "nowhere to go, so we will proceed to the next step in the task.", + active->_context->requester_id().c_str()); + + active->_context->worker().schedule( + [finished = active->_finished](const auto&) + { + finished(); + }); + return active; + } + active->_negotiator = Negotiator::make( active->_context, @@ -281,8 +294,13 @@ rmf_traffic::Duration GoToPlace::Active::remaining_time_estimate() const } } + if (!_chosen_goal.has_value()) + { + return rmf_traffic::Duration(0); + } + const auto& estimate = - _context->planner()->setup(_context->location(), _goal); + _context->planner()->setup(_context->location(), _chosen_goal->waypoint()); if (estimate.ideal_cost().has_value()) return rmf_traffic::time::from_seconds(*estimate.ideal_cost()); @@ -334,8 +352,7 @@ void GoToPlace::Active::cancel() { RCLCPP_INFO( _context->node()->get_logger(), - "Canceling go_to_place [%lu] for robot [%s]", - _goal.waypoint(), + "Canceling go_to_place for robot [%s]", _context->requester_id().c_str()); _stop_and_clear(); _state->update_status(Status::Canceled); @@ -383,15 +400,121 @@ std::string wp_name(const agv::RobotContext& context) return "#" + std::to_string(locations.front().waypoint()); } +//============================================================================== +std::optional GoToPlace::Active::_choose_goal( + bool only_same_map) const +{ + auto current_location = _context->location(); + auto graph = _context->navigation_graph(); + if (current_location.size() == 0) + { + //unable to get location. We should return some form of error stste. + RCLCPP_ERROR( + _context->node()->get_logger(), + "Robot [%s] can't get location", + _context->requester_id().c_str()); + return std::nullopt; + } + + RCLCPP_INFO( + _context->node()->get_logger(), + "Selecting a new go_to_place location from [%lu] choices for robot [%s]", + _description.one_of().size(), + _context->requester_id().c_str()); + + auto lowest_cost = std::numeric_limits::infinity(); + std::optional selected_idx; + for (std::size_t i = 0; i < _description.one_of().size(); ++i) + { + const auto wp_idx = _description.one_of()[i].waypoint(); + if (only_same_map) + { + const auto& wp = graph.get_waypoint(wp_idx); + + // Check if same map. If not don't consider location. This is to ensure + // the robot does not try to board a lift. + if (wp.get_map_name() != _context->map()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Skipping [%lu] as it is on map [%s] but robot is on map [%s].", + wp_idx, + wp.get_map_name().c_str(), + _context->map().c_str()); + continue; + } + } + + // Find distance to said point + auto result = _context->planner()->quickest_path(current_location, wp_idx); + if (result.has_value()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Got distance from [%lu] as %f", + wp_idx, + result->cost()); + + if (result->cost() < lowest_cost) + { + selected_idx = i; + lowest_cost = result->cost(); + } + } + else + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "No path found for robot [%s] to waypoint [%lu]", + wp_idx, + _context->requester_id().c_str()); + } + } + + if (selected_idx.has_value()) + { + return _description.one_of()[*selected_idx]; + } + + return std::nullopt; +} + //============================================================================== void GoToPlace::Active::_find_plan() { if (_is_interrupted) return; + if (!_chosen_goal.has_value() && _description.prefer_same_map()) + { + _chosen_goal = _choose_goal(true); + } + + if (!_chosen_goal.has_value()) + { + _chosen_goal = _choose_goal(false); + } + + if (!_chosen_goal.has_value()) + { + std::string error_msg = "Unable to find a path to any of the goal options [" + + _description.destination_name(*_context->task_parameters()) + + "]"; + + _state->update_log().error(error_msg); + RCLCPP_ERROR( + _context->node()->get_logger(), + "%s for [%s]", + error_msg.c_str(), + _context->requester_id().c_str()); + + _schedule_retry(); + return; + } + _state->update_status(Status::Underway); const auto start_name = wp_name(*_context); - const auto goal_name = wp_name(*_context, _goal); + const auto goal_name = wp_name(*_context, *_chosen_goal); _state->update_log().info( "Generating plan to move from [" + start_name + "] to [" + goal_name + "]"); @@ -408,7 +531,7 @@ void GoToPlace::Active::_find_plan() // TODO(MXG): Make the planning time limit configurable _find_path_service = std::make_shared( - _context->planner(), _context->location(), _goal, + _context->planner(), _context->location(), *_chosen_goal, _context->schedule()->snapshot(), _context->itinerary().id(), _context->profile(), std::chrono::seconds(5)); @@ -417,7 +540,7 @@ void GoToPlace::Active::_find_plan() _find_path_service) .observe_on(rxcpp::identity_same_worker(_context->worker())) .subscribe( - [w = weak_from_this(), start_name, goal_name]( + [w = weak_from_this(), start_name, goal_name, goal = *_chosen_goal]( const services::FindPath::Result& result) { const auto self = w.lock(); @@ -432,6 +555,9 @@ void GoToPlace::Active::_find_plan() "Failed to find a plan to move from [" + start_name + "] to [" + goal_name + "]. Will retry soon."); + // Reset the chosen goal in case this goal has become impossible to + // reach + self->_chosen_goal = std::nullopt; self->_execution = std::nullopt; self->_schedule_retry(); @@ -447,12 +573,14 @@ void GoToPlace::Active::_find_plan() + start_name + "] to [" + goal_name + "]"); auto full_itinerary = project_itinerary( - *result, self->_followed_by, *self->_context->planner()); + *result, self->_description.expected_next_destinations(), + *self->_context->planner()); self->_execute_plan( self->_context->itinerary().assign_plan_id(), *std::move(result), - std::move(full_itinerary)); + std::move(full_itinerary), + goal); self->_find_path_service = nullptr; self->_retry_timer = nullptr; @@ -476,8 +604,8 @@ void GoToPlace::Active::_find_plan() } //============================================================================== -GoToPlace::Active::Active(rmf_traffic::agv::Plan::Goal goal) -: _goal(std::move(goal)) +GoToPlace::Active::Active(Description description) +: _description(std::move(description)) { // Do nothing } @@ -509,7 +637,8 @@ void GoToPlace::Active::_schedule_retry() void GoToPlace::Active::_execute_plan( const rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, - rmf_traffic::schedule::Itinerary full_itinerary) + rmf_traffic::schedule::Itinerary full_itinerary, + rmf_traffic::agv::Plan::Goal goal) { if (_is_interrupted) return; @@ -523,24 +652,28 @@ void GoToPlace::Active::_execute_plan( _context->node()->get_logger(), "Robot [%s] is already at its goal [%lu]", _context->requester_id().c_str(), - _goal.waypoint()); + goal.waypoint()); const auto& graph = _context->navigation_graph(); _context->retain_mutex_groups( - {graph.get_waypoint(_goal.waypoint()).in_mutex_group()}); + {graph.get_waypoint(goal.waypoint()).in_mutex_group()}); _finished(); return; } + const auto& graph = _context->navigation_graph(); + RCLCPP_INFO( _context->node()->get_logger(), - "Executing go_to_place [%lu] for robot [%s]", - _goal.waypoint(), + "Executing go_to_place [%s] for robot [%s]", + graph.get_waypoint(plan.get_waypoints().back().graph_index().value()) + .name_or_index().c_str(), _context->requester_id().c_str()); _execution = ExecutePlan::make( - _context, plan_id, std::move(plan), _goal, std::move(full_itinerary), + _context, plan_id, std::move(plan), std::move(goal), + std::move(full_itinerary), _assign_id, _state, _update, _finished, _tail_period); if (!_execution.has_value()) @@ -568,7 +701,13 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder) { - auto approval_cb = [w = weak_from_this()]( + if (!_chosen_goal.has_value()) + { + responder->forfeit({}); + return nullptr; + } + + auto approval_cb = [w = weak_from_this(), goal = *_chosen_goal]( const rmf_traffic::PlanId plan_id, const rmf_traffic::agv::Plan& plan, rmf_traffic::schedule::Itinerary itinerary) @@ -576,7 +715,7 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( { if (auto self = w.lock()) { - self->_execute_plan(plan_id, plan, std::move(itinerary)); + self->_execute_plan(plan_id, plan, std::move(itinerary), goal); return self->_context->itinerary().version(); } @@ -586,7 +725,8 @@ Negotiator::NegotiatePtr GoToPlace::Active::_respond( const auto evaluator = Negotiator::make_evaluator(table_view); return services::Negotiate::path( _context->itinerary().assign_plan_id(), _context->planner(), - _context->location(), _goal, _followed_by, table_view, + _context->location(), *_chosen_goal, + _description.expected_next_destinations(), table_view, responder, std::move(approval_cb), std::move(evaluator)); } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp index 89eb829ee..fca095903 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/GoToPlace.hpp @@ -49,7 +49,7 @@ class GoToPlace : public rmf_task_sequence::Event const AssignIDPtr& id, const std::function& get_state, const rmf_task::ConstParametersPtr& parameters, - const rmf_task_sequence::events::GoToPlace::Description& description, + const Description& description, std::function update, std::optional tail_period = std::nullopt); @@ -63,10 +63,9 @@ class GoToPlace : public rmf_task_sequence::Event private: - Standby(rmf_traffic::agv::Plan::Goal goal); + Standby(Description description); - rmf_traffic::agv::Plan::Goal _goal; - std::vector _followed_by; + Description _description; AssignIDPtr _assign_id; agv::RobotContextPtr _context; rmf_traffic::Duration _time_estimate; @@ -85,8 +84,7 @@ class GoToPlace : public rmf_task_sequence::Event static std::shared_ptr make( const AssignIDPtr& id, agv::RobotContextPtr context, - rmf_traffic::agv::Plan::Goal goal, - std::vector followed_by, + Description description, std::optional tail_period, rmf_task::events::SimpleEventStatePtr state, std::function update, @@ -106,16 +104,20 @@ class GoToPlace : public rmf_task_sequence::Event private: - Active(rmf_traffic::agv::Plan::Goal goal); + Active(Description description); void _schedule_retry(); + std::optional _choose_goal( + bool only_same_map) const; + void _find_plan(); void _execute_plan( rmf_traffic::PlanId plan_id, rmf_traffic::agv::Plan plan, - rmf_traffic::schedule::Itinerary full_itinerary); + rmf_traffic::schedule::Itinerary full_itinerary, + rmf_traffic::agv::Plan::Goal goal); void _stop_and_clear(); @@ -123,8 +125,8 @@ class GoToPlace : public rmf_task_sequence::Event const Negotiator::TableViewerPtr& table_view, const Negotiator::ResponderPtr& responder); - rmf_traffic::agv::Plan::Goal _goal; - std::vector _followed_by; + Description _description; + std::optional _chosen_goal; AssignIDPtr _assign_id; agv::RobotContextPtr _context; std::optional _tail_period; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp index 40f89104f..79c7db92a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ResponsiveWait.cpp @@ -319,8 +319,7 @@ void ResponsiveWait::Active::_begin_movement() _go_to_place = GoToPlace::Active::make( _assign_id, _context, - std::move(goal), - {}, + *GoToPlace::Description::make(goal), _description.period, _state, _update, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp index 75cefef9f..9c7e772d1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -50,6 +50,42 @@ void add_patrol( -> agv::DeserializedEvent { nlohmann::json place_msg; + const auto one_of = msg.find("one_of"); + if (one_of != msg.end()) + { + std::vector goals; + std::vector errors; + for (const auto& place_msg : one_of.value()) + { + auto place = place_deser(place_msg); + if (!place.description.has_value()) + { + return {nullptr, std::move(place.errors)}; + } + + goals.push_back(*place.description); + errors.insert( + errors.end(), + std::make_move_iterator(place.errors.begin()), + std::make_move_iterator(place.errors.end())); + } + + auto desc = GoToPlace::Description::make_for_one_of(goals); + const auto constraints = msg.find("constraints"); + if (constraints != msg.end()) + { + for (const auto& constraint : constraints.value()) + { + if (constraint["category"].get() == "prefer_same_map") + { + desc->prefer_same_map(true); + } + } + } + + return {desc, errors}; + } + const auto place_it = msg.find("place"); if (place_it == msg.end()) place_msg = msg;