Skip to content

Commit

Permalink
Fix battery drain crash for GoToPlace (#94)
Browse files Browse the repository at this point in the history 
Signed-off-by: Yadunund <yadunund@openrobotics.org>
  • Loading branch information
@Yadunund
Yadunund committed Jul 13, 2023
1 parent 5e43b58 commit 8ede0ef
Show file tree
Hide file tree
Showing 3 changed files with 307 additions and 9 deletions.
18 changes: 12 additions & 6 deletions rmf_task_sequence/src/rmf_task_sequence/events/GoToPlace.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,16 +137,22 @@ std::optional<Estimate> GoToPlace::Model::estimate_finish(

const auto wait_until_time = arrival_time - travel->duration();
finish.time(wait_until_time + travel->duration());
auto battery_soc = finish.battery_soc().value();

if (constraints.drain_battery())
battery_soc = battery_soc - travel->change_in_charge();

finish.battery_soc(battery_soc);
{
const auto new_battery_soc =
finish.battery_soc().value() - travel->change_in_charge();
if (new_battery_soc < 0.0)
{
return std::nullopt;
}
finish.battery_soc(new_battery_soc);
}

const auto battery_threshold = constraints.threshold_soc();
if (battery_soc <= battery_threshold)
if (finish.battery_soc().value() <= constraints.threshold_soc())
{
return std::nullopt;
}

return Estimate(finish, wait_until_time);
}
Expand Down
18 changes: 15 additions & 3 deletions rmf_task_sequence/src/rmf_task_sequence/events/WaitFor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,9 +115,13 @@ WaitFor::Model::Model(
{
if (parameters.ambient_sink())
{
// Handle cases where duration is invalid.
const auto duration =
_duration.count() < 0 ? rmf_traffic::Duration(0) : _duration;

_invariant_battery_drain =
parameters.ambient_sink()->compute_change_in_charge(
rmf_traffic::time::to_seconds(_duration));
rmf_traffic::time::to_seconds(duration));
}
else
{
Expand All @@ -135,9 +139,17 @@ std::optional<Estimate> WaitFor::Model::estimate_finish(
state.time(state.time().value() + _duration);

if (constraints.drain_battery())
state.battery_soc(state.battery_soc().value() - _invariant_battery_drain);
{
const auto new_battery_soc =
state.battery_soc().value() - _invariant_battery_drain;
if (new_battery_soc < 0.0)
{
return std::nullopt;
}
state.battery_soc(new_battery_soc);
}

if (state.battery_soc().value() <= _invariant_battery_drain)
if (state.battery_soc().value() <= constraints.threshold_soc())
return std::nullopt;

return Estimate(state, earliest_arrival_time);
Expand Down
280 changes: 280 additions & 0 deletions rmf_task_sequence/test/unit/test_ComposedTaskPlanning.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <rmf_task_sequence/Event.hpp>
#include <rmf_task_sequence/events/GoToPlace.hpp>
#include <rmf_task_sequence/events/PerformAction.hpp>
#include <rmf_task_sequence/events/WaitFor.hpp>

#include <rmf_task_sequence/Task.hpp>
#include <rmf_task_sequence/phases/SimplePhase.hpp>
Expand All @@ -40,6 +41,7 @@ using TaskPlanner = rmf_task::TaskPlanner;

using PerformAction = rmf_task_sequence::events::PerformAction;
using GoToPlace = rmf_task_sequence::events::GoToPlace;
using WaitFor = rmf_task_sequence::events::WaitFor;

using BatterySystem = rmf_battery::agv::BatterySystem;
using MechanicalSystem = rmf_battery::agv::MechanicalSystem;
Expand Down Expand Up @@ -208,3 +210,281 @@ SCENARIO("Test GoToPlace and PerformAction Compose task")
}
}
}

//==============================================================================
SCENARIO("Test battery drain for events")
{
// Simple graph with just two waypoints
// 0 ---- 1
// |
// .
// |
// 2
rmf_traffic::agv::Graph graph;
const std::string map_name = "test_map";
graph.add_waypoint(map_name, {0.0, 0.0}).set_charger(true);
graph.add_waypoint(map_name, {0.0, 10.0});
graph.add_waypoint(map_name, {1e6, 0.0});

graph.add_lane(0, 1);
graph.add_lane(1, 0);
graph.add_lane(0, 2);
graph.add_lane(2, 0);

// Vehicle traits
const auto shape = rmf_traffic::geometry::make_final_convex<
rmf_traffic::geometry::Circle>(1.0);
const rmf_traffic::Profile profile{shape, shape};
const rmf_traffic::agv::VehicleTraits traits(
{1.0, 0.7}, {0.6, 0.5}, profile);
const auto default_planner_options = rmf_traffic::agv::Planner::Options{
nullptr};

auto planner = std::make_shared<rmf_traffic::agv::Planner>(
rmf_traffic::agv::Planner::Configuration{graph, traits},
default_planner_options);

// Battery and task planning parameters
const bool drain_battery = true;
auto battery_system_optional = BatterySystem::make(24.0, 40.0, 8.8);
REQUIRE(battery_system_optional);
BatterySystem& battery_system = *battery_system_optional;
auto mechanical_system_optional = MechanicalSystem::make(70.0, 40.0, 0.22);
REQUIRE(mechanical_system_optional);
MechanicalSystem& mechanical_system = *mechanical_system_optional;
auto power_system_optional = PowerSystem::make(1.0);
REQUIRE(power_system_optional);
PowerSystem& power_system_processor = *power_system_optional;
// Will consume 50% battery for the requested 1h task
auto tool_system_optional = PowerSystem::make(480.0);
REQUIRE(tool_system_optional);
PowerSystem& tool_system_processor = *tool_system_optional;

std::shared_ptr<SimpleMotionPowerSink> motion_sink =
std::make_shared<SimpleMotionPowerSink>(battery_system, mechanical_system);
std::shared_ptr<SimpleDevicePowerSink> device_sink =
std::make_shared<SimpleDevicePowerSink>(battery_system,
power_system_processor);
std::shared_ptr<SimpleDevicePowerSink> tool_sink =
std::make_shared<SimpleDevicePowerSink>(battery_system,
tool_system_processor);

const auto cost_calculator =
rmf_task::BinaryPriorityScheme::make_cost_calculator();

const rmf_task::Constraints constraints{0.2, 1.0, drain_battery};
const rmf_task::Parameters parameters{
planner,
battery_system,
motion_sink,
device_sink,
tool_sink};

const TaskPlanner::Configuration task_config{
parameters,
constraints,
cost_calculator};

// But default we use the optimal solver
const auto default_options = TaskPlanner::Options{
false,
nullptr,
nullptr};

// Common robot states and description.
// Result for both scenarios when battery is low should be the same
const auto now = std::chrono::steady_clock::now();
const double default_orientation = 0.0;
const double initial_soc = 0.3;

rmf_traffic::agv::Plan::Start first_location{now, 0, default_orientation};
rmf_task::State initial_state = rmf_task::State().load_basic(first_location,
0,
initial_soc);

const auto gotoplace_description0 =
GoToPlace::Description::make(GoToPlace::Goal(0));
auto gotoplace0_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
gotoplace_description0);
const auto gotoplace_description1 =
GoToPlace::Description::make(GoToPlace::Goal(1));
auto gotoplace1_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
gotoplace_description1);
const auto gotoplace_description2 =
GoToPlace::Description::make(GoToPlace::Goal(2));
auto gotoplace2_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
gotoplace_description2);

