Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add reached API to read-only fleet adapter #387

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Add reached API to read-only fleet adapter #387

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
14ec741
Set checkpoints and mark them as reached
xiyuoh Oct 1, 2024
4e9d7b6
Merge branch 'main' into xiyu/read_only
xiyuoh Oct 4, 2024
0e7c613
Remove unnecessary headers
xiyuoh Oct 4, 2024
12cf783
Merge branch 'main' into xiyu/read_only
mxgrey Nov 12, 2024
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
43 changes: 43 additions & 0 deletions rmf_fleet_adapter/src/read_only/FleetAdapterNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,15 @@ void FleetAdapterNode::push_route(

it->second->cumulative_delay = std::chrono::seconds(0);
it->second->route = make_route(state, _traits, it->second->sitting);
// Set checkpoints for this route
std::set<uint64_t> checkpoints;
uint64_t checkpoint_id = 0;
for (const auto& wp : it->second->route->trajectory())
{
checkpoints.insert(checkpoint_id);
checkpoint_id++;
}
it->second->route->checkpoints(checkpoints);
it->second->schedule->push_routes({*it->second->route});
}

Expand All @@ -234,6 +243,40 @@ void FleetAdapterNode::update_robot(
const RobotState& state,
const ScheduleEntries::iterator& it)
{
if (it->second->route.has_value())
{
auto& participant = it->second->schedule->participant();
uint64_t route_id = participant.itinerary().size() - 1;
uint64_t last_checkpoint_reached = participant.reached()[route_id];

uint64_t checkpoint_id = 0;
for (const auto& wp : it->second->route.value().trajectory())
{
if (checkpoint_id <= last_checkpoint_reached)
{
checkpoint_id++;
continue;
}

Eigen::Vector2d current_location =
Eigen::Vector2d(state.location.x, state.location.y);
Eigen::Vector2d checkpoint_pose =
Eigen::Vector2d(wp.position()[0], wp.position()[1]);
Eigen::Vector2d diff = current_location - checkpoint_pose;
// TODO(@xiyuoh) Make this merge_waypoint_distance configurable
if (diff.norm() < 0.3)
Copy link
Contributor

Choose a reason for hiding this comment

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

Being within some distance of the waypoint is not a robust criteria. There are two opposite issues to consider:

  1. 30cm can be a considerable distance in tight spaces. If a robot gets stopped 30cm from its goal but we report it as reached, that could trigger a waiting robot to approach prematurely, thinking that this read-only robot is completely out of the way already.
  2. If a robot manages to move through the 30cm radius before a state update takes place then we won't see that it reached the point until it reaches the next one, causing avoidable delays.

This also isn't quite the right location to identify where a robot has reached. At this point in the code we don't know if the robot is even following the same path as its last update, so we might report reaching a waypoint of an out-of-date path.

In the function handle_delay we examine whether the path of the new state update matches the path that was previously put into the traffic schedule. This is where we verify that the waypoints in the state message match the tail of the waypoints for this robot in the traffic schedule.

This point in handle_delay is where we update the delay to the previously reported path, so that's where we should be reporting the most recently reached waypoint. Since we've already verified that the new and old paths match by that point in the code, we can actually do a simple difference between the number of waypoints in the route and the number of waypoints in the remaining path as reported by the state message. It would look like:

const auto route_size = it->second->route.trajectory().size();
const auto remaining_path_size = state.path.size();
if (route_size > remaining_path_size)
{
  participant.reached(
    participant.current_plan_id(),
    0,
    route_size - remaining_path_size - 1);
}

The - 1 is because we should only report that we have "reached" points which are no longer included in the remaining path of the state message.

{
// The robot is close enough to the checkpoint, mark as reached.
participant.reached(
participant.current_plan_id(), route_id, checkpoint_id);
}
else
break;

checkpoint_id++;
}
}

if (handle_delay(state, it))
return;

Expand Down
Loading