From b3113fd2468991a91501ae9710cd9cfe77caaef5 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 9 Apr 2024 15:55:30 +0800 Subject: [PATCH 01/35] Lower debug level of some messages in rmf_websocket (#340) Signed-off-by: Arjo Chakravarty --- rmf_websocket/src/rmf_websocket/BroadcastClient.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp index b27627fc..01d3ac1b 100644 --- a/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp +++ b/rmf_websocket/src/rmf_websocket/BroadcastClient.cpp @@ -207,12 +207,12 @@ class BroadcastClient::Implementation } else { - RCLCPP_INFO( + RCLCPP_DEBUG( this->_node->get_logger(), "Sent successfully"); } _queue.pop_item(); } - RCLCPP_INFO( + RCLCPP_DEBUG( this->_node->get_logger(), "Emptied queue"); } // create pimpl From 0dad5022560661eb22153bc10863628988cf8810 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 17 Apr 2024 18:28:53 +0800 Subject: [PATCH 02/35] Manual release of mutex groups (#339) Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/StandardNames.hpp | 2 ++ .../agv/FleetUpdateHandle.cpp | 17 ++++++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 32 +++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 28 +++++++++++++++- 4 files changed, 78 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp index 32c1a016..52f95185 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp @@ -73,6 +73,8 @@ const std::string ChargingAssignmentsTopicName = "charging_assignments"; const std::string MutexGroupRequestTopicName = "mutex_group_request"; const std::string MutexGroupStatesTopicName = "mutex_group_states"; +const std::string MutexGroupManualReleaseTopicName = + "mutex_group_manual_release"; const uint64_t Unclaimed = (uint64_t)(-1); 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 ebad11c9..9bb9519a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1285,6 +1285,23 @@ void FleetUpdateHandle::Implementation::update_fleet_state() const commission.is_performing_idle_behavior(); json["commission"] = commission_json; + + nlohmann::json mutex_groups_json; + std::vector locked_mutex_groups; + for (const auto& g : context->locked_mutex_groups()) + { + locked_mutex_groups.push_back(g.first); + } + mutex_groups_json["locked"] = std::move(locked_mutex_groups); + + std::vector requesting_mutex_groups; + for (const auto& g : context->requesting_mutex_groups()) + { + requesting_mutex_groups.push_back(g.first); + } + mutex_groups_json["requesting"] = std::move(requesting_mutex_groups); + + json["mutex_groups"] = std::move(mutex_groups_json); } try diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 91d94eae..9a575ad5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1069,6 +1069,13 @@ RobotContext::locked_mutex_groups() const return _locked_mutex_groups; } +//============================================================================== +const std::unordered_map& +RobotContext::requesting_mutex_groups() const +{ + return _requesting_mutex_groups; +} + //============================================================================== const rxcpp::observable& RobotContext::request_mutex_groups( std::unordered_set groups, @@ -1495,6 +1502,7 @@ void RobotContext::_check_mutex_groups( } } +//============================================================================== void RobotContext::_retain_mutex_groups( const std::unordered_set& retain, std::unordered_map& groups) @@ -1596,5 +1604,29 @@ void RobotContext::_publish_mutex_group_requests() } } +//============================================================================== +void RobotContext::_handle_mutex_group_manual_release( + const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg) +{ + if (msg.fleet != group()) + return; + + if (msg.robot != name()) + return; + + std::unordered_set retain; + for (const auto& g : _locked_mutex_groups) + { + retain.insert(g.first); + } + + for (const auto& g : msg.release_mutex_groups) + { + retain.erase(g); + } + + retain_mutex_groups(retain); +} + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index 65d84e03..a96be33e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -23,6 +23,9 @@ #include #include #include +#include + +#include #include #include @@ -673,9 +676,14 @@ class RobotContext /// Check if a door is being held const std::optional& holding_door() const; - /// What mutex group is currently being locked. + /// What mutex groups are currently locked by this robot. const std::unordered_map& locked_mutex_groups() const; + /// What mutex groups are currently being requested (but have not yet been + /// locked) by this robot. + const std::unordered_map& + requesting_mutex_groups() const; + /// Set the mutex group that this robot needs to lock. const rxcpp::observable& request_mutex_groups( std::unordered_set groups, @@ -767,6 +775,20 @@ class RobotContext self->_publish_mutex_group_requests(); }); + context->_mutex_group_manual_release_sub = + context->_node->create_subscription< + rmf_fleet_msgs::msg::MutexGroupManualRelease>( + MutexGroupManualReleaseTopicName, + rclcpp::SystemDefaultsQoS() + .reliable() + .keep_last(10), + [w = context->weak_from_this()]( + rmf_fleet_msgs::msg::MutexGroupManualRelease::SharedPtr msg) + { + if (const auto self = w.lock()) + self->_handle_mutex_group_manual_release(*msg); + }); + return context; } @@ -872,12 +894,16 @@ class RobotContext std::unordered_map& _groups); void _release_mutex_group(const MutexGroupData& data) const; void _publish_mutex_group_requests(); + void _handle_mutex_group_manual_release( + const rmf_fleet_msgs::msg::MutexGroupManualRelease& msg); std::unordered_map _requesting_mutex_groups; std::unordered_map _locked_mutex_groups; rxcpp::subjects::subject _mutex_group_lock_subject; rxcpp::observable _mutex_group_lock_obs; rclcpp::TimerBase::SharedPtr _mutex_group_heartbeat; rmf_rxcpp::subscription_guard _mutex_group_sanity_check; + rclcpp::Subscription::SharedPtr + _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; }; From 6dfafa0dfad1ae197ba1c5e5344bc5c14a3a600e Mon Sep 17 00:00:00 2001 From: Xiyu Date: Thu, 18 Apr 2024 10:12:25 +0800 Subject: [PATCH 03/35] Disable automatic retreat (#330) * Add retreat_to_charger param Signed-off-by: Xiyu Oh * If idle task is charge, do not retreat to charger Signed-off-by: Xiyu Oh * Use duration instead of boolean for retreat to charger Signed-off-by: Xiyu Oh * Undo adding request type Signed-off-by: Xiyu Oh * Change retreat to timer from bool to optional duration Signed-off-by: Xiyu Oh * Store param in FleetUpdateHandle Signed-off-by: Xiyu Oh * Uncrustify and remove request_type from ChargeBattery Signed-off-by: Xiyu Oh * Tweak some details and remove API breakages Signed-off-by: Michael X. Grey * Fix style Signed-off-by: Michael X. Grey --------- Signed-off-by: Xiyu Oh Signed-off-by: Michael X. Grey Co-authored-by: Michael X. Grey Co-authored-by: Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 14 +++++ .../agv/FleetUpdateHandle.hpp | 10 ++++ .../src/rmf_fleet_adapter/TaskManager.cpp | 45 +++++++++++---- .../src/rmf_fleet_adapter/TaskManager.hpp | 6 ++ .../src/rmf_fleet_adapter/agv/Adapter.cpp | 3 + .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 56 +++++++++++++++++++ .../agv/FleetUpdateHandle.cpp | 22 ++++++++ .../agv/internal_FleetUpdateHandle.hpp | 2 + rmf_fleet_adapter_python/src/adapter.cpp | 7 +++ 9 files changed, 155 insertions(+), 10 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index 2be6c069..c6eb2cd3 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -513,6 +513,12 @@ class EasyFullControl::FleetConfiguration /// vehicles in this fleet when battery levels fall below the /// recharge_threshold. /// + /// \param[in] retreat_to_charger_interval + /// Specify whether to allow automatic retreat to charger if the robot's + /// battery is estimated to fall below its recharge_threshold before it is + /// able to complete its current task. Provide a duration between checks in + /// seconds. If nullopt, retreat to charger would be disabled. + /// /// \param[in] task_categories /// Provide callbacks for considering tasks belonging to each category. /// @@ -706,6 +712,14 @@ class EasyFullControl::FleetConfiguration /// Set whether or not to account for battery drain during task planning. void set_account_for_battery_drain(bool value); + /// Get the duration between retreat to charger checks. + std::optional retreat_to_charger_interval() const; + + /// Set the duration between retreat to charger checks. Passing in a nullopt + /// will turn off these checks entirely. + void set_retreat_to_charger_interval( + std::optional value); + /// Get the task categories const std::unordered_map& task_consideration() const; diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 290f5bd3..4c536bd9 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -391,6 +391,16 @@ class FleetUpdateHandle : public std::enable_shared_from_this FleetUpdateHandle& set_update_listener( std::function listener); + /// Get the duration between retreat to charger checks. + std::optional retreat_to_charger_interval() const; + + /// Specify whether to allow automatic retreat to charger if the robot's + /// battery is estimated to fall below its recharge_threshold before it is + /// able to complete its current task. Provide a duration between checks in + /// seconds. If nullopt, retreat to charger would be disabled. + FleetUpdateHandle& set_retreat_to_charger_interval( + std::optional duration); + /// Get the rclcpp::Node that this fleet update handle will be using for /// communication. std::shared_ptr node(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 78a50f5c..1f4c71b2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -162,16 +162,6 @@ TaskManagerPtr TaskManager::make( } }); - mgr->_retreat_timer = mgr->context()->node()->try_create_wall_timer( - std::chrono::seconds(10), - [w = mgr->weak_from_this()]() - { - if (auto mgr = w.lock()) - { - mgr->retreat_to_charger(); - } - }); - mgr->_begin_waiting(); // TODO(MXG): The tests allow a task manager to be created before a task @@ -1752,6 +1742,41 @@ std::function TaskManager::_make_resume_from_waiting() }; } +//============================================================================== +void TaskManager::configure_retreat_to_charger( + std::optional duration) +{ + if (duration.has_value() && *duration <= rmf_traffic::Duration(0)) + { + RCLCPP_ERROR( + _context->node()->get_logger(), + "[TaskManager::configure_retreat_to_charger] " + "Invalid value for duration: %f", + rmf_traffic::time::to_seconds(*duration)); + } + + if (!duration.has_value() || *duration <= rmf_traffic::Duration(0)) + { + if (_retreat_timer && !_retreat_timer->is_canceled()) + { + _retreat_timer->cancel(); + } + return; + } + + if (_retreat_timer) + _retreat_timer->reset(); + _retreat_timer = _context->node()->try_create_wall_timer( + duration.value(), + [w = weak_from_this()]() + { + if (auto mgr = w.lock()) + { + mgr->retreat_to_charger(); + } + }); +} + //============================================================================== void TaskManager::retreat_to_charger() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 7a409a7e..e2b2c87f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -156,6 +156,12 @@ class TaskManager : public std::enable_shared_from_this /// when robot is idle and battery level drops below a retreat threshold. void retreat_to_charger(); + /// Start the retreat timer that periodically checks whether the robot + /// should retreat to charger if its battery state of charge is close to + /// the recharge threshold. + void configure_retreat_to_charger( + std::optional duration); + /// Get the list of task ids for tasks that have started execution. /// The list will contain upto 100 latest task ids only. const std::vector& get_executed_tasks() const; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 2bb57739..9c1e4d5d 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -317,6 +317,9 @@ std::shared_ptr Adapter::add_easy_fleet( config.fleet_name().c_str()); } + fleet_handle->set_retreat_to_charger_interval( + config.retreat_to_charger_interval()); + for (const auto& [task, consider] : config.task_consideration()) { if (task == "delivery" && consider) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index 780deab8..b44238d4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -1689,6 +1689,7 @@ class EasyFullControl::FleetConfiguration::Implementation double recharge_threshold; double recharge_soc; bool account_for_battery_drain; + std::optional retreat_to_charger_interval; std::unordered_map task_consideration; std::unordered_map action_consideration; rmf_task::ConstRequestFactoryPtr finishing_request; @@ -1744,6 +1745,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(recharge_threshold), std::move(recharge_soc), std::move(account_for_battery_drain), + std::chrono::seconds(10), std::move(task_consideration), std::move(action_consideration), std::move(finishing_request), @@ -1971,6 +1973,45 @@ EasyFullControl::FleetConfiguration::from_config_files( { recharge_soc = rmf_fleet["recharge_soc"].as(); } + // Retreat to charger + std::optional retreat_to_charger_interval = + rmf_traffic::time::from_seconds(10); + const auto retreat_to_charger_yaml = rmf_fleet["retreat_to_charger_interval"]; + if (!retreat_to_charger_yaml) + { + std::cout << "[retreat_to_charger_interval] value is not provided, " + << "default to 10 seconds" << std::endl; + } + else if (retreat_to_charger_yaml.IsScalar()) + { + const double retreat_to_charger_interval_sec = + retreat_to_charger_yaml.as(); + if (retreat_to_charger_interval_sec <= 0.0) + { + std::cout + << "[retreat_to_charger_interval] has invalid value [" + << retreat_to_charger_interval_sec << "]. Turning off retreat to " + << "behavior." << std::endl; + retreat_to_charger_interval = std::nullopt; + } + else + { + retreat_to_charger_interval = + rmf_traffic::time::from_seconds(retreat_to_charger_interval_sec); + } + } + else if (retreat_to_charger_yaml.IsNull()) + { + retreat_to_charger_interval = std::nullopt; + } + else + { + const auto mark = retreat_to_charger_yaml.Mark(); + std::cout << "[retreat_to_charger_interval] Unsupported value type " + << "provided: line " << mark.line << ", column " << mark.column + << std::endl; + return std::nullopt; + } // Task capabilities std::unordered_map task_consideration; @@ -2322,6 +2363,7 @@ EasyFullControl::FleetConfiguration::from_config_files( default_max_merge_lane_distance, default_min_lane_length); config.change_lift_emergency_levels() = lift_emergency_levels; + config.set_retreat_to_charger_interval(retreat_to_charger_interval); return config; } @@ -2519,6 +2561,20 @@ void EasyFullControl::FleetConfiguration::set_account_for_battery_drain( _pimpl->account_for_battery_drain = value; } +//============================================================================== +std::optional +EasyFullControl::FleetConfiguration::retreat_to_charger_interval() const +{ + return _pimpl->retreat_to_charger_interval; +} + +//============================================================================== +void EasyFullControl::FleetConfiguration::set_retreat_to_charger_interval( + std::optional value) +{ + _pimpl->retreat_to_charger_interval = value; +} + //============================================================================== const std::unordered_map& EasyFullControl::FleetConfiguration::task_consideration() const diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 9bb9519a..28c8ddf5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -1911,6 +1911,7 @@ void FleetUpdateHandle::add_robot( } mgr->set_idle_task(fleet->_pimpl->idle_task); + mgr->configure_retreat_to_charger(fleet->retreat_to_charger_interval()); // -- Calling the handle_cb should always happen last -- if (handle_cb) @@ -2516,6 +2517,27 @@ FleetUpdateHandle& FleetUpdateHandle::set_update_listener( return *this; } +//============================================================================== +std::optional +FleetUpdateHandle::retreat_to_charger_interval() const +{ + return _pimpl->retreat_to_charger_interval; +} + +//============================================================================== +FleetUpdateHandle& FleetUpdateHandle::set_retreat_to_charger_interval( + std::optional duration) +{ + _pimpl->retreat_to_charger_interval = duration; + + // Start retreat timer + for (const auto& t : _pimpl->task_managers) + { + t.second->configure_retreat_to_charger(duration); + } + return *this; +} + //============================================================================== std::shared_ptr FleetUpdateHandle::node() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp index 2e218e88..7b580044 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_FleetUpdateHandle.hpp @@ -288,6 +288,8 @@ class FleetUpdateHandle::Implementation rmf_utils::optional default_maximum_delay = std::chrono::nanoseconds(std::chrono::seconds(10)); + rmf_utils::optional retreat_to_charger_interval = + std::chrono::nanoseconds(std::chrono::seconds(10)); AcceptDeliveryRequest accept_delivery = nullptr; std::unordered_map Date: Tue, 23 Apr 2024 10:06:29 +0800 Subject: [PATCH 04/35] Fix deadlock in websocket server (#342) This is a rather dumb mistake. I think I left this in as a vestige of my testing. However, the websocket client was refusing to reconnect thanks to a deadlock caused by this variable. Signed-off-by: Arjo Chakravarty --- .../client/ClientWebSocketEndpoint.cpp | 38 ++++++++----------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp index 65df1aa1..99a823ba 100644 --- a/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp +++ b/rmf_websocket/src/rmf_websocket/client/ClientWebSocketEndpoint.cpp @@ -125,7 +125,6 @@ websocketpp::lib::error_code ClientWebSocketEndpoint::connect() { websocketpp::lib::error_code ec; - _init = true; _con = _endpoint->get_connection(_uri, ec); RCLCPP_INFO(_node->get_logger(), "Attempting to connect to %s", _uri.c_str()); @@ -140,30 +139,25 @@ websocketpp::lib::error_code ClientWebSocketEndpoint::connect() auto reconnect_socket = [this]() { // TODO(arjo) Parametrize the timeout. - using namespace std::chrono_literals; - if (!_reconnect_enqueued) - { - RCLCPP_ERROR(_node->get_logger(), - "Connection lost\n" - "> Reconnecting in 1s\n" - "> Host: %s", _uri.c_str()); - _endpoint->stop_perpetual(); - auto io_service = &_endpoint->get_io_service(); - _endpoint = std::make_unique(); - _endpoint->clear_access_channels(websocketpp::log::alevel::all); - _endpoint->clear_error_channels(websocketpp::log::elevel::all); - _endpoint->init_asio(io_service); - _endpoint->start_perpetual(); - websocketpp::lib::error_code ec; - - _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, - this)); - _reconnect_enqueued = true; - } + RCLCPP_ERROR(_node->get_logger(), + "Connection lost\n" + "> Reconnecting in 1s\n" + "> Host: %s", _uri.c_str()); + _endpoint->stop_perpetual(); + auto io_service = &_endpoint->get_io_service(); + _endpoint = std::make_unique(); + _endpoint->clear_access_channels(websocketpp::log::alevel::all); + _endpoint->clear_error_channels(websocketpp::log::elevel::all); + _endpoint->init_asio(io_service); + _endpoint->start_perpetual(); + websocketpp::lib::error_code ec; + + _endpoint->set_timer(1000, std::bind(&ClientWebSocketEndpoint::connect, + this)); + }; auto connected_cb = [this]() { - _reconnect_enqueued = false; _connection_cb(); }; From 132e516d5c7e55486cd30ba422ee09a1e648fc1e Mon Sep 17 00:00:00 2001 From: Grey Date: Thu, 25 Apr 2024 15:13:35 +0800 Subject: [PATCH 05/35] Automatically begin or cancel idle behavior when commission changes (#346) Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/TaskManager.cpp | 23 +++++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 13 ++++++++--- .../rmf_fleet_adapter/agv/RobotContext.cpp | 22 ++++++++++++++++-- 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 1f4c71b2..94723088 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -1356,6 +1356,15 @@ bool TaskManager::kill_task( return false; } +//============================================================================== +void TaskManager::_cancel_idle_behavior(std::vector labels) +{ + if (_waiting) + { + _waiting.cancel(std::move(labels), _context->now()); + } +} + //============================================================================== void TaskManager::_begin_next_task() { @@ -1373,7 +1382,6 @@ void TaskManager::_begin_next_task() if (_queue.empty() && _direct_queue.empty()) { - if (!_waiting && !_finished_waiting) { _begin_waiting(); @@ -1470,7 +1478,9 @@ void TaskManager::_begin_next_task() else { if (!_waiting && !_finished_waiting) + { _begin_waiting(); + } } _context->worker().schedule( @@ -1730,7 +1740,16 @@ std::function TaskManager::_make_resume_from_waiting() if (!self) return; - self->_finished_waiting = true; + // This condition deals with an awkward edge case where idle behavior + // would not restart when toggling the idle behavior commission back + // on. We fix this by keeping the _finished_waiting flag clean if + // idle behavior commissioning is off, so there's nothing to block + // idle behavior from beginning again if it gets toggled back on. + if (self->_context->commission().is_performing_idle_behavior()) + { + self->_finished_waiting = true; + } + self->_waiting = ActiveTask(); self->_begin_next_task(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index e2b2c87f..549412cd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -233,6 +233,16 @@ class TaskManager : public std::enable_shared_from_this const std::string& task_id, std::vector labels); + /// This should only be triggered by RobotContext::set_commission(~), and only + /// in scenarios where the idle behavior commission has been toggled off. + void _cancel_idle_behavior(std::vector labels); + + /// Begin the next task for this robot if there is a new task ready to perform + /// and the robot is not already performing a task or caught in an emergency or + /// interruption. If no task is being performed and no new task is ready, the + /// idle behavior will be triggered. + void _begin_next_task(); + private: TaskManager( @@ -408,9 +418,6 @@ class TaskManager : public std::enable_shared_from_this // Map task_id to task_log.json for all tasks managed by this TaskManager std::unordered_map _task_logs = {}; - /// Callback for task timer which begins next task if its deployment time has passed - void _begin_next_task(); - /// Begin performing an emergency pullover. This should only be called when an /// emergency is active. void _begin_pullover(); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 9a575ad5..b1df13f1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -16,6 +16,7 @@ */ #include "internal_RobotUpdateHandle.hpp" +#include "../TaskManager.hpp" #include @@ -963,8 +964,25 @@ std::shared_ptr RobotContext::task_manager() //============================================================================== void RobotContext::set_commission(RobotUpdateHandle::Commission value) { - std::lock_guard lock(*_commission_mutex); - _commission = std::move(value); + { + std::lock_guard lock(*_commission_mutex); + _commission = std::move(value); + } + + if (const auto mgr = _task_manager.lock()) + { + if (!_commission.is_performing_idle_behavior()) + { + mgr->_cancel_idle_behavior({"decommissioned"}); + } + else + { + // We trigger this in case the robot needs to begin its idle behavior. + // If it shouldn't perform idle behavior for any reason (e.g. already + // performing a task), then this will have no effect. + mgr->_begin_next_task(); + } + } } //============================================================================== From 915f9c5a9f32b407bc7c5bc54ccd07163f529c2a Mon Sep 17 00:00:00 2001 From: Xiyu Date: Mon, 6 May 2024 20:20:30 +0800 Subject: [PATCH 06/35] Add fleet-level reassign dispatched tasks API (#348) Signed-off-by: Xiyu Oh --- .../rmf_fleet_adapter/agv/FleetUpdateHandle.hpp | 3 +++ .../rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 14 ++++++++++++++ rmf_fleet_adapter_python/src/adapter.cpp | 4 +++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp index 4c536bd9..9a4c4d9d 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp @@ -401,6 +401,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this FleetUpdateHandle& set_retreat_to_charger_interval( std::optional duration); + /// Trigger a replan for task allocation for robots in this fleet. + void reassign_dispatched_tasks(); + /// Get the rclcpp::Node that this fleet update handle will be using for /// communication. std::shared_ptr node(); 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 28c8ddf5..cd9d3a2c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -2538,6 +2538,20 @@ FleetUpdateHandle& FleetUpdateHandle::set_retreat_to_charger_interval( return *this; } +//============================================================================== +void FleetUpdateHandle::reassign_dispatched_tasks() +{ + _pimpl->worker.schedule( + [w = weak_from_this()](const auto&) + { + const auto self = w.lock(); + if (!self) + return; + self->_pimpl->reassign_dispatched_tasks([]() {}, [](auto) {}); + } + ); +} + //============================================================================== std::shared_ptr FleetUpdateHandle::node() { diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index 512710f4..fa610273 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -596,7 +596,9 @@ PYBIND11_MODULE(rmf_adapter, m) { ); }, py::arg("category"), - py::arg("consider")); + py::arg("consider")) + .def("reassign_dispatched_tasks", + &agv::FleetUpdateHandle::reassign_dispatched_tasks); // TASK REQUEST CONFIRMATION =============================================== auto m_fleet_update_handle = m.def_submodule("fleet_update_handle"); From e7b0f8e32bfa353f1ac5258c8ad05ae6bef932f1 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 8 May 2024 01:28:10 +0800 Subject: [PATCH 07/35] Fix schema dictionary used during robot status override (#349) Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.cpp | 66 +++++++++++-------- .../agv/internal_RobotUpdateHandle.hpp | 15 +---- 2 files changed, 41 insertions(+), 40 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 68a46eca..dba26ffa 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -279,6 +279,44 @@ void RobotUpdateHandle::update_battery_soc(const double battery_soc) } } +//============================================================================== +std::function +make_schema_loader(const rclcpp::Node::SharedPtr& node) +{ + // Initialize schema dictionary + const std::vector schemas = { + rmf_api_msgs::schemas::robot_state, + rmf_api_msgs::schemas::location_2D, + rmf_api_msgs::schemas::commission, + }; + + std::unordered_map schema_dictionary; + + for (const auto& schema : schemas) + { + const auto json_uri = nlohmann::json_uri{schema["$id"]}; + schema_dictionary.insert({json_uri.url(), schema}); + } + + return [schema_dictionary = std::move(schema_dictionary), node]( + const nlohmann::json_uri& id, + nlohmann::json& value) + { + const auto it = schema_dictionary.find(id.url()); + if (it == schema_dictionary.end()) + { + RCLCPP_ERROR( + node->get_logger(), + "url: %s not found in schema dictionary. " + "Status for robot will not be overwritten.", + id.url().c_str()); + return; + } + + value = it->second; + }; +} + //============================================================================== void RobotUpdateHandle::override_status(std::optional status) { @@ -286,40 +324,16 @@ void RobotUpdateHandle::override_status(std::optional status) { if (status.has_value()) { - // Here we capture [this] to avoid potential costly copy of - // schema_dictionary when more enties are inserted in the future. - // It is permissible here since the lambda will only be used within the - // scope of this function. - const auto loader = - [context, this]( - const nlohmann::json_uri& id, - nlohmann::json& value) - { - const auto it = _pimpl->schema_dictionary.find(id.url()); - if (it == _pimpl->schema_dictionary.end()) - { - RCLCPP_ERROR( - context->node()->get_logger(), - "url: %s not found in schema dictionary. " - "Status for robot [%s] will not be overwritten.", - id.url().c_str(), - context->name().c_str()); - return; - } - - value = it->second; - }; - try { static const auto validator = nlohmann::json_schema::json_validator( - rmf_api_msgs::schemas::robot_state, loader); + rmf_api_msgs::schemas::robot_state, + make_schema_loader(context->node())); nlohmann::json dummy_msg; dummy_msg["status"] = status.value(); validator.validate(dummy_msg); - } catch (const std::exception& e) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 7e6bc133..5d3f7886 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include @@ -261,8 +262,6 @@ class RobotUpdateHandle::Implementation std::string name; RobotUpdateHandle::Unstable unstable = RobotUpdateHandle::Unstable(); bool reported_loss = false; - std::unordered_map schema_dictionary = {}; - static std::shared_ptr make(RobotContextPtr context) { @@ -274,18 +273,6 @@ class RobotUpdateHandle::Implementation handle._pimpl->unstable._pimpl = &RobotUpdateHandle::Implementation::get(handle); - // Initialize schema dictionary - const std::vector schemas = { - rmf_api_msgs::schemas::robot_state, - rmf_api_msgs::schemas::location_2D, - }; - - for (const auto& schema : schemas) - { - const auto json_uri = nlohmann::json_uri{schema["$id"]}; - handle._pimpl->schema_dictionary.insert({json_uri.url(), schema}); - } - return std::make_shared(std::move(handle)); } From 2d35a199ff5a02f8ef88c5211900ab4708a00e57 Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 10 May 2024 18:38:18 +0800 Subject: [PATCH 08/35] Filter DoorOpen insertion by map name (#353) Signed-off-by: Michael X. Grey --- .../src/rmf_fleet_adapter/events/ExecutePlan.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index c13982cc..8e24390f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -739,13 +739,18 @@ std::optional ExecutePlan::make( if (first_graph_wp.has_value()) { - const Eigen::Vector2d p1 = - graph.get_waypoint(*first_graph_wp).get_location(); + const auto& wp = graph.get_waypoint(*first_graph_wp); + const Eigen::Vector2d p1 = wp.get_location(); + const auto& map = wp.get_map_name(); // Check if the line from the start of the plan to this waypoint crosses // through a door, and add a DoorOpen phase if it does for (const auto& door : graph.all_known_doors()) { + if (door->map() != map) + { + continue; + } if (door->intersects(p0, p1, envelope)) { @@ -762,7 +767,6 @@ std::optional ExecutePlan::make( } } - const auto& map = graph.get_waypoint(*first_graph_wp).get_map_name(); // Check if the robot is going into a lift and summon the lift for (const auto& lift : graph.all_known_lifts()) { From 5e58af95b399724eb5ab58fdc27a64f6e26372b3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Sat, 11 May 2024 02:39:10 +0800 Subject: [PATCH 09/35] Event based lift / door logic (#320) Signed-off-by: Luca Della Vedova --- rmf_fleet_adapter/src/door_supervisor/Node.cpp | 3 ++- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 10 +++++++--- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 6 ++++++ 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp index 5b5f149a..83abfc92 100644 --- a/rmf_fleet_adapter/src/door_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/door_supervisor/Node.cpp @@ -28,7 +28,8 @@ const std::string DoorSupervisorRequesterID = "door_supervisor"; Node::Node() : rclcpp::Node("door_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto default_qos = + rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); _door_request_pub = create_publisher( FinalDoorRequestTopicName, default_qos); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index e1bb36e6..de59217a 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,7 +27,8 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); + const auto default_qos = + rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); const auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); @@ -68,12 +69,14 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) { if (curr_request->session_id == msg->session_id) { + msg->request_time = this->now(); + _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) + { curr_request = std::move(msg); + } else { - msg->request_time = this->now(); - _lift_request_pub->publish(*msg); RCLCPP_INFO( this->get_logger(), "[%s] Published end lift session from lift supervisor", @@ -85,6 +88,7 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) } else { + _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) { curr_request = std::move(msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index b1df13f1..6503edc2 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1067,6 +1067,12 @@ void RobotContext::release_lift() "Releasing lift [%s] for [%s]", _lift_destination->lift_name.c_str(), requester_id().c_str()); + rmf_lift_msgs::msg::LiftRequest msg; + msg.lift_name = _lift_destination->lift_name; + msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; + msg.session_id = requester_id(); + msg.destination_floor = _lift_destination->destination_floor; + _node->lift_request()->publish(msg); } _lift_destination = nullptr; _initial_time_idle_outside_lift = std::nullopt; From 0f60c410b64f6e0a505b596c3b9bde1b09351b35 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 13 May 2024 20:16:11 +0800 Subject: [PATCH 10/35] dispatcher to only publish on websocket if the dispatch failed Signed-off-by: Aaron Chong --- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index cfb2d3ec..c64b53fc 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -612,8 +612,7 @@ class Dispatcher::Implementation bid_notice.task_id, std::chrono::steady_clock::now()); new_dispatch_state->request = request; - // Publish this initial task state message to the websocket - auto state = publish_task_state_ws(new_dispatch_state, "queued"); + const auto state = create_task_state_json(new_dispatch_state, "queued"); active_dispatch_states[bid_notice.task_id] = new_dispatch_state; @@ -796,7 +795,10 @@ class Dispatcher::Implementation } /// Publish failed bid - publish_task_state_ws(dispatch_state, "failed"); + const auto task_state_update = + create_task_state_json(dispatch_state, "failed"); + if (broadcast_client) + broadcast_client->publish(task_state_update); auctioneer->ready_for_next_bid(); return; @@ -829,7 +831,7 @@ class Dispatcher::Implementation } //============================================================================== - nlohmann::json publish_task_state_ws( + nlohmann::json create_task_state_json( const std::shared_ptr state, const std::string& status) { @@ -873,12 +875,11 @@ class Dispatcher::Implementation task_state_update["data"] = task_state; /// TODO: (YL) json validator for taskstateupdate - - if (broadcast_client) - broadcast_client->publish(task_state_update); return task_state; } + + //============================================================================== void move_to_finished(const std::string& task_id) { const auto active_it = active_dispatch_states.find(task_id); From 7f21225056e51ba25a7dcdf7be7772119e8c746f Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 13 May 2024 21:27:29 +0800 Subject: [PATCH 11/35] Fix mistake on task_state_update json Signed-off-by: Aaron Chong --- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index c64b53fc..036289e5 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -795,10 +795,14 @@ class Dispatcher::Implementation } /// Publish failed bid - const auto task_state_update = + const auto task_state = create_task_state_json(dispatch_state, "failed"); if (broadcast_client) + { + auto task_state_update = _task_state_update_json; + task_state_update["data"] = task_state; broadcast_client->publish(task_state_update); + } auctioneer->ready_for_next_bid(); return; @@ -871,9 +875,6 @@ class Dispatcher::Implementation dispatch_json["errors"] = state->errors; task_state["dispatch"] = dispatch_json; - auto task_state_update = _task_state_update_json; - task_state_update["data"] = task_state; - /// TODO: (YL) json validator for taskstateupdate return task_state; } From f3ecf8c573c096325bab6e23d0e808444fb315cb Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 17 May 2024 17:26:38 +0800 Subject: [PATCH 12/35] Only create task_state if dispatcher has web socket connection Signed-off-by: Aaron Chong --- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index 036289e5..b4beb89c 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -795,10 +795,10 @@ class Dispatcher::Implementation } /// Publish failed bid - const auto task_state = - create_task_state_json(dispatch_state, "failed"); if (broadcast_client) { + const auto task_state = + create_task_state_json(dispatch_state, "failed"); auto task_state_update = _task_state_update_json; task_state_update["data"] = task_state; broadcast_client->publish(task_state_update); From df95bc5a4bc57acc859975f10953f5a1c0ffa434 Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 7 Jun 2024 15:17:24 +0800 Subject: [PATCH 13/35] Fix serialization of exit events (#364) Signed-off-by: Michael X. Grey --- rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp index 7aec4ddb..784bc075 100644 --- a/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp +++ b/rmf_traffic_ros2/src/rmf_traffic_ros2/convert_Graph.cpp @@ -906,7 +906,7 @@ std::unique_ptr convert( if (exit_event != nullptr) { EventPhaseFactory factory("exit", params); - entry_event->execute(factory); + exit_event->execute(factory); } const auto* exit_orientation = lane.entry().orientation_constraint(); if (exit_orientation != nullptr) From a5d411aeacd9c4a05223b590bfee351793c56384 Mon Sep 17 00:00:00 2001 From: Yadu Date: Fri, 7 Jun 2024 19:18:15 -0700 Subject: [PATCH 14/35] Fix race condition for ingesting/dispensing and disable uncrustify tests by default (#362) * Disable uncrustify tests by default Signed-off-by: Yadunund * Try to increase timeout Signed-off-by: Luca Della Vedova * Revert "Try to increase timeout" This reverts commit 9104594af99c94d9cdab57696e7ceaf430514bc5. Signed-off-by: Luca Della Vedova * Fix failing test in rolling Signed-off-by: Luca Della Vedova * Fix race condition for ingesting and dispensing Signed-off-by: Michael X. Grey --------- Signed-off-by: Yadunund Signed-off-by: Luca Della Vedova Signed-off-by: Michael X. Grey Co-authored-by: Luca Della Vedova Co-authored-by: Michael X. Grey --- rmf_fleet_adapter/CMakeLists.txt | 4 ++- .../rmf_fleet_adapter/phases/DispenseItem.cpp | 10 ++++--- .../rmf_fleet_adapter/phases/IngestItem.cpp | 10 ++++--- .../src/rmf_fleet_adapter/phases/Utils.cpp | 2 +- rmf_task_ros2/CMakeLists.txt | 26 +++++++++--------- rmf_task_ros2/test/bidding/test_SelfBid.cpp | 11 ++++---- rmf_traffic_ros2/CMakeLists.txt | 27 +++++++++++-------- rmf_websocket/CMakeLists.txt | 26 ++++++++++-------- 8 files changed, 66 insertions(+), 50 deletions(-) diff --git a/rmf_fleet_adapter/CMakeLists.txt b/rmf_fleet_adapter/CMakeLists.txt index 7ae87b9f..ef83cb60 100644 --- a/rmf_fleet_adapter/CMakeLists.txt +++ b/rmf_fleet_adapter/CMakeLists.txt @@ -49,7 +49,9 @@ foreach(pkg ${dep_pkgs}) find_package(${pkg} REQUIRED) endforeach() -if(BUILD_TESTING) +# Disable uncrustify tests by default. +set(TEST_UNCRUSTIFY "Off") +if(BUILD_TESTING AND TEST_UNCRUSTIFY) find_package(ament_cmake_uncrustify REQUIRED) find_file(uncrustify_config_file NAMES "rmf_code_style.cfg" diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp index 8cebd1aa..c4ffeafd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/DispenseItem.cpp @@ -160,11 +160,13 @@ LegacyTask::StatusMsg DispenseItem::ActivePhase::_get_status( status.state = LegacyTask::StatusMsg::STATE_ACTIVE; using rmf_dispenser_msgs::msg::DispenserResult; - if (dispenser_result - && dispenser_result->request_guid == _request_guid - && is_newer(dispenser_result->time, _last_msg)) + if (dispenser_result && dispenser_result->request_guid == _request_guid) { - _last_msg = dispenser_result->time; + if (is_newer(dispenser_result->time, _last_msg)) + { + _last_msg = dispenser_result->time; + } + switch (dispenser_result->status) { case DispenserResult::ACKNOWLEDGED: diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp index 3db97ec6..3f3c643a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/IngestItem.cpp @@ -159,11 +159,13 @@ LegacyTask::StatusMsg IngestItem::ActivePhase::_get_status( status.state = LegacyTask::StatusMsg::STATE_ACTIVE; using rmf_ingestor_msgs::msg::IngestorResult; - if (ingestor_result - && ingestor_result->request_guid == _request_guid - && is_newer(ingestor_result->time, _last_msg)) + if (ingestor_result && ingestor_result->request_guid == _request_guid) { - _last_msg = ingestor_result->time; + if (is_newer(ingestor_result->time, _last_msg)) + { + _last_msg = ingestor_result->time; + } + switch (ingestor_result->status) { case IngestorResult::ACKNOWLEDGED: diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/Utils.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/Utils.cpp index 46c1c22b..cfac699e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/Utils.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/Utils.cpp @@ -23,7 +23,7 @@ namespace phases { bool is_newer(const builtin_interfaces::msg::Time& a, const builtin_interfaces::msg::Time& b) { - return a.sec > b.sec || (a.sec == b.sec && a.nanosec > b.nanosec); + return a.sec > b.sec || (a.sec == b.sec && a.nanosec >= b.nanosec); } } // namespace phases diff --git a/rmf_task_ros2/CMakeLists.txt b/rmf_task_ros2/CMakeLists.txt index 1ded00bd..4f9574cf 100644 --- a/rmf_task_ros2/CMakeLists.txt +++ b/rmf_task_ros2/CMakeLists.txt @@ -55,20 +55,22 @@ ament_export_targets(rmf_task_ros2 HAS_LIBRARY_TARGET) ament_export_dependencies(rmf_traffic rmf_task_msgs rclcpp nlohmann_json) #=============================================================================== - +set(TEST_UNCRUSTIFY "Off") if(BUILD_TESTING) find_package(ament_cmake_catch2 QUIET) - find_package(ament_cmake_uncrustify REQUIRED) - find_file(uncrustify_config_file - NAMES "rmf_code_style.cfg" - PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - - ament_uncrustify( - ARGN include src test - CONFIG_FILE ${uncrustify_config_file} - LANGUAGE C++ - MAX_LINE_LENGTH 80 - ) + if (TEST_UNCRUSTIFY) + find_package(ament_cmake_uncrustify REQUIRED) + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + ament_uncrustify( + ARGN include src test + CONFIG_FILE ${uncrustify_config_file} + LANGUAGE C++ + MAX_LINE_LENGTH 80 + ) + endif() file(GLOB_RECURSE unit_test_srcs "test/*.cpp") diff --git a/rmf_task_ros2/test/bidding/test_SelfBid.cpp b/rmf_task_ros2/test/bidding/test_SelfBid.cpp index c944dfd8..6a9ed3f0 100644 --- a/rmf_task_ros2/test/bidding/test_SelfBid.cpp +++ b/rmf_task_ros2/test/bidding/test_SelfBid.cpp @@ -90,16 +90,16 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") exec_options.context = rcl_context; rclcpp::executors::SingleThreadedExecutor executor(exec_options); executor.add_node(node); + const auto now = std::chrono::steady_clock::now(); auto bidder1 = AsyncBidder::make( node, - [&test_notice_bidder1](const auto& notice, auto respond) + [&test_notice_bidder1, &now](const auto& notice, auto respond) { Response::Proposal best_robot_estimate; test_notice_bidder1 = notice.request; best_robot_estimate.fleet_name = "bidder1"; - best_robot_estimate.finish_time = - std::chrono::steady_clock::time_point::max(); + best_robot_estimate.finish_time = now + std::chrono::seconds(1000); respond(Response{best_robot_estimate, {}}); } @@ -107,7 +107,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") auto bidder2 = AsyncBidder::make( node, - [&test_notice_bidder2](const auto& notice, auto respond) + [&test_notice_bidder2, &now](const auto& notice, auto respond) { auto request = nlohmann::json::parse(notice.request); if (request["category"] == "patrol") @@ -117,8 +117,7 @@ SCENARIO("Auction with 2 Bids", "[TwoBids]") Response::Proposal best_robot_estimate; best_robot_estimate.new_cost = 2.3; // lower cost than bidder1 best_robot_estimate.fleet_name = "bidder2"; - best_robot_estimate.finish_time = - std::chrono::steady_clock::time_point::min(); + best_robot_estimate.finish_time = now + std::chrono::seconds(1); test_notice_bidder2 = notice.request; respond(Response{best_robot_estimate, {}}); diff --git a/rmf_traffic_ros2/CMakeLists.txt b/rmf_traffic_ros2/CMakeLists.txt index 4bea2220..baa12325 100644 --- a/rmf_traffic_ros2/CMakeLists.txt +++ b/rmf_traffic_ros2/CMakeLists.txt @@ -41,19 +41,24 @@ if (rmf_traffic_FOUND) message(STATUS "rmf_traffic_INCLUDE_DIRS: ${rmf_traffic_INCLUDE_DIRS}") endif() +# Disable uncrustify tests by default. +set(TEST_UNCRUSTIFY "Off") if(BUILD_TESTING) find_package(ament_cmake_catch2 QUIET) - find_package(ament_cmake_uncrustify REQUIRED) - find_file(uncrustify_config_file - NAMES "rmf_code_style.cfg" - PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - - ament_uncrustify( - ARGN include src examples - CONFIG_FILE ${uncrustify_config_file} - LANGUAGE C++ - MAX_LINE_LENGTH 80 - ) + + if(TEST_UNCRUSTIFY) + find_package(ament_cmake_uncrustify REQUIRED) + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + ament_uncrustify( + ARGN include src examples + CONFIG_FILE ${uncrustify_config_file} + LANGUAGE C++ + MAX_LINE_LENGTH 80 + ) + endif() file(GLOB_RECURSE unit_test_srcs "test/unit/*.cpp") diff --git a/rmf_websocket/CMakeLists.txt b/rmf_websocket/CMakeLists.txt index 685c553c..6c6745a2 100644 --- a/rmf_websocket/CMakeLists.txt +++ b/rmf_websocket/CMakeLists.txt @@ -88,18 +88,22 @@ install( ARCHIVE DESTINATION lib ) +# Disable uncrustify tests by default. +set(TEST_UNCRUSTIFY "Off") if(BUILD_TESTING) - find_package(ament_cmake_uncrustify REQUIRED) - find_file(uncrustify_config_file - NAMES "rmf_code_style.cfg" - PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") - - ament_uncrustify( - include src - CONFIG_FILE ${uncrustify_config_file} - LANGUAGE C++ - MAX_LINE_LENGTH 80 - ) + if(TEST_UNCRUSTIFY) + find_package(ament_cmake_uncrustify REQUIRED) + find_file(uncrustify_config_file + NAMES "rmf_code_style.cfg" + PATHS "${rmf_utils_DIR}/../../../share/rmf_utils/") + + ament_uncrustify( + include src + CONFIG_FILE ${uncrustify_config_file} + LANGUAGE C++ + MAX_LINE_LENGTH 80 + ) + endif() # unit test find_package(ament_cmake_catch2 REQUIRED) From a01fc2df430902774b448e1f0a216a2d2e121740 Mon Sep 17 00:00:00 2001 From: Yadu Date: Sat, 8 Jun 2024 16:15:26 -0700 Subject: [PATCH 15/35] Bump main to 2.7.0 (#359) * Update changelogs Signed-off-by: Yadunund * 2.7.0 Signed-off-by: Yadunund * Update changelogs again Signed-off-by: Yadunund --------- Signed-off-by: Yadunund --- rmf_charging_schedule/CHANGELOG.rst | 3 +++ rmf_charging_schedule/package.xml | 2 +- rmf_charging_schedule/setup.py | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 16 ++++++++++++++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 9 +++++++++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 6 ++++++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 6 ++++++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 8 ++++++++ rmf_websocket/package.xml | 2 +- 13 files changed, 55 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index a3514535..7a086b48 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ + 2.6.0 (2024-03-13) ------------------ diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index 826e4651..c8ae4a7d 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.6.0 + 2.7.0 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index bee67625..63d0f3c5 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='2.6.0', + version='2.7.0', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index 2c9482fe..c8e06b18 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ +* Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) +* Event based lift / door logic (`#320 `_) +* Filter DoorOpen insertion by map name (`#353 `_) +* Fix schema dictionary used during robot status override (`#349 `_) +* Add fleet-level reassign dispatched tasks API (`#348 `_) +* Automatically begin or cancel idle behavior when commission changes (`#346 `_) +* Disable automatic retreat (`#330 `_) +* Manual release of mutex groups (`#339 `_) +* Stabilize commissioning feature (`#338 `_) +* Release other mutexes if robot started charging (`#334 `_) +* Support labels in booking information (`#328 `_) +* Fix interaction between emergency pullover and finishing task (`#333 `_) +* Contributors: Aaron Chong, Grey, Luca Della Vedova, Xiyu, Yadunund + 2.6.0 (2024-03-13) ------------------ * Removes a line of dead code (`#322 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index cc73e8d1..df0d729c 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.6.0 + 2.7.0 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index 48c1ea0c..ea202d9a 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ +* Add fleet-level reassign dispatched tasks API (`#348 `_) +* Disable automatic retreat (`#330 `_) +* Stabilize commissioning feature (`#338 `_) +* Add all_known_lifts in Graph binding (`#336 `_) +* Add Speed Limit Requests (`#335 `_) +* Contributors: cwrx777, Grey, Pranay Shirodkar, Xiyu, Yadunund + 2.6.0 (2024-03-13) ------------------ * add in_lift readonly property in Graph::Waypoint binding. (`#326 `_) diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index 3f345e84..83d4a4a0 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.6.0 + 2.7.0 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index ec86b904..ca91cfbb 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ +* Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) +* Dispatcher only use websockets when dispatch fails (`#355 `_) +* Contributors: Aaron Chong, Grey, Luca Della Vedova, Yadunund + 2.6.0 (2024-03-13) ------------------ * Add Backward-ROS for improved logging in event of segfaults (`#327 `_) diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 5122c525..41cabb8a 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.6.0 + 2.7.0 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index 4648c045..ad0c4a6c 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ +* Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) +* Fix serialization of exit events (`#364 `_) +* Contributors: Grey, Luca Della Vedova, Yadunund + 2.6.0 (2024-03-13) ------------------ * Add Backward-ROS for improved logging in event of segfaults (`#327 `_) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 2ea8243c..faae6521 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.6.0 + 2.7.0 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index dcae2c66..40aa72a5 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.0 (2024-06-01) +------------------ +* Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) +* Fix deadlock in websocket server (`#342 `_) +* Lower debug level of some messages in rmf_websocket (`#340 `_) +* Refactors the socket broadcast client (`#329 `_) +* Contributors: Arjo Chakravarty, Grey, Luca Della Vedova, Yadunund + 2.6.0 (2024-03-13) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index 6919de27..2195ffc7 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.6.0 + 2.7.0 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From dfb656f16d5d3d73f76944fa1ec4b71cd01f5917 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 10 Jun 2024 17:34:43 +0800 Subject: [PATCH 16/35] Fix charging status (#347) Signed-off-by: Michael X. Grey --- rmf_charging_schedule/README.md | 5 +++ .../rmf_charging_schedule/main.py | 31 ++++++++++++++----- .../src/rmf_fleet_adapter/TaskManager.cpp | 13 +++++++- .../rmf_fleet_adapter/agv/RobotContext.cpp | 13 ++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 10 +++++- .../agv/RobotUpdateHandle.cpp | 2 +- .../phases/WaitForCharge.cpp | 1 + .../phases/WaitForCharge.hpp | 1 + 8 files changed, 65 insertions(+), 11 deletions(-) diff --git a/rmf_charging_schedule/README.md b/rmf_charging_schedule/README.md index 3a14c818..9e37973e 100644 --- a/rmf_charging_schedule/README.md +++ b/rmf_charging_schedule/README.md @@ -10,6 +10,7 @@ The format for the schedule looks like this: "02:00": { "robot_3": "charger_A", "robot_1": "queue_A" } "03:55": { "robot_2": "queue_B" } "04:00": { "robot_1": "charger_B", "robot_2": "queue_A" } +parking: ["queue_A", "queue_B"] ``` The time format is `"HH:MM"` where `HH` ranges from `00` to `23` and `MM` ranges @@ -37,3 +38,7 @@ the intended charger assignments at midnight. When the node is launched, it will move through the schedule from the earliest entry up until the last relevant one and issue an initial charger assignment message based on what the assignments would have been if the schedule had run from `00:00`. + +If any of the waypoints are parking points instead of charging points, put them +into a list called `parking`. Note that this node does not support the existence +of a fleet with the name `parking`. diff --git a/rmf_charging_schedule/rmf_charging_schedule/main.py b/rmf_charging_schedule/rmf_charging_schedule/main.py index 9d90062c..a4071e14 100644 --- a/rmf_charging_schedule/rmf_charging_schedule/main.py +++ b/rmf_charging_schedule/rmf_charging_schedule/main.py @@ -68,7 +68,11 @@ def __init__(self, fleet, robot, charger): self.charger = charger -def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): +def publish_assignments( + publisher: Publisher, + assignments: dict[dict[str]], + parking: list[str] +): for fleet, robots in assignments.items(): msg = ChargingAssignments() msg.fleet_name = fleet @@ -76,9 +80,11 @@ def publish_assignments(publisher: Publisher, assignments: dict[dict[str]]): assignment = ChargingAssignment() assignment.robot_name = robot assignment.waypoint_name = charger - # The mode isn't actually used yet, so it doesn't matter what we set - # it to. - assignment.mode = ChargingAssignment.MODE_CHARGE + + if charger in parking: + assignment.mode = ChargingAssignment.MODE_WAIT + else: + assignment.mode = ChargingAssignment.MODE_CHARGE msg.assignments.append(assignment) publisher.publish(msg) @@ -89,6 +95,7 @@ def update_assignments( sorted_times: list, schedule: dict, assignments: dict, + parking: list[str], publisher: Publisher, node: Node, ): @@ -100,7 +107,7 @@ def update_assignments( f'Sending {change.fleet}/{change.robot} to {change.charger} at ' f'{key.hour:02d}:{key.minute:02d}' ) - publish_assignments(publisher, assignments) + publish_assignments(publisher, assignments, parking) def simulation_time(node: Node) -> ScheduleTimePoint: @@ -176,7 +183,14 @@ def main(argv=sys.argv): schedule_yaml = yaml.safe_load(f) unsorted_schedule = {} + parking = [] for fleet, change in schedule_yaml.items(): + if fleet == 'parking': + # We treat the parking entry as a special case that simply lists + # which waypoints are parking spots + parking = change + continue + for time_text, assignments in change.items(): time = ScheduleTimePoint.parse(time_text) entry: list[Assignment] = unsorted_schedule.get(time, list()) @@ -206,7 +220,7 @@ def main(argv=sys.argv): last_update_index = bisect.bisect_right(sorted_times, get_time()) update_assignments( None, last_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) def update(): @@ -215,12 +229,13 @@ def update(): nonlocal schedule nonlocal assignments nonlocal publisher + nonlocal parking next_update_index = bisect.bisect_right(sorted_times, get_time()) if last_update_index < next_update_index: update_assignments( last_update_index, next_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) last_update_index = next_update_index @@ -228,7 +243,7 @@ def update(): # The cycle must have restarted, e.g. passing midnight update_assignments( None, next_update_index, - sorted_times, schedule, assignments, publisher, node, + sorted_times, schedule, assignments, parking, publisher, node, ) last_update_index = next_update_index diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 94723088..38cca49b 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -812,10 +812,21 @@ std::string TaskManager::robot_status() const if (_context->override_status().has_value()) return _context->override_status().value(); + if (_context->is_charging()) + { + if (_context->waiting_for_charger()) + { + return "idle"; + } + else + { + return "charging"; + } + } + if (!_active_task) return "idle"; - // TODO(MXG): Identify if the robot is charging and report that status here return "working"; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 6503edc2..ec5ebb45 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -846,6 +846,18 @@ bool RobotContext::waiting_for_charger() const return _waiting_for_charger; } +//============================================================================== +std::shared_ptr RobotContext::be_charging() +{ + return _lock_charging; +} + +//============================================================================== +bool RobotContext::is_charging() const +{ + return _lock_charging.use_count() > 1; +} + //============================================================================== const rxcpp::observable& RobotContext::observe_battery_soc() const { @@ -1158,6 +1170,7 @@ RobotContext::RobotContext( _requester_id( _itinerary.description().owner() + "/" + _itinerary.description().name()), _charging_wp(state.dedicated_charging_waypoint().value()), + _lock_charging(std::make_shared(0)), _current_task_end_state(state), _current_task_id(std::nullopt), _task_planner(std::move(task_planner)), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index a96be33e..d4417b24 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -591,6 +591,13 @@ class RobotContext /// (false)? bool waiting_for_charger() const; + /// This function will indicate that the robot is currently charging for as + /// long as the return value is held onto. + std::shared_ptr be_charging(); + + /// Check if the robot is currently doing a battery charging task. + bool is_charging() const; + // Get a reference to the battery soc observer of this robot. const rxcpp::observable& observe_battery_soc() const; @@ -847,7 +854,8 @@ class RobotContext std::size_t _charging_wp; /// When the robot reaches its _charging_wp, is there to wait for a charger /// (true) or to actually charge (false)? - bool _waiting_for_charger; + bool _waiting_for_charger = false; + std::shared_ptr _lock_charging; rxcpp::subjects::subject _battery_soc_publisher; rxcpp::observable _battery_soc_obs; rmf_task::State _current_task_end_state; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index dba26ffa..54170b73 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -254,7 +254,7 @@ RobotUpdateHandle& RobotUpdateHandle::set_charger_waypoint( if (!self) return; - self->_set_charging(charger_wp, true); + self->_set_charging(charger_wp, false); RCLCPP_INFO( self->node()->get_logger(), "Charger waypoint for robot [%s] set to index [%ld]", diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index 1b397e1c..be9c81c6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -119,6 +119,7 @@ WaitForCharge::Active::Active( } _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING); + _lock_charging = _context->be_charging(); } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp index b6138936..2c03adb6 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.hpp @@ -76,6 +76,7 @@ class WaitForCharge rmf_traffic::Time _last_update_time; double _initial_battery_soc; double _expected_charging_rate; // % per hour + std::shared_ptr _lock_charging; }; From 0329646138b92d1fe969c25904c51d7f5924eb04 Mon Sep 17 00:00:00 2001 From: Yadu Date: Mon, 10 Jun 2024 16:53:33 -0700 Subject: [PATCH 17/35] Bump main to 2.7.1 (#365) Signed-off-by: Yadunund --- rmf_charging_schedule/CHANGELOG.rst | 5 +++++ rmf_charging_schedule/package.xml | 2 +- rmf_charging_schedule/setup.py | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 5 +++++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 3 +++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 3 +++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 3 +++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 13 files changed, 29 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index 7a086b48..840fdd8d 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ +* Fix charging status (`#347 `_) +* Contributors: Grey + 2.7.0 (2024-06-01) ------------------ diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index c8ae4a7d..2bae4bf3 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.7.0 + 2.7.1 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index 63d0f3c5..65725a26 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='2.7.0', + version='2.7.1', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index c8e06b18..ed612d5f 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ +* Fix charging status (`#347 `_) +* Contributors: Grey + 2.7.0 (2024-06-01) ------------------ * Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index df0d729c..1363780b 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.7.0 + 2.7.1 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index ea202d9a..a8c465ba 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ + 2.7.0 (2024-06-01) ------------------ * Add fleet-level reassign dispatched tasks API (`#348 `_) diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index 83d4a4a0..27344e85 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.7.0 + 2.7.1 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index ca91cfbb..84d22344 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ + 2.7.0 (2024-06-01) ------------------ * Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 41cabb8a..391ac6a2 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.7.0 + 2.7.1 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index ad0c4a6c..8b7132a8 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ + 2.7.0 (2024-06-01) ------------------ * Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index faae6521..69bae1ea 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.7.0 + 2.7.1 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index 40aa72a5..c6e93272 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.7.1 (2024-06-11) +------------------ + 2.7.0 (2024-06-01) ------------------ * Fix race condition for ingesting/dispensing and disable uncrustify tests by default (`#362 `_) diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index 2195ffc7..f45c2c47 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.7.0 + 2.7.1 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From f2d40bf58373d589a3bfcb050f8fcd8bb5a66136 Mon Sep 17 00:00:00 2001 From: Yadu Date: Wed, 12 Jun 2024 09:52:47 -0700 Subject: [PATCH 18/35] Bump main after jazzy branching (#368) * Update changelogs Signed-off-by: Yadunund * 2.8.0 Signed-off-by: Yadunund --------- Signed-off-by: Yadunund --- rmf_charging_schedule/CHANGELOG.rst | 3 +++ rmf_charging_schedule/package.xml | 2 +- rmf_charging_schedule/setup.py | 2 +- rmf_fleet_adapter/CHANGELOG.rst | 3 +++ rmf_fleet_adapter/package.xml | 2 +- rmf_fleet_adapter_python/CHANGELOG.rst | 3 +++ rmf_fleet_adapter_python/package.xml | 2 +- rmf_task_ros2/CHANGELOG.rst | 3 +++ rmf_task_ros2/package.xml | 2 +- rmf_traffic_ros2/CHANGELOG.rst | 3 +++ rmf_traffic_ros2/package.xml | 2 +- rmf_websocket/CHANGELOG.rst | 3 +++ rmf_websocket/package.xml | 2 +- 13 files changed, 25 insertions(+), 7 deletions(-) diff --git a/rmf_charging_schedule/CHANGELOG.rst b/rmf_charging_schedule/CHANGELOG.rst index 840fdd8d..e0f4659d 100644 --- a/rmf_charging_schedule/CHANGELOG.rst +++ b/rmf_charging_schedule/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_charging_schedule ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ * Fix charging status (`#347 `_) diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index 2bae4bf3..e6f8abe6 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -2,7 +2,7 @@ rmf_charging_schedule - 2.7.1 + 2.8.0 Node for a fixed 24-hour rotating charger usage schedule Grey Apache License 2.0 diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index 65725a26..b5c4771d 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='2.7.1', + version='2.8.0', packages=find_packages(), data_files=[ ('share/ament_index/resource_index/packages', diff --git a/rmf_fleet_adapter/CHANGELOG.rst b/rmf_fleet_adapter/CHANGELOG.rst index ed612d5f..12ee6cad 100644 --- a/rmf_fleet_adapter/CHANGELOG.rst +++ b/rmf_fleet_adapter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_fleet_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ * Fix charging status (`#347 `_) diff --git a/rmf_fleet_adapter/package.xml b/rmf_fleet_adapter/package.xml index 1363780b..dd3c3b2d 100644 --- a/rmf_fleet_adapter/package.xml +++ b/rmf_fleet_adapter/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter - 2.7.1 + 2.8.0 Fleet Adapter package for RMF fleets. Grey Aaron diff --git a/rmf_fleet_adapter_python/CHANGELOG.rst b/rmf_fleet_adapter_python/CHANGELOG.rst index a8c465ba..f6c92049 100644 --- a/rmf_fleet_adapter_python/CHANGELOG.rst +++ b/rmf_fleet_adapter_python/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_fleet_adapter_python ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ diff --git a/rmf_fleet_adapter_python/package.xml b/rmf_fleet_adapter_python/package.xml index 27344e85..20b99009 100644 --- a/rmf_fleet_adapter_python/package.xml +++ b/rmf_fleet_adapter_python/package.xml @@ -2,7 +2,7 @@ rmf_fleet_adapter_python - 2.7.1 + 2.8.0 Python bindings for the rmf_fleet_adapter methylDragon Marco A. Gutiérrez diff --git a/rmf_task_ros2/CHANGELOG.rst b/rmf_task_ros2/CHANGELOG.rst index 84d22344..93225b53 100644 --- a/rmf_task_ros2/CHANGELOG.rst +++ b/rmf_task_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_task_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ diff --git a/rmf_task_ros2/package.xml b/rmf_task_ros2/package.xml index 391ac6a2..461b36bc 100644 --- a/rmf_task_ros2/package.xml +++ b/rmf_task_ros2/package.xml @@ -2,7 +2,7 @@ rmf_task_ros2 - 2.7.1 + 2.8.0 A package managing the dispatching of tasks in RMF system. Yadunund Marco A. Gutiérrez diff --git a/rmf_traffic_ros2/CHANGELOG.rst b/rmf_traffic_ros2/CHANGELOG.rst index 8b7132a8..e09616dc 100644 --- a/rmf_traffic_ros2/CHANGELOG.rst +++ b/rmf_traffic_ros2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_traffic_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ diff --git a/rmf_traffic_ros2/package.xml b/rmf_traffic_ros2/package.xml index 69bae1ea..af48d9f4 100644 --- a/rmf_traffic_ros2/package.xml +++ b/rmf_traffic_ros2/package.xml @@ -2,7 +2,7 @@ rmf_traffic_ros2 - 2.7.1 + 2.8.0 A package containing messages used by the RMF traffic management system. Grey Marco A. Gutiérrez diff --git a/rmf_websocket/CHANGELOG.rst b/rmf_websocket/CHANGELOG.rst index c6e93272..ace4b4e5 100644 --- a/rmf_websocket/CHANGELOG.rst +++ b/rmf_websocket/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rmf_websocket ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.8.0 (2024-06-12) +------------------ + 2.7.1 (2024-06-11) ------------------ diff --git a/rmf_websocket/package.xml b/rmf_websocket/package.xml index f45c2c47..ff853366 100644 --- a/rmf_websocket/package.xml +++ b/rmf_websocket/package.xml @@ -2,7 +2,7 @@ rmf_websocket - 2.7.1 + 2.8.0 A package managing the websocket api endpoints in RMF system. Yadunund Marco A. Gutiérrez From f3bf60e1730b1368ddfd17eafd6f65a70e74ef4b Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 14 Jun 2024 17:29:43 +0800 Subject: [PATCH 19/35] Fix incorrect std rounding function used when generating timestamp strings for task ID (#366) Signed-off-by: Aaron Chong --- rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp index b4beb89c..c8d92f0d 100644 --- a/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp +++ b/rmf_task_ros2/src/rmf_task_ros2/Dispatcher.cpp @@ -524,8 +524,7 @@ class Dispatcher::Implementation else if (use_timestamp_for_task_id) { task_id += std::to_string( - static_cast(std::round(node->get_clock()->now().seconds() * - 1e3))); + std::lround(node->get_clock()->now().seconds() * 1e3)); } else { From 5bf18e93342ce0ab53c3db4e1d7a55f7f7a62411 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 19 Jun 2024 10:54:28 +0800 Subject: [PATCH 20/35] Add icecream as a rosdep dependency (#358) Depends on https://github.com/ros/rosdistro/pull/41432 Signed-off-by: Arjo Chakravarty Co-authored-by: Yadu --- rmf_charging_schedule/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_charging_schedule/package.xml b/rmf_charging_schedule/package.xml index e6f8abe6..d02c7548 100644 --- a/rmf_charging_schedule/package.xml +++ b/rmf_charging_schedule/package.xml @@ -9,6 +9,7 @@ rclpy rmf_fleet_msgs + python3-icecream ament_python From 2b7e67503f0fa64ee7063ff8f91c84142587f6b5 Mon Sep 17 00:00:00 2001 From: Xiyu Date: Wed, 19 Jun 2024 04:56:42 +0100 Subject: [PATCH 21/35] Quiet cancel API (#357) Signed-off-by: Xiyu Oh --- .../agv/RobotUpdateHandle.hpp | 19 +++++++ .../src/rmf_fleet_adapter/TaskManager.cpp | 55 +++++++++++++++++-- .../src/rmf_fleet_adapter/TaskManager.hpp | 11 ++++ .../agv/RobotUpdateHandle.cpp | 31 +++++++++++ rmf_fleet_adapter_python/src/adapter.cpp | 15 +++++ 5 files changed, 127 insertions(+), 4 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 1a7514c3..6b8330b0 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -542,6 +542,25 @@ class RobotUpdateHandle /// Turn on/off a debug dump of how position updates are being processed void debug_positions(bool on); + /// Cancel a task but keep the task state displayed as completed, if it has + /// been assigned to this robot + /// + /// \param[in] task_id + /// The ID of the task to be canceled + /// + /// \param[in] labels + /// Labels that will be assigned to this cancellation. It is recommended to + /// include information about why the cancellation is happening. + /// + /// \param[in] on_cancellation + /// Callback that will be triggered after the cancellation is issued. + /// task_was_found will be true if the task was successfully found and + /// issued the cancellation, false otherwise. + void quiet_cancel_task( + std::string task_id, + std::vector labels, + std::function on_cancellation); + private: friend Implementation; Implementation* _pimpl; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 38cca49b..ee8d8b48 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -312,7 +312,8 @@ nlohmann::json& copy_phase_data( nlohmann::json& phases, const rmf_task::Phase::Active& snapshot, rmf_task::Log::Reader& reader, - nlohmann::json& all_phase_logs) + nlohmann::json& all_phase_logs, + bool quiet_cancel = false) { const auto& tag = *snapshot.tag(); const auto& header = tag.header(); @@ -345,6 +346,11 @@ nlohmann::json& copy_phase_data( auto& event_state = event_states[std::to_string(top->id())]; event_state["id"] = top->id(); event_state["status"] = status_to_string(top->status()); + if (quiet_cancel && top->status() == rmf_task::Event::Status::Canceled) + { + event_state["status"] = status_to_string( + rmf_task::Event::Status::Completed); + } // TODO(MXG): Keep a VersionedString Reader to know when to actually update // this string @@ -485,7 +491,7 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr) { const auto& snapshot = completed->snapshot(); auto& phase = copy_phase_data( - phases, *snapshot, mgr._log_reader, phase_logs); + phases, *snapshot, mgr._log_reader, phase_logs, _quiet_cancel); phase["unix_millis_start_time"] = to_millis(completed->start_time().time_since_epoch()).count(); @@ -500,7 +506,8 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr) if (active_phase == nullptr) return; auto& active = - copy_phase_data(phases, *active_phase, mgr._log_reader, phase_logs); + copy_phase_data( + phases, *active_phase, mgr._log_reader, phase_logs, _quiet_cancel); if (_task->active_phase_start_time().has_value()) { active["unix_millis_start_time"] = @@ -527,7 +534,7 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr) } } - if (_cancellation.has_value()) + if (_cancellation.has_value() && !_quiet_cancel) _state_msg["cancellation"] = *_cancellation; if (_killed.has_value()) @@ -543,6 +550,12 @@ void TaskManager::ActiveTask::publish_task_state(TaskManager& mgr) } } + if (_quiet_cancel && + _task->status_overview() == rmf_task::Event::Status::Canceled) + { + _state_msg["status"] = status_to_string(rmf_task::Event::Status::Completed); + } + task_state_update["data"] = _state_msg; static const auto task_update_validator = @@ -715,6 +728,25 @@ void TaskManager::ActiveTask::rewind(uint64_t phase_id) _task->rewind(phase_id); } +//============================================================================== +void TaskManager::ActiveTask::quiet_cancel( + std::vector labels, + rmf_traffic::Time time) +{ + if (_cancellation.has_value()) + return; + + nlohmann::json cancellation; + cancellation["unix_millis_request_time"] = + to_millis(time.time_since_epoch()).count(); + + cancellation["labels"] = std::move(labels); + + _cancellation = std::move(cancellation); + _quiet_cancel = true; + _task->cancel(); +} + //============================================================================== std::string TaskManager::ActiveTask::skip( uint64_t phase_id, @@ -1367,6 +1399,21 @@ bool TaskManager::kill_task( return false; } +//============================================================================== +bool TaskManager::quiet_cancel_task( + const std::string& task_id, + std::vector labels) +{ + if (_active_task && _active_task.id() == task_id) + { + _task_state_update_available = true; + _active_task.quiet_cancel(std::move(labels), _context->now()); + return true; + } + + return false; +} + //============================================================================== void TaskManager::_cancel_idle_behavior(std::vector labels) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp index 549412cd..429b2080 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.hpp @@ -233,6 +233,12 @@ class TaskManager : public std::enable_shared_from_this const std::string& task_id, std::vector labels); + /// Cancel a task for this robot while marking it completed. Returns true if + /// the task was being managed by this task manager, or false if it was not. + bool quiet_cancel_task( + const std::string& task_id, + std::vector labels); + /// This should only be triggered by RobotContext::set_commission(~), and only /// in scenarios where the idle behavior commission has been toggled off. void _cancel_idle_behavior(std::vector labels); @@ -294,6 +300,10 @@ class TaskManager : public std::enable_shared_from_this void rewind(uint64_t phase_id); + void quiet_cancel( + std::vector labels, + rmf_traffic::Time time); + std::string skip( uint64_t phase_id, std::vector labels, @@ -335,6 +345,7 @@ class TaskManager : public std::enable_shared_from_this std::unordered_map _skip_info_map; uint64_t _next_token = 0; + bool _quiet_cancel = false; }; friend class ActiveTask; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 54170b73..276cb7ae 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -1002,6 +1002,37 @@ void RobotUpdateHandle::Unstable::debug_positions(bool on) } } +//============================================================================== +void RobotUpdateHandle::Unstable::quiet_cancel_task( + std::string task_id, + std::vector labels, + std::function on_cancellation) +{ + if (const auto context = _pimpl->get_context()) + { + context->worker().schedule( + [ + task_id = std::move(task_id), + labels = std::move(labels), + on_cancellation = std::move(on_cancellation), + c = context->weak_from_this() + ](const auto&) + { + const auto context = c.lock(); + if (!context) + return; + + const auto mgr = context->task_manager(); + if (!mgr) + return; + + const auto result = mgr->quiet_cancel_task(task_id, labels); + if (on_cancellation) + on_cancellation(result); + }); + } +} + //============================================================================== void RobotUpdateHandle::ActionExecution::update_remaining_time( rmf_traffic::Duration remaining_time_estimate) diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index fa610273..a913faa9 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -254,6 +254,21 @@ PYBIND11_MODULE(rmf_adapter, m) { self.unstable().debug_positions(on); }, py::arg("on")) + .def("unstable_quiet_cancel_task", + [&](agv::RobotUpdateHandle& self, + std::string task_id, + std::vector labels, + std::functionon_cancellation) + { + self.unstable().quiet_cancel_task( + task_id, + labels, + on_cancellation + ); + }, + py::arg("task_id"), + py::arg("labels"), + py::arg("on_cancellation")) .def("set_action_executor", &agv::RobotUpdateHandle::set_action_executor, py::arg("action_executor")) From 141a33bd9c49794d98a75fcd1c998561b75cf272 Mon Sep 17 00:00:00 2001 From: Xiyu Date: Wed, 26 Jun 2024 10:16:55 +0100 Subject: [PATCH 22/35] Update with new RobotMode field (#345) Signed-off-by: Xiyu Oh Signed-off-by: Luca Della Vedova Co-authored-by: Luca Della Vedova --- rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp | 3 ++- .../src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index ee8d8b48..33bec639 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -1109,7 +1109,8 @@ TaskManager::RobotModeMsg TaskManager::robot_mode() const .mode(_active_task.is_finished() ? RobotModeMsg::MODE_IDLE : _context->current_mode()) - .mode_request_id(0); + .mode_request_id(0) + .performing_action(""); return mode; } diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp index 0ffb362c..bbe0cb45 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyTrafficLight.cpp @@ -1040,14 +1040,16 @@ void EasyTrafficLight::Implementation::Shared::publish_fleet_state() const .mode(rmf_fleet_msgs::msg::RobotMode::MODE_WAITING) // NOTE(MXG): This field is currently only used by the fleet drivers. // For now, we will just fill it with a zero. - .mode_request_id(0); + .mode_request_id(0) + .performing_action(""); } return rmf_fleet_msgs::build() .mode(rmf_fleet_msgs::msg::RobotMode::MODE_MOVING) // NOTE(MXG): This field is currently only used by the fleet drivers. // For now, we will just fill it with a zero. - .mode_request_id(0); + .mode_request_id(0) + .performing_action(""); } (); const auto& map = reported_location->map; From f881a0afd817f5309e47b5590e4189e41df48dfb Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 27 Jun 2024 11:26:15 +0800 Subject: [PATCH 23/35] Revert "Event based lift / door logic (#320)" (#372) This reverts commit 5e58af95b399724eb5ab58fdc27a64f6e26372b3. Signed-off-by: Luca Della Vedova --- rmf_fleet_adapter/src/door_supervisor/Node.cpp | 3 +-- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 10 +++------- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 6 ------ 3 files changed, 4 insertions(+), 15 deletions(-) diff --git a/rmf_fleet_adapter/src/door_supervisor/Node.cpp b/rmf_fleet_adapter/src/door_supervisor/Node.cpp index 83abfc92..5b5f149a 100644 --- a/rmf_fleet_adapter/src/door_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/door_supervisor/Node.cpp @@ -28,8 +28,7 @@ const std::string DoorSupervisorRequesterID = "door_supervisor"; Node::Node() : rclcpp::Node("door_supervisor") { - const auto default_qos = - rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); _door_request_pub = create_publisher( FinalDoorRequestTopicName, default_qos); diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index de59217a..e1bb36e6 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -27,8 +27,7 @@ namespace lift_supervisor { Node::Node() : rclcpp::Node("rmf_lift_supervisor") { - const auto default_qos = - rclcpp::SystemDefaultsQoS().durability_volatile().keep_last(100).reliable(); + const auto default_qos = rclcpp::SystemDefaultsQoS().keep_last(10); const auto transient_qos = rclcpp::SystemDefaultsQoS() .reliable().keep_last(100).transient_local(); @@ -69,14 +68,12 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) { if (curr_request->session_id == msg->session_id) { - msg->request_time = this->now(); - _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) - { curr_request = std::move(msg); - } else { + msg->request_time = this->now(); + _lift_request_pub->publish(*msg); RCLCPP_INFO( this->get_logger(), "[%s] Published end lift session from lift supervisor", @@ -88,7 +85,6 @@ void Node::_adapter_lift_request_update(LiftRequest::UniquePtr msg) } else { - _lift_request_pub->publish(*msg); if (msg->request_type != LiftRequest::REQUEST_END_SESSION) { curr_request = std::move(msg); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index ec5ebb45..8991d75c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1079,12 +1079,6 @@ void RobotContext::release_lift() "Releasing lift [%s] for [%s]", _lift_destination->lift_name.c_str(), requester_id().c_str()); - rmf_lift_msgs::msg::LiftRequest msg; - msg.lift_name = _lift_destination->lift_name; - msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; - msg.session_id = requester_id(); - msg.destination_floor = _lift_destination->destination_floor; - _node->lift_request()->publish(msg); } _lift_destination = nullptr; _initial_time_idle_outside_lift = std::nullopt; From 0180dc78ef7b97266d873686d93deca12602413d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 1 Jul 2024 10:17:49 +0800 Subject: [PATCH 24/35] Use main branch for repos file in CI (#374) Signed-off-by: Luca Della Vedova --- .github/workflows/asan.yaml | 1 + .github/workflows/build.yaml | 1 + .github/workflows/tsan.yaml | 1 + 3 files changed, 3 insertions(+) diff --git a/.github/workflows/asan.yaml b/.github/workflows/asan.yaml index bcfc8cc2..a1f234ca 100644 --- a/.github/workflows/asan.yaml +++ b/.github/workflows/asan.yaml @@ -16,6 +16,7 @@ jobs: [{"ros_distribution": "humble", "ubuntu_distribution": "jammy"}] # NOTE: Avoid adding comments in the package lines, this can break some of the called scripts in github actions + repos-branch-override: "main" packages: | rmf_traffic_ros2 rmf_task_ros2 diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 332aa5ad..f41b0ab8 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -14,6 +14,7 @@ jobs: uses: open-rmf/rmf_ci_templates/.github/workflows/reusable_build.yaml@main with: # NOTE: Avoid adding comments in the package lines, this can break some of the called scripts in github actions + repos-branch-override: "main" packages: | rmf_fleet_adapter rmf_fleet_adapter_python diff --git a/.github/workflows/tsan.yaml b/.github/workflows/tsan.yaml index 97220650..5b101fe0 100644 --- a/.github/workflows/tsan.yaml +++ b/.github/workflows/tsan.yaml @@ -16,6 +16,7 @@ jobs: [{"ros_distribution": "humble", "ubuntu_distribution": "jammy"}] # NOTE: Avoid adding comments in the package lines, this can break some of the called scripts in github actions + repos-branch-override: "main" packages: | rmf_traffic_ros2 rmf_task_ros2 From 214d2d3fa1fd05c522ebd35a02cb1c48a62bbc8f Mon Sep 17 00:00:00 2001 From: Xiyu Date: Mon, 1 Jul 2024 04:09:14 +0100 Subject: [PATCH 25/35] Populate LiftProperties for Destination in localize callbacks (#373) Signed-off-by: Xiyu Oh --- .../src/rmf_fleet_adapter/phases/RequestLift.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index 57272a2d..fedeeb0c 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -235,6 +235,10 @@ void RequestLift::ActivePhase::_init_obs() agv::Destination::Implementation::get(*me->_data.localize_after) .position = me->_context->position(); + const auto graph = me->_context->navigation_graph(); + agv::Destination::Implementation::get(*me->_data.localize_after) + .lift = graph.find_known_lift(me->_lift_name); + if (me->_context->localize(*me->_data.localize_after, std::move(cmd))) { From 4eaa86ababd84648e7cad2e26c4f52a202f808b9 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Mon, 15 Jul 2024 16:31:47 +0800 Subject: [PATCH 26/35] Fixes warning given by `colcon_ros` on Jazzy (#377) This is a small fix for when we build systems on jazzy we get the warning: `[5.452s] WARNING:colcon.colcon_ros.task.ament_python.build:Package 'rmf_charging_schedule' doesn't explicitly install the 'package.xml' file (colcon-ros currently does it implicitly but that fallback will be removed in the future)` This pr fixes this warning by installing the `package.xml` Signed-off-by: Arjo Chakravarty --- rmf_charging_schedule/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_charging_schedule/setup.py b/rmf_charging_schedule/setup.py index b5c4771d..f3087b67 100644 --- a/rmf_charging_schedule/setup.py +++ b/rmf_charging_schedule/setup.py @@ -7,6 +7,7 @@ version='2.8.0', packages=find_packages(), data_files=[ + ('share/' + package_name, ['package.xml']), ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ], From e53d0879d54dd02c4a17a41bcf0edb41ba368c8a Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 17 Jul 2024 13:56:22 +0800 Subject: [PATCH 27/35] Provide an API that says the robot's lift destination (#376) Signed-off-by: Michael X. Grey --- .../agv/RobotUpdateHandle.hpp | 20 ++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 24 ++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.hpp | 20 ++++++++++++ .../agv/RobotUpdateHandle.cpp | 31 +++++++++++++++++++ .../agv/internal_RobotUpdateHandle.hpp | 17 ++++++++++ .../rmf_fleet_adapter/events/ExecutePlan.cpp | 14 +++++++++ .../rmf_fleet_adapter/phases/RequestLift.cpp | 5 +++ .../rmf_fleet_adapter/phases/RequestLift.hpp | 8 +++++ rmf_fleet_adapter_python/src/adapter.cpp | 15 ++++++++- 9 files changed, 153 insertions(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp index 6b8330b0..132c749c 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp @@ -469,6 +469,26 @@ class RobotUpdateHandle /// could change in the future. void reassign_dispatched_tasks(); + /// Information about where the lift will be asked to go for a robot. + class LiftDestination + { + public: + /// Name of the lift that is being used. + const std::string& lift() const; + + /// Name of the level that the lift will be going to. + const std::string& level() const; + + class Implementation; + private: + LiftDestination(); + rmf_utils::impl_ptr _pimpl; + }; + + /// If this robot has begun a lift session, this will contain information + /// about where the robot will ask to go, and which lift it intends to use. + std::optional lift_destination() const; + class Implementation; /// This API is experimental and will not be supported in the future. Users diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 8991d75c..bcb280a4 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1084,6 +1084,30 @@ void RobotContext::release_lift() _initial_time_idle_outside_lift = std::nullopt; _lift_stubbornness = nullptr; _lift_arrived = false; + clear_final_lift_destination(); +} + +//============================================================================== +std::optional +RobotContext::final_lift_destination() const +{ + std::unique_lock lock(*_final_lift_destination_mutex); + return _final_lift_destination; +} + +//============================================================================== +void RobotContext::set_final_lift_destination( + RobotUpdateHandle::LiftDestination destination) +{ + std::unique_lock lock(*_final_lift_destination_mutex); + _final_lift_destination = std::move(destination); +} + +//============================================================================== +void RobotContext::clear_final_lift_destination() +{ + std::unique_lock lock(*_final_lift_destination_mutex); + _final_lift_destination = std::nullopt; } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index d4417b24..ea6d1d5f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -680,6 +680,22 @@ class RobotContext /// Indicate that the lift is no longer needed void release_lift(); + /// Get the final lift destination that the overall session intends to end at. + /// This is used to give robots advanced notice of what floor they will be + /// going to, in case they need this information in order to enter the lift + /// correctly. + std::optional + final_lift_destination() const; + + /// Change what the final lift destination is currently set to. Typically this + /// should only be called by RequestLift. + void set_final_lift_destination( + RobotUpdateHandle::LiftDestination destination); + + /// Clear the information about the final lift destination. Typically this + /// should only be called by release_lift(). + void clear_final_lift_destination(); + /// Check if a door is being held const std::optional& holding_door() const; @@ -913,6 +929,10 @@ class RobotContext rclcpp::Subscription::SharedPtr _mutex_group_manual_release_sub; std::chrono::steady_clock::time_point _last_active_task_time; + + std::optional _final_lift_destination; + std::unique_ptr _final_lift_destination_mutex = + std::make_unique(); }; using RobotContextPtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp index 276cb7ae..761d1121 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotUpdateHandle.cpp @@ -828,6 +828,37 @@ void RobotUpdateHandle::reassign_dispatched_tasks() } } +//============================================================================== +RobotUpdateHandle::LiftDestination::LiftDestination() + : _pimpl(rmf_utils::make_impl()) +{ + // Do nothing +} + +//============================================================================== +const std::string& RobotUpdateHandle::LiftDestination::lift() const +{ + return _pimpl->lift; +} + +//============================================================================== +const std::string& RobotUpdateHandle::LiftDestination::level() const +{ + return _pimpl->level; +} + +//============================================================================== +std::optional +RobotUpdateHandle::lift_destination() const +{ + if (const auto context = _pimpl->get_context()) + { + return context->final_lift_destination(); + } + + return std::nullopt; +} + //============================================================================== RobotUpdateHandle::RobotUpdateHandle() { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp index 5d3f7886..10a1c265 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_RobotUpdateHandle.hpp @@ -333,6 +333,23 @@ class RobotUpdateHandle::Unstable::Stubbornness::Implementation } }; +//============================================================================== +class RobotUpdateHandle::LiftDestination::Implementation +{ +public: + std::string lift; + std::string level; + + static RobotUpdateHandle::LiftDestination make( + std::string lift, std::string level) + { + RobotUpdateHandle::LiftDestination result; + result._pimpl->lift = std::move(lift); + result._pimpl->level = std::move(level); + return result; + } +}; + } // namespace agv } // namespace rmf_fleet_adapter diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp index 8e24390f..2e7c99a5 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/events/ExecutePlan.cpp @@ -29,6 +29,8 @@ #include "../agv/internal_EasyFullControl.hpp" +#include "../agv/internal_RobotUpdateHandle.hpp" + #include namespace rmf_fleet_adapter { @@ -515,6 +517,10 @@ std::optional search_for_lift_group( return std::nullopt; } + auto final_lift_destination = + agv::RobotUpdateHandle::LiftDestination::Implementation::make( + lift_name, lift_end->destination()); + auto category = "Take [lift:" + lift_name + "] to [floor:" + lift_end->destination() + "]"; @@ -534,6 +540,14 @@ std::optional search_for_lift_group( if (it->phase) { + if (auto* lift_request = dynamic_cast(it->phase.get())) + { + // Inject the final lift destination information into every lift + // request in this group. + lift_request->data().final_lift_destination = + final_lift_destination; + } + lift_group.push_back( [legacy = it->phase, context, event_id](UpdateFn update) { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index fedeeb0c..a44452ef 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -97,6 +97,11 @@ void RequestLift::ActivePhase::_init_obs() { using rmf_lift_msgs::msg::LiftState; + if (_data.final_lift_destination.has_value()) + { + _context->set_final_lift_destination(*_data.final_lift_destination); + } + if (_data.located == Located::Outside && _context->current_lift_destination()) { // Check if the current destination is the one we want and also has arrived. diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp index cc6b9233..4e50aa30 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.hpp @@ -43,6 +43,9 @@ struct RequestLift std::shared_ptr resume_itinerary = nullptr; std::optional hold_point = std::nullopt; + + std::optional + final_lift_destination = std::nullopt; }; class ActivePhase : public LegacyTask::ActivePhase, @@ -128,6 +131,11 @@ struct RequestLift return _lift_name; } + Data& data() + { + return _data; + } + private: agv::RobotContextPtr _context; std::string _lift_name; diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index a913faa9..2c5bc568 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -63,6 +63,7 @@ using RobotInterruption = agv::RobotUpdateHandle::Interruption; using IssueTicket = agv::RobotUpdateHandle::IssueTicket; using Commission = agv::RobotUpdateHandle::Commission; using Stubbornness = agv::RobotUpdateHandle::Unstable::Stubbornness; +using LiftDestination = agv::RobotUpdateHandle::LiftDestination; void bind_types(py::module&); void bind_graph(py::module&); @@ -314,7 +315,9 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("commission", &agv::RobotUpdateHandle::commission) .def("reassign_dispatched_tasks", - &agv::RobotUpdateHandle::reassign_dispatched_tasks); + &agv::RobotUpdateHandle::reassign_dispatched_tasks) + .def("lift_destination", + &agv::RobotUpdateHandle::lift_destination); // ACTION EXECUTOR ======================================================= auto m_robot_update_handle = m.def_submodule("robot_update_handle"); @@ -399,6 +402,16 @@ PYBIND11_MODULE(rmf_adapter, m) { .def("release", &Stubbornness::release); + // LiftDestination ====================================================== + py::class_( + m_robot_update_handle, "LiftDestination") + .def_property_readonly( + "lift", + &LiftDestination::lift) + .def_property_readonly( + "level", + &LiftDestination::level); + // FLEETUPDATE HANDLE ====================================================== py::class_>( From a265614cc072c0d2dc68372fc8dfed307b5203f0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 19 Jul 2024 10:27:02 +0800 Subject: [PATCH 28/35] Fix a minor typo in fleet adapter error log (#307) Signed-off-by: Arjo Chakravarty --- .../src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index cd9d3a2c..72d34304 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -469,7 +469,7 @@ void FleetUpdateHandle::Implementation::bid_notice_cb( RCLCPP_INFO( node->get_logger(), "Fleet [%s] does not have any robots to accept task [%s]. Use " - "FleetUpdateHadndle::add_robot(~) to add robots to this fleet. ", + "FleetUpdateHandle::add_robot(~) to add robots to this fleet. ", name.c_str(), task_id.c_str()); return; } From 47722c616b7c5cabcbad1b27f718e23b5916d406 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 29 Jul 2024 11:49:19 +0800 Subject: [PATCH 29/35] Only fail on_cancel deserializations when none of the activities are possible (#378) Signed-off-by: Aaron Chong --- .../src/rmf_fleet_adapter/tasks/Patrol.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) 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 9c7e772d..6842e0ce 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/tasks/Patrol.cpp @@ -58,16 +58,20 @@ void add_patrol( for (const auto& place_msg : one_of.value()) { auto place = place_deser(place_msg); + errors.insert( + errors.end(), + std::make_move_iterator(place.errors.begin()), + std::make_move_iterator(place.errors.end())); if (!place.description.has_value()) { - return {nullptr, std::move(place.errors)}; + continue; } goals.push_back(*place.description); - errors.insert( - errors.end(), - std::make_move_iterator(place.errors.begin()), - std::make_move_iterator(place.errors.end())); + } + if (goals.empty()) + { + return {nullptr, errors}; } auto desc = GoToPlace::Description::make_for_one_of(goals); From 48f5cb1bb7a4c22744dd5a45817c2abf0fefad07 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 5 Aug 2024 13:26:19 +0800 Subject: [PATCH 30/35] Configure strict lanes (#367) Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/EasyFullControl.hpp | 11 ++ .../src/rmf_fleet_adapter/agv/Adapter.cpp | 1 + .../rmf_fleet_adapter/agv/EasyFullControl.cpp | 124 ++++++++++++++++++ .../rmf_fleet_adapter/agv/RobotContext.cpp | 121 +++++++++++------ .../rmf_fleet_adapter/agv/RobotContext.hpp | 13 +- .../agv/internal_EasyFullControl.hpp | 4 +- 6 files changed, 229 insertions(+), 45 deletions(-) diff --git a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp index c6eb2cd3..3a3b02fe 100644 --- a/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp +++ b/rmf_fleet_adapter/include/rmf_fleet_adapter/agv/EasyFullControl.hpp @@ -821,6 +821,17 @@ class EasyFullControl::FleetConfiguration const std::unordered_map& lift_emergency_levels() const; + /// A set of lanes which must strictly be navigated from from the start to end + /// of the lane when used. This means when replanning, the planner cannot ask + /// a robot in the middle of one of these lanes to immediately go to the end + /// of the lane. + const std::unordered_set& strict_lanes() const; + + /// Get a mutable reference to the set of strict lanes. + /// + /// \sa strict_lanes + std::unordered_set& change_strict_lanes(); + class Implementation; private: rmf_utils::impl_ptr _pimpl; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp index 9c1e4d5d..ee49b354 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Adapter.cpp @@ -379,6 +379,7 @@ std::shared_ptr Adapter::add_easy_fleet( fleet_handle, config.skip_rotation_commands(), tf_dict, + config.strict_lanes(), config.default_responsive_wait(), config.default_max_merge_waypoint_distance(), config.default_max_merge_lane_distance(), diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp index b44238d4..387f7f48 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp @@ -373,6 +373,13 @@ class EasyFullControl::CommandExecution::Implementation::Data continue; } + if (nav_params->strict_lanes.count(lane_id) > 0) + { + // The robot needs to fully approach the beginning waypoint before + // using this lane + continue; + } + auto wp_exit = graph.get_lane(lane_id).exit().waypoint_index(); starts.push_back( rmf_traffic::agv::Plan::Start(now, wp_exit, yaw, p, lane_id)); @@ -401,6 +408,12 @@ class EasyFullControl::CommandExecution::Implementation::Data continue; } + if (nav_params->strict_lanes.count(lane_id) > 0) + { + // This is a strict lane, so we cannot start from its middle. + continue; + } + const auto& lane = graph.get_lane(lane_id); const auto p0 = graph.get_waypoint(lane.entry().waypoint_index()).get_location(); @@ -1562,6 +1575,7 @@ void EasyFullControl::EasyRobotUpdateHandle::update( RobotState state, ConstActivityIdentifierPtr current_activity) { + _pimpl->worker.schedule( [ state = std::move(state), @@ -1702,6 +1716,7 @@ class EasyFullControl::FleetConfiguration::Implementation double default_max_merge_lane_distance; double default_min_lane_length; std::unordered_map lift_emergency_levels; + std::unordered_set strict_lanes; }; //============================================================================== @@ -1757,6 +1772,7 @@ EasyFullControl::FleetConfiguration::FleetConfiguration( std::move(default_max_merge_waypoint_distance), std::move(default_max_merge_lane_distance), std::move(default_min_lane_length), + {}, {} })) { @@ -2338,6 +2354,99 @@ EasyFullControl::FleetConfiguration::from_config_files( } } + std::unordered_set strict_lanes; + const YAML::Node& strict_lanes_yaml = rmf_fleet["strict_lanes"]; + if (strict_lanes_yaml) + { + if (!strict_lanes_yaml.IsSequence()) + { + std::cerr + << "[strict_lanes] element is not a sequence in the config file [" + << config_file << "] so we cannot parse which lanes need to be " + << "considered strict." << std::endl; + return std::nullopt; + } + + const auto vertex_stacks = compute_stacked_vertices( + graph, default_max_merge_waypoint_distance); + for (const auto& lane_yaml : strict_lanes_yaml) + { + if (!lane_yaml.IsSequence() || lane_yaml.size() != 2) + { + const auto mark = lane_yaml.Mark(); + std::cerr << "[strict_lanes] Unrecognized lane format at line " + << mark.line << ", column " << mark.column << std::endl; + return std::nullopt; + } + + const auto wp0_name = lane_yaml[0].as(); + const auto wp1_name = lane_yaml[1].as(); + const auto* wp0 = graph.find_waypoint(wp0_name); + if (!wp0) + { + const auto mark = lane_yaml[0].Mark(); + std::cerr << "[strict_lanes] Unrecognized waypoint name [" << wp0_name + << "] at line " << mark.line << ", column " << mark.column << std::endl; + return std::nullopt; + } + + const auto* wp1 = graph.find_waypoint(wp1_name); + if (!wp1) + { + const auto mark = lane_yaml[1].Mark(); + std::cerr << "[strict_lanes] Unrecognized waypoint name [" << wp1_name + << "] at line " << mark.line << ", column " << mark.column << std::endl; + return std::nullopt; + } + + const auto stack_0_it = vertex_stacks.find(wp0->index()); + VertexStack stack_0; + if (stack_0_it == vertex_stacks.end()) + { + stack_0 = std::make_shared>(); + stack_0->insert(wp0->index()); + } + else + { + stack_0 = stack_0_it->second; + } + + const auto stack_1_it = vertex_stacks.find(wp1->index()); + VertexStack stack_1; + if (stack_1_it == vertex_stacks.end()) + { + stack_1 = std::make_shared>(); + stack_1->insert(wp1->index()); + } + else + { + stack_1 = stack_1_it->second; + } + + bool found_lane = false; + for (std::size_t wp_i : *stack_0) + { + for (std::size_t wp_j : *stack_1) + { + const auto* lane = graph.lane_from(wp_i, wp_j); + if (lane) + { + strict_lanes.insert(lane->index()); + found_lane = true; + } + } + } + + if (!found_lane) + { + const auto mark = lane_yaml.Mark(); + std::cerr << "[strict_lanes] Unable to find a lane from [" << wp0_name + << "] to [" << wp1_name << "] at line " << mark.line << ", column " + << mark.column << std::endl; + } + } + } + auto config = FleetConfiguration( fleet_name, std::move(tf_dict), @@ -2364,6 +2473,7 @@ EasyFullControl::FleetConfiguration::from_config_files( default_min_lane_length); config.change_lift_emergency_levels() = lift_emergency_levels; config.set_retreat_to_charger_interval(retreat_to_charger_interval); + config.change_strict_lanes() = std::move(strict_lanes); return config; } @@ -2747,6 +2857,20 @@ EasyFullControl::FleetConfiguration::lift_emergency_levels() const return _pimpl->lift_emergency_levels; } +//============================================================================== +const std::unordered_set& +EasyFullControl::FleetConfiguration::strict_lanes() const +{ + return _pimpl->strict_lanes; +} + +//============================================================================== +std::unordered_set& +EasyFullControl::FleetConfiguration::change_strict_lanes() +{ + return _pimpl->strict_lanes; +} + //============================================================================== using EasyCommandHandlePtr = std::shared_ptr; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index bcb280a4..805d3758 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -31,49 +31,11 @@ namespace rmf_fleet_adapter { namespace agv { //============================================================================== -void NavParams::search_for_location( - const std::string& map, - Eigen::Vector3d position, - RobotContext& context) -{ - auto planner = context.planner(); - if (!planner) - { - RCLCPP_ERROR( - context.node()->get_logger(), - "Planner unavailable for robot [%s], cannot update its location", - context.requester_id().c_str()); - return; - } - const auto& graph = planner->get_configuration().graph(); - const auto now = context.now(); - auto starts = compute_plan_starts(graph, map, position, now); - if (!starts.empty()) - { - if (context.debug_positions) - { - std::stringstream ss; - ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() - << " starts:" << print_starts(starts, graph); - std::cout << ss.str() << std::endl; - } - context.set_location(std::move(starts)); - } - else - { - if (context.debug_positions) - { - std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " - << map << " <" << position.block<2, 1>(0, 0).transpose() - << "> orientation " << position[2] * 180.0 / M_PI << std::endl; - } - context.set_lost(Location { now, map, position }); - } -} - -//============================================================================== -void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) +std::unordered_map compute_stacked_vertices( + const rmf_traffic::agv::Graph& graph, + double max_merge_waypoint_distance) { + std::unordered_map stacked_vertices; for (std::size_t i = 0; i < graph.num_waypoints() - 1; ++i) { const auto& wp_i = graph.get_waypoint(i); @@ -128,6 +90,56 @@ void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) stacked_vertices[j] = stack_j; } } + + return stacked_vertices; +} + +//============================================================================== +void NavParams::search_for_location( + const std::string& map, + Eigen::Vector3d position, + RobotContext& context) +{ + auto planner = context.planner(); + if (!planner) + { + RCLCPP_ERROR( + context.node()->get_logger(), + "Planner unavailable for robot [%s], cannot update its location", + context.requester_id().c_str()); + return; + } + const auto& graph = planner->get_configuration().graph(); + const auto now = context.now(); + auto starts = compute_plan_starts(graph, map, position, now); + if (!starts.empty()) + { + if (context.debug_positions) + { + std::stringstream ss; + ss << __FILE__ << "|" << __LINE__ << ": " << starts.size() + << " starts:" << print_starts(starts, graph); + std::cout << ss.str() << std::endl; + } + context.set_location(std::move(starts)); + } + else + { + if (context.debug_positions) + { + std::cout << __FILE__ << "|" << __LINE__ << ": setting robot to LOST | " + << map << " <" << position.block<2, 1>(0, 0).transpose() + << "> orientation " << position[2] * 180.0 / M_PI << std::endl; + } + context.set_lost(Location { now, map, position }); + } +} + +//============================================================================== +void NavParams::find_stacked_vertices(const rmf_traffic::agv::Graph& graph) +{ + stacked_vertices = compute_stacked_vertices( + graph, max_merge_waypoint_distance); } //============================================================================== @@ -166,7 +178,8 @@ rmf_traffic::agv::Plan::StartSet NavParams::process_locations( const rmf_traffic::agv::Graph& graph, rmf_traffic::agv::Plan::StartSet locations) const { - return _lift_boundary_filter(graph, _descend_stacks(graph, locations)); + return _strict_lane_filter( + _lift_boundary_filter(graph, _descend_stacks(graph, locations))); } //============================================================================== @@ -321,6 +334,28 @@ rmf_traffic::agv::Plan::StartSet NavParams::_lift_boundary_filter( return locations; } +//============================================================================== +rmf_traffic::agv::Plan::StartSet NavParams::_strict_lane_filter( + rmf_traffic::agv::Plan::StartSet locations) const +{ + const auto r_it = std::remove_if( + locations.begin(), + locations.end(), + [this](const rmf_traffic::agv::Plan::Start& location) + { + const auto lane = location.lane(); + if (lane.has_value()) + { + return this->strict_lanes.count(*lane) > 0; + } + + return false; + }); + + locations.erase(r_it, locations.end()); + return locations; +} + //============================================================================== bool NavParams::in_same_stack( std::size_t wp0, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index ea6d1d5f..e8160383 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -243,11 +243,17 @@ inline std::string print_starts( return ss.str(); } +//============================================================================== +std::unordered_map compute_stacked_vertices( + const rmf_traffic::agv::Graph& graph, + double max_merge_waypoint_distance); + //============================================================================== struct NavParams { bool skip_rotation_commands; std::shared_ptr transforms_to_robot_coords; + std::unordered_set strict_lanes; double max_merge_waypoint_distance = 1e-3; double max_merge_lane_distance = 0.3; double min_lane_length = 1e-8; @@ -318,7 +324,7 @@ struct NavParams return {}; } - std::unordered_map stacked_vertices; + std::unordered_map stacked_vertices = {}; void find_stacked_vertices(const rmf_traffic::agv::Graph& graph); @@ -340,6 +346,11 @@ struct NavParams const rmf_traffic::agv::Graph& graph, rmf_traffic::agv::Plan::StartSet locations) const; + // If one of the starts is mid-lane on a strict lane, filter it out of the + // start set. + rmf_traffic::agv::Plan::StartSet _strict_lane_filter( + rmf_traffic::agv::Plan::StartSet locations) const; + bool in_same_stack(std::size_t wp0, std::size_t wp1) const; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp index f56291c1..83224d48 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/internal_EasyFullControl.hpp @@ -41,6 +41,7 @@ class EasyFullControl::Implementation std::shared_ptr fleet_handle, bool skip_rotation_commands, std::shared_ptr transforms_to_robot_coords, + std::unordered_set strict_lanes, bool default_responsive_wait, double default_max_merge_waypoint_distance, double default_max_merge_lane_distance, @@ -54,9 +55,10 @@ class EasyFullControl::Implementation NavParams{ skip_rotation_commands, std::move(transforms_to_robot_coords), + std::move(strict_lanes), default_max_merge_waypoint_distance, default_max_merge_lane_distance, - default_min_lane_length + default_min_lane_length, }, default_responsive_wait }); From ceab18b025d7524c3a8141ba166456751172d877 Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 21 Aug 2024 13:49:42 +0800 Subject: [PATCH 31/35] Add a timeout before automatically releasing lift (#369) Signed-off-by: Michael X. Grey --- .../rmf_fleet_adapter/agv/RobotContext.cpp | 32 +++++++++++++++---- .../rmf_fleet_adapter/agv/RobotContext.hpp | 8 ----- .../rmf_fleet_adapter/phases/RequestLift.cpp | 16 ++++++++-- 3 files changed, 39 insertions(+), 17 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 805d3758..2fcf97dd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1386,11 +1386,29 @@ void RobotContext::_check_lift_state( { if (_lift_destination && !_lift_destination->requested_from_inside) { - // The lift destination reference count dropping to one while the lift - // destination request is on the outside means the task that prompted the - // lift usage was cancelled while the robot was outside of the lift. - // Therefore we should release the usage of the lift. - release_lift(); + const auto now = std::chrono::steady_clock::now(); + if (_initial_time_idle_outside_lift.has_value()) + { + const auto lapse = now - *_initial_time_idle_outside_lift; + if (lapse > std::chrono::seconds(30)) + { + // The lift destination reference count dropping to one while the lift + // destination request is on the outside means the task that prompted + // the lift usage was cancelled while the robot was outside of the lift. + // Therefore we should release the usage of the lift. + RCLCPP_INFO( + _node->get_logger(), + "Requesting lift [%s] to be released for [%s] because it is outside " + "the lift and not holding a claim for an extended period of time.", + _lift_destination->lift_name.c_str(), + requester_id().c_str()); + release_lift(); + } + } + else + { + _initial_time_idle_outside_lift = now; + } } else if (_lift_destination && !_current_task_id.has_value()) { @@ -1415,7 +1433,7 @@ void RobotContext::_check_lift_state( if (_initial_time_idle_outside_lift.has_value()) { const auto lapse = now - *_initial_time_idle_outside_lift; - if (lapse > std::chrono::seconds(2)) + if (lapse > std::chrono::seconds(10)) { RCLCPP_INFO( _node->get_logger(), @@ -1423,8 +1441,8 @@ void RobotContext::_check_lift_state( "outside of the lift.", _lift_destination->lift_name.c_str(), requester_id().c_str()); + release_lift(); } - release_lift(); } else { diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp index e8160383..16953a7a 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.hpp @@ -397,14 +397,6 @@ struct MutexGroupData TimeMsg claim_time; }; -//============================================================================== -struct MutexGroupSwitch -{ - std::string from; - std::string to; - std::function accept; -}; - //============================================================================== class RobotContext : public std::enable_shared_from_this, diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp index a44452ef..3fd3832e 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/RequestLift.cpp @@ -116,6 +116,16 @@ void RequestLift::ActivePhase::_init_obs() if (!self) return; + if (self->_data.located == Located::Outside) + { + // The robot is going to start moving into the lift now, so we + // should lock in the lift by saying that the request is coming from + // inside the lift. This will prevent the auto-detection system from + // releasing the lift prematurely. + self->_context->set_lift_destination( + self->_lift_name, self->_destination, true); + } + if (self->_data.resume_itinerary) { self->_context->schedule_itinerary( @@ -420,8 +430,10 @@ bool RequestLift::ActivePhase::_finish() if (_data.located == Located::Outside) { - // The robot is going to start moving into the lift now, so we should lock - // the destination in. + // The robot is going to start moving into the lift now, so we + // should lock in the lift by saying that the request is coming from + // inside the lift. This will prevent the auto-detection system from + // releasing the lift prematurely. _context->set_lift_destination(_lift_name, _destination, true); // We should replan to make sure there are no traffic issues that came up From 44cd76c38494afe35c352522c6820819bac04d72 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 2 Sep 2024 11:28:23 +0800 Subject: [PATCH 32/35] Emergency trigger to use transient local qos (#341) * Emergency trigger to use transient local qos Signed-off-by: Aaron Chong * Removing lift supervisor publisher Signed-off-by: Aaron Chong --------- Signed-off-by: Aaron Chong --- rmf_fleet_adapter/src/lift_supervisor/Node.cpp | 16 ---------------- rmf_fleet_adapter/src/lift_supervisor/Node.hpp | 4 ---- .../src/rmf_fleet_adapter/agv/Node.cpp | 2 +- 3 files changed, 1 insertion(+), 21 deletions(-) diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp index e1bb36e6..8b1bb57e 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.cpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.cpp @@ -47,9 +47,6 @@ Node::Node() { _lift_state_update(std::move(msg)); }); - - _emergency_notice_pub = create_publisher( - rmf_traffic_ros2::EmergencyTopicName, default_qos); } //============================================================================== @@ -124,19 +121,6 @@ void Node::_lift_state_update(LiftState::UniquePtr msg) request.request_type = LiftRequest::REQUEST_END_SESSION; _lift_request_pub->publish(request); } - - // For now, we do not need to publish this. - -// std_msgs::msg::Bool emergency_msg; -// emergency_msg.data = false; - -// if (LiftState::MODE_FIRE == msg->current_mode -// || LiftState::MODE_EMERGENCY == msg->current_mode) -// { -// emergency_msg.data = true; -// } - -// _emergency_notice_pub->publish(emergency_msg); } } // namespace lift_supervisor diff --git a/rmf_fleet_adapter/src/lift_supervisor/Node.hpp b/rmf_fleet_adapter/src/lift_supervisor/Node.hpp index a3ff3868..31d31613 100644 --- a/rmf_fleet_adapter/src/lift_supervisor/Node.hpp +++ b/rmf_fleet_adapter/src/lift_supervisor/Node.hpp @@ -53,10 +53,6 @@ class Node : public rclcpp::Node LiftStateSub::SharedPtr _lift_state_sub; void _lift_state_update(LiftState::UniquePtr msg); - using EmergencyNotice = std_msgs::msg::Bool; - using EmergencyNoticePub = rclcpp::Publisher; - EmergencyNoticePub::SharedPtr _emergency_notice_pub; - std::unordered_map _active_sessions; }; diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp index f3b36799..cc596946 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/Node.cpp @@ -76,7 +76,7 @@ std::shared_ptr Node::make( node->_emergency_notice_obs = node->create_observable( - rmf_traffic_ros2::EmergencyTopicName, default_qos); + rmf_traffic_ros2::EmergencyTopicName, transient_qos); node->_ingestor_request_pub = node->create_publisher( From 1ab38d717283aece61050874b9827d111de66b29 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 3 Sep 2024 09:13:56 +0800 Subject: [PATCH 33/35] Populate `Booking.Priority`, added schema to validate `Priority` (#381) * Populate Booking.Priority, added schema to validate Priority Signed-off-by: Aaron Chong * Fix c_str Signed-off-by: Aaron Chong --------- Signed-off-by: Aaron Chong --- .../schemas/priority_description__binary.json | 12 +++++ .../src/rmf_fleet_adapter/TaskManager.cpp | 6 ++- .../agv/FleetUpdateHandle.cpp | 47 ++++++++----------- 3 files changed, 37 insertions(+), 28 deletions(-) create mode 100644 rmf_fleet_adapter/schemas/priority_description__binary.json diff --git a/rmf_fleet_adapter/schemas/priority_description__binary.json b/rmf_fleet_adapter/schemas/priority_description__binary.json new file mode 100644 index 00000000..4c86ee96 --- /dev/null +++ b/rmf_fleet_adapter/schemas/priority_description__binary.json @@ -0,0 +1,12 @@ +{ + "$schema": "https://json-schema.org/draft/2020-12/schema", + "$id": "https://raw.githubusercontent.com/open-rmf/rmf_ros2/main/rmf_fleet_adapter/schemas/priority_description__binary.json", + "title": "Binary Priority Description", + "description": "Describes the priority of a task as a binary", + "type": "object", + "properties": { + "type": { "type": "string" }, + "value": { "type": "integer", "minimum": 0, "maximum": 1 } + }, + "required": ["type", "value"] +} diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 33bec639..6f50f62f 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -421,7 +421,11 @@ void copy_booking_data( { booking_json["labels"] = booking.labels(); } - // TODO(MXG): Add priority + const auto priority = booking.priority(); + if (priority) + { + booking_json["priority"] = booking.priority()->serialize(); + } } //============================================================================== 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 72d34304..9067ff51 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -51,6 +51,7 @@ #include #include +#include #include namespace rmf_fleet_adapter { @@ -241,38 +242,30 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( const auto p_it = request_msg.find("priority"); if (p_it != request_msg.end()) { - // Assume the schema is not valid until we have successfully parsed it. - bool valid_schema = false; - // TODO(YV): Validate with priority_description_Binary.json - if (p_it->contains("type") && p_it->contains("value")) + try { - const auto& p_type = (*p_it)["type"]; - if (p_type.is_string() && p_type.get() == "binary") - { - const auto& p_value = (*p_it)["value"]; - if (p_value.is_number_integer()) - { - // The message matches the expected schema, so now we can mark it as - // valid. - valid_schema = true; + static const auto validator = + make_validator(rmf_fleet_adapter::schemas::priority_description__binary); + validator.validate(*p_it); - // If we have an integer greater than 0, we assign a high priority. - // Else the priority will default to low. - if (p_value.get() > 0) - { - priority = rmf_task::BinaryPriorityScheme::make_high_priority(); - } - } + const auto& p_value = (*p_it)["value"]; + if (p_value.get() > 0) + { + priority = rmf_task::BinaryPriorityScheme::make_high_priority(); } } - - if (!valid_schema) + catch (const std::exception& e) { - errors.push_back( - make_error_str( - 4, "Unsupported type", - "Fleet [" + name + "] does not support priority request: " - + p_it->dump() + "\nDefaulting to low binary priority.")); + const std::string error_str = make_error_str( + 4, "Unsupported type", + "Fleet [" + name + "] does not support priority request: " + + p_it->dump() + "\nDefaulting to low binary priority."); + RCLCPP_ERROR( + node->get_logger(), + "Malformed incoming priority description: %s\n%s", + e.what(), + error_str.c_str()); + errors.push_back(error_str); } } From 4d4d693afabf90c2ce95a18b81e7a6dfefa87117 Mon Sep 17 00:00:00 2001 From: cwrx777 Date: Tue, 10 Sep 2024 01:31:09 +0800 Subject: [PATCH 34/35] Update _last_active_task_time if robot is executing task (#384) Signed-off-by: Charly Wu --- .../src/rmf_fleet_adapter/agv/RobotContext.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 2fcf97dd..6aa5e4c1 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1514,10 +1514,17 @@ void RobotContext::_check_door_supervisor( { const auto now = std::chrono::steady_clock::now(); const auto dt = std::chrono::seconds(10); - if (_last_active_task_time + dt < now) + if (_current_task_id.has_value()) { - // Do not hold a door if a robot is idle for more than 10 seconds - _holding_door = std::nullopt; + _last_active_task_time = now; + } + else + { + if (_last_active_task_time + dt < now) + { + // Do not hold a door if a robot is idle for more than 10 seconds + _holding_door = std::nullopt; + } } for (const auto& door : state.all_sessions) From 16e5557fc36ff832afe04219b4b4d01e7846f31d Mon Sep 17 00:00:00 2001 From: cwrx777 Date: Tue, 17 Sep 2024 14:49:58 +0800 Subject: [PATCH 35/35] set request_time in lift end session request. (#385) Signed-off-by: Charly Wu --- rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp index 6aa5e4c1..857aad15 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/RobotContext.cpp @@ -1460,6 +1460,7 @@ void RobotContext::_check_lift_state( msg.lift_name = state.lift_name; msg.request_type = rmf_lift_msgs::msg::LiftRequest::REQUEST_END_SESSION; msg.session_id = requester_id(); + msg.request_time = _node->now(); _node->lift_request()->publish(msg); }