From afdc8b829b403c5634dfd2eec045c00b7509ab41 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Tue, 1 Dec 2020 12:36:06 +0800 Subject: [PATCH 1/3] Take into account charge depletion due to waiting to begin task --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 18 ++++++++++++++---- rmf_task/src/rmf_task/requests/Delivery.cpp | 15 ++++++++++++++- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index 404d1eec2..df6ffcf13 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -231,7 +231,7 @@ class Candidates Range range; range.begin = _value_map.begin(); auto it = range.begin; - while (it->first == range.begin->first) + while (it->first == range.begin->first && it != _value_map.end()) ++it; range.end = it; @@ -251,8 +251,18 @@ class Candidates State previous_state, bool require_charge_battery) { - const auto it = _candidate_map.at(candidate); - _value_map.erase(it); + if (candidate >= _candidate_map.size()) + { + _candidate_map.resize(candidate + 1); + } + else + { + const auto it = _candidate_map.at(candidate); + if (it != _value_map.end()) + { + _value_map.erase(it); + } + } _candidate_map[candidate] = _value_map.insert( { state.finish_time(), @@ -276,7 +286,7 @@ class Candidates { const auto c = it->second.candidate; if (_candidate_map.size() <= c) - _candidate_map.resize(c+1); + _candidate_map.resize(c+1, _value_map.end()); _candidate_map[c] = it; } diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 0f372b6a6..868e1f941 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -152,7 +152,7 @@ rmf_utils::optional Delivery::estimate_finish( variant_duration = cache_result->duration; battery_soc = battery_soc - cache_result->dsoc; } - else + else if (endpoints.first != endpoints.second) { // Compute plan to pickup waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; @@ -186,6 +186,19 @@ rmf_utils::optional Delivery::estimate_finish( initial_state.finish_time() > ideal_start ? initial_state.finish_time() : ideal_start; + + if (wait_until > initial_state.finish_time() && initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_device; + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Factor in invariants state.finish_time( wait_until + variant_duration + _pimpl->_invariant_duration); From 750e3a020ce757dc99b72b1e2d52318c70b3c0a6 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Wed, 2 Dec 2020 09:33:42 +0800 Subject: [PATCH 2/3] Add charge depletion for Loop and Clean tasks. Rename Delivery::Implementation Also modifies ChargeBattery requests to return a value even if charge is already at 100%, if a robot is not currently at a charger. --- rmf_task/src/rmf_task/agv/TaskPlanner.cpp | 19 +-- .../src/rmf_task/requests/ChargeBattery.cpp | 9 +- rmf_task/src/rmf_task/requests/Clean.cpp | 16 ++ rmf_task/src/rmf_task/requests/Delivery.cpp | 142 +++++++++--------- rmf_task/src/rmf_task/requests/Loop.cpp | 16 ++ 5 files changed, 117 insertions(+), 85 deletions(-) diff --git a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp index df6ffcf13..7f660fe10 100644 --- a/rmf_task/src/rmf_task/agv/TaskPlanner.cpp +++ b/rmf_task/src/rmf_task/agv/TaskPlanner.cpp @@ -231,7 +231,7 @@ class Candidates Range range; range.begin = _value_map.begin(); auto it = range.begin; - while (it->first == range.begin->first && it != _value_map.end()) + while (it->first == range.begin->first) ++it; range.end = it; @@ -251,18 +251,8 @@ class Candidates State previous_state, bool require_charge_battery) { - if (candidate >= _candidate_map.size()) - { - _candidate_map.resize(candidate + 1); - } - else - { - const auto it = _candidate_map.at(candidate); - if (it != _value_map.end()) - { - _value_map.erase(it); - } - } + const auto it = _candidate_map.at(candidate); + _value_map.erase(it); _candidate_map[candidate] = _value_map.insert( { state.finish_time(), @@ -286,7 +276,7 @@ class Candidates { const auto c = it->second.candidate; if (_candidate_map.size() <= c) - _candidate_map.resize(c+1, _value_map.end()); + _candidate_map.resize(c+1); _candidate_map[c] = it; } @@ -1347,7 +1337,6 @@ class TaskPlanner::Implementation // Add copies and with a newly assigned task to queue for (const auto&n : new_nodes) priority_queue.push(n); - } return nullptr; diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 8c71c850d..86886bbfc 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -112,7 +112,14 @@ rmf_utils::optional ChargeBattery::estimate_finish( const agv::StateConfig& state_config, const std::shared_ptr estimate_cache) const { - if (abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) + // Important to return nullopt if a charging task is not needed. In the task + // planner, if a charging task is added, the node's latest time may be set to + // the finishing time of the charging task, and consequently fall below the + // segmentation threshold, causing `solve` to return. This may cause an infinite + // loop as a new identical charging task is added in each call to `solve` before + // returning. + if ((abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) + && initial_state.waypoint() == initial_state.charging_waypoint()) return rmf_utils::nullopt; // Compute time taken to reach charging waypoint from current location diff --git a/rmf_task/src/rmf_task/requests/Clean.cpp b/rmf_task/src/rmf_task/requests/Clean.cpp index e7519c7b7..f0df52a83 100644 --- a/rmf_task/src/rmf_task/requests/Clean.cpp +++ b/rmf_task/src/rmf_task/requests/Clean.cpp @@ -182,6 +182,22 @@ rmf_utils::optional Clean::estimate_finish( initial_state.finish_time() > ideal_start ? initial_state.finish_time() : ideal_start; + // Factor in battery drain while waiting to move to start waypoint. If a robot + // is initially at a charging waypoint, it is assumed to be continually charging + if (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_ambient = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_ambient; + + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Factor in invariants state.finish_time( wait_until + variant_duration + _pimpl->invariant_duration + end_duration); diff --git a/rmf_task/src/rmf_task/requests/Delivery.cpp b/rmf_task/src/rmf_task/requests/Delivery.cpp index 868e1f941..176370313 100644 --- a/rmf_task/src/rmf_task/requests/Delivery.cpp +++ b/rmf_task/src/rmf_task/requests/Delivery.cpp @@ -30,20 +30,20 @@ class Delivery::Implementation Implementation() {} - std::string _id; - std::size_t _pickup_waypoint; - std::string _pickup_dispenser; - std::size_t _dropoff_waypoint; - std::string _dropoff_ingestor; - std::vector _items; - std::shared_ptr _motion_sink; - std::shared_ptr _device_sink; - std::shared_ptr _planner; - bool _drain_battery; - rmf_traffic::Time _start_time; - - rmf_traffic::Duration _invariant_duration; - double _invariant_battery_drain; + std::string id; + std::size_t pickup_waypoint; + std::string pickup_dispenser; + std::size_t dropoff_waypoint; + std::string dropoff_ingestor; + std::vector items; + std::shared_ptr motion_sink; + std::shared_ptr device_sink; + std::shared_ptr planner; + bool drain_battery; + rmf_traffic::Time start_time; + + rmf_traffic::Duration invariant_duration; + double invariant_battery_drain; }; //============================================================================== @@ -61,47 +61,47 @@ rmf_task::ConstRequestPtr Delivery::make( bool drain_battery) { std::shared_ptr delivery(new Delivery()); - delivery->_pimpl->_id = id; - delivery->_pimpl->_pickup_waypoint = pickup_waypoint; - delivery->_pimpl->_pickup_dispenser = std::move(pickup_dispenser); - delivery->_pimpl->_dropoff_waypoint = dropoff_waypoint; - delivery->_pimpl->_dropoff_ingestor = std::move(dropoff_ingestor); - delivery->_pimpl->_items = std::move(items); - delivery->_pimpl->_motion_sink = std::move(motion_sink); - delivery->_pimpl->_device_sink = std::move(device_sink); - delivery->_pimpl->_planner = std::move(planner); - delivery->_pimpl->_drain_battery = drain_battery; - delivery->_pimpl->_start_time = start_time; + delivery->_pimpl->id = id; + delivery->_pimpl->pickup_waypoint = pickup_waypoint; + delivery->_pimpl->pickup_dispenser = std::move(pickup_dispenser); + delivery->_pimpl->dropoff_waypoint = dropoff_waypoint; + delivery->_pimpl->dropoff_ingestor = std::move(dropoff_ingestor); + delivery->_pimpl->items = std::move(items); + delivery->_pimpl->motion_sink = std::move(motion_sink); + delivery->_pimpl->device_sink = std::move(device_sink); + delivery->_pimpl->planner = std::move(planner); + delivery->_pimpl->drain_battery = drain_battery; + delivery->_pimpl->start_time = start_time; // Calculate duration of invariant component of task - delivery->_pimpl->_invariant_duration = rmf_traffic::Duration{0}; - delivery->_pimpl->_invariant_battery_drain = 0.0; + delivery->_pimpl->invariant_duration = rmf_traffic::Duration{0}; + delivery->_pimpl->invariant_battery_drain = 0.0; - if (delivery->_pimpl->_pickup_waypoint != delivery->_pimpl->_dropoff_waypoint) + if (delivery->_pimpl->pickup_waypoint != delivery->_pimpl->dropoff_waypoint) { const auto plan_start_time = std::chrono::steady_clock::now(); rmf_traffic::agv::Planner::Start start{ plan_start_time, - delivery->_pimpl->_pickup_waypoint, + delivery->_pimpl->pickup_waypoint, 0.0}; - rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->_dropoff_waypoint}; - const auto result_to_dropoff = delivery->_pimpl->_planner->plan(start, goal); + rmf_traffic::agv::Planner::Goal goal{delivery->_pimpl->dropoff_waypoint}; + const auto result_to_dropoff = delivery->_pimpl->planner->plan(start, goal); const auto trajectory = result_to_dropoff->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); - delivery->_pimpl->_invariant_duration = finish_time - plan_start_time; + delivery->_pimpl->invariant_duration = finish_time - plan_start_time; - if (delivery->_pimpl->_drain_battery) + if (delivery->_pimpl->drain_battery) { // Compute battery drain const double dSOC_motion = - delivery->_pimpl->_motion_sink->compute_change_in_charge(trajectory); + delivery->_pimpl->motion_sink->compute_change_in_charge(trajectory); const double dSOC_device = - delivery->_pimpl->_device_sink->compute_change_in_charge( - rmf_traffic::time::to_seconds(delivery->_pimpl->_invariant_duration)); - delivery->_pimpl->_invariant_battery_drain = dSOC_motion + dSOC_device; + delivery->_pimpl->device_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(delivery->_pimpl->invariant_duration)); + delivery->_pimpl->invariant_battery_drain = dSOC_motion + dSOC_device; } } @@ -116,7 +116,7 @@ Delivery::Delivery() //============================================================================== std::string Delivery::id() const { - return _pimpl->_id; + return _pimpl->id; } //============================================================================== @@ -127,7 +127,7 @@ rmf_utils::optional Delivery::estimate_finish( { rmf_traffic::agv::Plan::Start final_plan_start{ initial_state.finish_time(), - _pimpl->_dropoff_waypoint, + _pimpl->dropoff_waypoint, initial_state.location().orientation()}; agv::State state{ std::move(final_plan_start), @@ -141,10 +141,11 @@ rmf_utils::optional Delivery::estimate_finish( double dSOC_motion = 0.0; double dSOC_device = 0.0; - if (initial_state.waypoint() != _pimpl->_pickup_waypoint) + // Factor in battery drain while moving to start waypoint of task + if (initial_state.waypoint() != _pimpl->pickup_waypoint) { const auto endpoints = std::make_pair(initial_state.waypoint(), - _pimpl->_pickup_waypoint); + _pimpl->pickup_waypoint); const auto& cache_result = estimate_cache->get(endpoints); // Use previously memoized values if possible if (cache_result) @@ -152,11 +153,11 @@ rmf_utils::optional Delivery::estimate_finish( variant_duration = cache_result->duration; battery_soc = battery_soc - cache_result->dsoc; } - else if (endpoints.first != endpoints.second) + else { // Compute plan to pickup waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result_to_pickup = _pimpl->_planner->plan( + const auto result_to_pickup = _pimpl->planner->plan( initial_state.location(), goal); // We assume we can always compute a plan const auto& trajectory = @@ -164,12 +165,12 @@ rmf_utils::optional Delivery::estimate_finish( const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { // Compute battery drain - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); dSOC_device = - _pimpl->_device_sink->compute_change_in_charge( + _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -181,18 +182,21 @@ rmf_utils::optional Delivery::estimate_finish( return rmf_utils::nullopt; } - const rmf_traffic::Time ideal_start = _pimpl->_start_time - variant_duration; + const rmf_traffic::Time ideal_start = _pimpl->start_time - variant_duration; const rmf_traffic::Time wait_until = initial_state.finish_time() > ideal_start ? initial_state.finish_time() : ideal_start; - - if (wait_until > initial_state.finish_time() && initial_state.waypoint() != initial_state.charging_waypoint()) + // Factor in battery drain while waiting to move to start waypoint. If a robot + // is initially at a charging waypoint, it is assumed to be continually charging + if (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) { rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + dSOC_device = _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(wait_duration)); battery_soc = battery_soc - dSOC_device; + if (battery_soc <= state_config.threshold_soc()) { return rmf_utils::nullopt; @@ -201,19 +205,19 @@ rmf_utils::optional Delivery::estimate_finish( // Factor in invariants state.finish_time( - wait_until + variant_duration + _pimpl->_invariant_duration); + wait_until + variant_duration + _pimpl->invariant_duration); - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { - battery_soc -= _pimpl->_invariant_battery_drain; + battery_soc -= _pimpl->invariant_battery_drain; if (battery_soc <= state_config.threshold_soc()) return rmf_utils::nullopt; // Check if the robot has enough charge to head back to nearest charger double retreat_battery_drain = 0.0; - if ( _pimpl->_dropoff_waypoint != state.charging_waypoint()) + if ( _pimpl->dropoff_waypoint != state.charging_waypoint()) { - const auto endpoints = std::make_pair(_pimpl->_dropoff_waypoint, + const auto endpoints = std::make_pair(_pimpl->dropoff_waypoint, state.charging_waypoint()); const auto& cache_result = estimate_cache->get(endpoints); if (cache_result) @@ -229,7 +233,7 @@ rmf_utils::optional Delivery::estimate_finish( rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result_to_charger = _pimpl->_planner->plan(start, goal); + const auto result_to_charger = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan const auto& trajectory = result_to_charger->get_itinerary().back().trajectory(); @@ -237,8 +241,8 @@ rmf_utils::optional Delivery::estimate_finish( const rmf_traffic::Duration retreat_duration = finish_time - state.finish_time(); - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge(trajectory); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge(trajectory); + dSOC_device = _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(retreat_duration)); retreat_battery_drain = dSOC_motion + dSOC_device; @@ -259,54 +263,54 @@ rmf_utils::optional Delivery::estimate_finish( //============================================================================== rmf_traffic::Duration Delivery::invariant_duration() const { - return _pimpl->_invariant_duration; + return _pimpl->invariant_duration; } //============================================================================== rmf_traffic::Time Delivery::earliest_start_time() const { - return _pimpl->_start_time; + return _pimpl->start_time; } //============================================================================== std::size_t Delivery::pickup_waypoint() const { - return _pimpl->_pickup_waypoint; + return _pimpl->pickup_waypoint; } //============================================================================== const std::string& Delivery::pickup_dispenser() const { - return _pimpl->_pickup_dispenser; + return _pimpl->pickup_dispenser; } //============================================================================== const std::string& Delivery::dropoff_ingestor() const { - return _pimpl->_dropoff_ingestor; + return _pimpl->dropoff_ingestor; } //============================================================================== std::size_t Delivery::dropoff_waypoint() const { - return _pimpl->_dropoff_waypoint; + return _pimpl->dropoff_waypoint; } //============================================================================== const std::vector& Delivery::items() const { - return _pimpl->_items; + return _pimpl->items; } //============================================================================== Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const { - if (start.waypoint() == _pimpl->_pickup_waypoint) + if (start.waypoint() == _pimpl->pickup_waypoint) return start; - rmf_traffic::agv::Planner::Goal goal{_pimpl->_pickup_waypoint}; + rmf_traffic::agv::Planner::Goal goal{_pimpl->pickup_waypoint}; - const auto result = _pimpl->_planner->plan(start, goal); + const auto result = _pimpl->planner->plan(start, goal); // We assume we can always compute a plan const auto& trajectory = result->get_itinerary().back().trajectory(); @@ -315,7 +319,7 @@ Delivery::Start Delivery::dropoff_start(const Delivery::Start& start) const rmf_traffic::agv::Planner::Start dropoff_start{ finish_time, - _pimpl->_pickup_waypoint, + _pimpl->pickup_waypoint, orientation}; return dropoff_start; diff --git a/rmf_task/src/rmf_task/requests/Loop.cpp b/rmf_task/src/rmf_task/requests/Loop.cpp index 0fd83ea8a..0a89b8137 100644 --- a/rmf_task/src/rmf_task/requests/Loop.cpp +++ b/rmf_task/src/rmf_task/requests/Loop.cpp @@ -178,6 +178,22 @@ rmf_utils::optional Loop::estimate_finish( initial_state.finish_time() > ideal_start ? initial_state.finish_time() : ideal_start; + // Factor in battery drain while waiting to move to start waypoint. If a robot + // is initially at a charging waypoint, it is assumed to be continually charging + if (_pimpl->drain_battery && wait_until > initial_state.finish_time() && + initial_state.waypoint() != initial_state.charging_waypoint()) + { + rmf_traffic::Duration wait_duration(wait_until - initial_state.finish_time()); + dSOC_device = _pimpl->ambient_sink->compute_change_in_charge( + rmf_traffic::time::to_seconds(wait_duration)); + battery_soc = battery_soc - dSOC_device; + + if (battery_soc <= state_config.threshold_soc()) + { + return rmf_utils::nullopt; + } + } + // Compute finish time const rmf_traffic::Time state_finish_time = wait_until + variant_duration + _pimpl->invariant_duration; From 16fe01ad05a25d5eaa54d9bf7f9f359371f0b856 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Wed, 2 Dec 2020 14:08:33 +0800 Subject: [PATCH 3/3] Remove leading underscores from ChargeBattery::Implementation --- .../src/rmf_task/requests/ChargeBattery.cpp | 60 +++++++++---------- 1 file changed, 30 insertions(+), 30 deletions(-) diff --git a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp index 86886bbfc..83d8f7411 100644 --- a/rmf_task/src/rmf_task/requests/ChargeBattery.cpp +++ b/rmf_task/src/rmf_task/requests/ChargeBattery.cpp @@ -54,17 +54,17 @@ class ChargeBattery::Implementation {} // fixed id for now - std::string _id = "Charge"; - rmf_battery::agv::BatterySystemPtr _battery_system; - std::shared_ptr _motion_sink; - std::shared_ptr _device_sink; - std::shared_ptr _planner; - rmf_traffic::Time _start_time; - bool _drain_battery; + std::string id = "Charge"; + rmf_battery::agv::BatterySystemPtr battery_system; + std::shared_ptr motion_sink; + std::shared_ptr device_sink; + std::shared_ptr planner; + rmf_traffic::Time start_time; + bool drain_battery; // soc to always charge the battery up to - double _charge_soc = 1.0; - rmf_traffic::Duration _invariant_duration; + double charge_soc = 1.0; + rmf_traffic::Duration invariant_duration; }; //============================================================================== @@ -77,20 +77,20 @@ rmf_task::ConstRequestPtr ChargeBattery::make( bool drain_battery) { std::shared_ptr charge_battery(new ChargeBattery()); - charge_battery->_pimpl->_id += generate_uuid(); - charge_battery->_pimpl->_battery_system = + charge_battery->_pimpl->id += generate_uuid(); + charge_battery->_pimpl->battery_system = rmf_battery::agv::BatterySystemPtr(new rmf_battery::agv::BatterySystem( battery_system.nominal_voltage(), battery_system.capacity(), battery_system.charging_current(), battery_system.type(), battery_system.profile())); - charge_battery->_pimpl->_motion_sink = std::move(motion_sink); - charge_battery->_pimpl->_device_sink = std::move(device_sink); - charge_battery->_pimpl->_planner = std::move(planner); - charge_battery->_pimpl->_start_time = start_time; - charge_battery->_pimpl->_drain_battery = drain_battery; - charge_battery->_pimpl->_invariant_duration = + charge_battery->_pimpl->motion_sink = std::move(motion_sink); + charge_battery->_pimpl->device_sink = std::move(device_sink); + charge_battery->_pimpl->planner = std::move(planner); + charge_battery->_pimpl->start_time = start_time; + charge_battery->_pimpl->drain_battery = drain_battery; + charge_battery->_pimpl->invariant_duration = rmf_traffic::time::from_seconds(0.0); return charge_battery; } @@ -103,7 +103,7 @@ ChargeBattery::ChargeBattery() //============================================================================== std::string ChargeBattery::id() const { - return _pimpl->_id; + return _pimpl->id; } //============================================================================== @@ -118,7 +118,7 @@ rmf_utils::optional ChargeBattery::estimate_finish( // segmentation threshold, causing `solve` to return. This may cause an infinite // loop as a new identical charging task is added in each call to `solve` before // returning. - if ((abs(initial_state.battery_soc() - _pimpl->_charge_soc) < 1e-3) + if ((abs(initial_state.battery_soc() - _pimpl->charge_soc) < 1e-3) && initial_state.waypoint() == initial_state.charging_waypoint()) return rmf_utils::nullopt; @@ -154,17 +154,17 @@ rmf_utils::optional ChargeBattery::estimate_finish( { // Compute plan to charging waypoint along with battery drain rmf_traffic::agv::Planner::Goal goal{endpoints.second}; - const auto result = _pimpl->_planner->plan( + const auto result = _pimpl->planner->plan( initial_state.location(), goal); const auto& trajectory = result->get_itinerary().back().trajectory(); const auto& finish_time = *trajectory.finish_time(); variant_duration = finish_time - start_time; - if (_pimpl->_drain_battery) + if (_pimpl->drain_battery) { - dSOC_motion = _pimpl->_motion_sink->compute_change_in_charge( + dSOC_motion = _pimpl->motion_sink->compute_change_in_charge( trajectory); - dSOC_device = _pimpl->_device_sink->compute_change_in_charge( + dSOC_device = _pimpl->device_sink->compute_change_in_charge( rmf_traffic::time::to_seconds(variant_duration)); battery_soc = battery_soc - dSOC_motion - dSOC_device; } @@ -179,18 +179,18 @@ rmf_utils::optional ChargeBattery::estimate_finish( } // Default _charge_soc = 1.0 - double delta_soc = _pimpl->_charge_soc - battery_soc; + double delta_soc = _pimpl->charge_soc - battery_soc; assert(delta_soc >= 0.0); double time_to_charge = - (3600 * delta_soc * _pimpl->_battery_system->capacity()) / - _pimpl->_battery_system->charging_current(); + (3600 * delta_soc * _pimpl->battery_system->capacity()) / + _pimpl->battery_system->charging_current(); const rmf_traffic::Time wait_until = initial_state.finish_time(); state.finish_time( wait_until + variant_duration + rmf_traffic::time::from_seconds(time_to_charge)); - state.battery_soc(_pimpl->_charge_soc); + state.battery_soc(_pimpl->charge_soc); return Estimate(state, wait_until); } @@ -198,19 +198,19 @@ rmf_utils::optional ChargeBattery::estimate_finish( //============================================================================== rmf_traffic::Duration ChargeBattery::invariant_duration() const { - return _pimpl->_invariant_duration; + return _pimpl->invariant_duration; } //============================================================================== rmf_traffic::Time ChargeBattery::earliest_start_time() const { - return _pimpl->_start_time; + return _pimpl->start_time; } //============================================================================== const rmf_battery::agv::BatterySystem& ChargeBattery::battery_system() const { - return *_pimpl->_battery_system; + return *_pimpl->battery_system; } } // namespace requests