WHEN("Request contains a single feasible GoToPlace event")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
rmf_task_sequence::Task::Builder builder;
builder.add_phase(gotoplace0_phase, {});
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto optimal_assignments = std::get_if<
TaskPlanner::Assignments>(&optimal_result);
CHECK(optimal_assignments);
}
WHEN("Request is infeasible due to two or more GoToPlace events")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
rmf_task_sequence::Task::Builder builder;
for (std::size_t i = 0; i < 1e3; ++i)
{
builder.add_phase(gotoplace1_phase, {});
builder.add_phase(gotoplace0_phase, {});
}
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto error = std::get_if<
TaskPlanner::TaskPlannerError>(&optimal_result);
REQUIRE(error);
CHECK(*error == TaskPlanner::TaskPlannerError::limited_capacity);
}
WHEN("Request contains a single infeasible GoToPlace event")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
rmf_task_sequence::Task::Builder builder;
builder.add_phase(gotoplace2_phase, {});
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto error = std::get_if<
TaskPlanner::TaskPlannerError>(&optimal_result);
REQUIRE(error);
CHECK(*error == TaskPlanner::TaskPlannerError::limited_capacity);
}
WHEN("Request contains a single feasible WaitFor event")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
const auto waitfor_description =
WaitFor::Description::make(std::chrono::steady_clock::duration(10));
auto waitfor_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
waitfor_description);

rmf_task_sequence::Task::Builder builder;
builder.add_phase(waitfor_phase, {});
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto assignments = std::get_if<
TaskPlanner::Assignments>(&optimal_result);
CHECK(assignments);
}
WHEN("Request contains a single infeasible WaitFor event")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
const auto waitfor_description =
WaitFor::Description::make(std::chrono::steady_clock::duration::max());
auto waitfor_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
waitfor_description);

rmf_task_sequence::Task::Builder builder;
builder.add_phase(waitfor_phase, {});
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto error = std::get_if<
TaskPlanner::TaskPlannerError>(&optimal_result);
REQUIRE(error);
CHECK(*error == TaskPlanner::TaskPlannerError::limited_capacity);
}
WHEN("Request contains a WaitFor event with invalid duration")
{
TaskPlanner task_planner(task_config, default_options);

// Generate a composable request.
const auto waitfor_description =
WaitFor::Description::make(std::chrono::steady_clock::duration::min());
auto waitfor_phase =
rmf_task_sequence::phases::SimplePhase::Description::make(
waitfor_description);

rmf_task_sequence::Task::Builder builder;
builder.add_phase(waitfor_phase, {});
auto compose_task = builder.build("mock_category", "mock_tag");
rmf_task::Task::ConstBookingPtr compose_booking =
std::make_shared<const rmf_task::Task::Booking>(
"mock_id",
std::chrono::steady_clock::now(),
nullptr);
auto compose_request = std::make_shared<rmf_task::Request>(
std::move(compose_booking),
std::move(compose_task));

// Generate the plan.
const auto optimal_result = task_planner.plan(
now, {initial_state}, {compose_request});
const auto assignments = std::get_if<
TaskPlanner::Assignments>(&optimal_result);
CHECK(assignments);
}
}

0 comments on commit 8ede0ef

Please sign in to comment.