Skip to content
This repository was archived by the owner on Jul 22, 2021. It is now read-only.

Deal with implicit charging before first assignment #209

Merged
Yadunund merged 1 commit into from
Nov 6, 2020
Merged

Deal with implicit charging before first assignment #209

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 50 additions & 9 deletions rmf_task/src/rmf_task/agv/TaskPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,8 @@ class Candidates
std::size_t candidate;
State state;
rmf_traffic::Time wait_until;
State previous_state;
bool require_charge_battery = false;
};

// Map finish time to Entry
Expand Down Expand Up @@ -243,14 +245,16 @@ class Candidates
void update_candidate(
std::size_t candidate,
State state,
rmf_traffic::Time wait_until)
rmf_traffic::Time wait_until,
State previous_state,
bool require_charge_battery)
{
const auto it = _candidate_map.at(candidate);
_value_map.erase(it);
_candidate_map[candidate] = _value_map.insert(
{
state.finish_time(),
Entry{candidate, state, wait_until}
Entry{candidate, state, wait_until, previous_state, require_charge_battery}
});
}

Expand Down Expand Up @@ -293,7 +297,12 @@ Candidates Candidates::make(
{
initial_map.insert({
finish.value().finish_state().finish_time(),
Entry{i, finish.value().finish_state(), finish.value().wait_until()}});
Entry{
i,
finish.value().finish_state(),
finish.value().wait_until(),
state,
false}});
}
else
{
Expand All @@ -306,7 +315,12 @@ Candidates Candidates::make(
assert(new_finish.has_value());
initial_map.insert(
{new_finish.value().finish_state().finish_time(),
Entry{i, new_finish.value().finish_state(), new_finish.value().wait_until()}});
Entry{
i,
new_finish.value().finish_state(),
new_finish.value().wait_until(),
Copy link
Copy Markdown
Contributor

@mrushyendra mrushyendra Nov 6, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here, the wait_until time of the entry with the charging task + original task is set to the time of the original task. I was wondering if that may be an issue in this line, where sometimes it might be more efficient to add a charging task much before we add the original task.

I think expand_charger should automatically take care of such cases, but I'm not a 100% sure, so just wanted confirmation.

Copy link
Copy Markdown
Member Author

@Yadunund Yadunund Nov 6, 2020

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I think expand_charger() should account for such cases.

state,
true}});
}
else
{
Expand Down Expand Up @@ -865,6 +879,7 @@ class TaskPlanner::Implementation

{
const auto& entry = it->second;
const auto& state_config = state_configs[entry.candidate];

if (parent->latest_time + segmentation_threshold < entry.wait_until)
{
Expand All @@ -875,15 +890,35 @@ class TaskPlanner::Implementation

auto new_node = std::make_shared<Node>(*parent);

// Assign the unassigned task
// Assign the unassigned task after checking for implicit charging requests
if (entry.require_charge_battery)
{
// Check if a battery task already precedes the latest assignment
auto& assignments = new_node->assigned_tasks[entry.candidate];
if (assignments.empty() || !std::dynamic_pointer_cast<const rmf_task::requests::ChargeBattery>(
assignments.back().request()))
{
auto charge_battery = make_charging_request(entry.previous_state.finish_time());
auto battery_estimate = charge_battery->estimate_finish(entry.previous_state, state_config);
if (battery_estimate.has_value())
{
assignments.push_back(
Assignment
{
charge_battery,
battery_estimate.value().finish_state(),
battery_estimate.value().wait_until()
});
}
}
}
new_node->assigned_tasks[entry.candidate].push_back(
Assignment{u.second.request, entry.state, entry.wait_until});

// Erase the assigned task from unassigned tasks
new_node->pop_unassigned(u.first);

// Update states of unassigned tasks for the candidate
const auto& state_config = state_configs[entry.candidate];
bool add_charger = false;
for (auto& new_u : new_node->unassigned_tasks)
{
Expand All @@ -896,7 +931,9 @@ class TaskPlanner::Implementation
new_u.second.candidates.update_candidate(
entry.candidate,
finish.value().finish_state(),
finish.value().wait_until());
finish.value().wait_until(),
entry.state,
false);
}
else
{
Expand Down Expand Up @@ -946,7 +983,7 @@ class TaskPlanner::Implementation
if (finish.has_value())
{
new_u.second.candidates.update_candidate(
entry.candidate, finish.value().finish_state(), finish.value().wait_until());
entry.candidate, finish.value().finish_state(), finish.value().wait_until(), entry.state, false);
}
else
{
Expand Down Expand Up @@ -1016,7 +1053,11 @@ class TaskPlanner::Implementation
if (finish.has_value())
{
new_u.second.candidates.update_candidate(
agent, finish.value().finish_state(), finish.value().wait_until());
agent,
finish.value().finish_state(),
finish.value().wait_until(),
state,
false);
}
else
{
Expand Down