Skip to content

Commit

Permalink
Merge branch 'main' into luca/export_targets
Browse files Browse the repository at this point in the history
  • Loading branch information
luca-della-vedova authored Dec 18, 2024
2 parents 14b16a5 + 9ea5a5b commit aa2f88f
Showing 1 changed file with 27 additions and 7 deletions.
34 changes: 27 additions & 7 deletions rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1055,9 +1055,21 @@ void EasyCommandHandle::follow_new_path(
for (std::size_t i = 0; i < waypoints.size(); ++i)
{
const auto& wp = waypoints[i];
if (wp.graph_index().has_value())
for (const auto& l : current_location)
{
for (const auto& l : current_location)
if (!nav_params->skip_rotation_commands)
{
// If the waypoint and robot orientations are not aligned with
// skip rotation command set to False, we will not consider this as
// a connection
if (std::abs(wp.position()[2] -
l.orientation())*180.0 / M_PI > 1e-2)
{
continue;
}
}

if (wp.graph_index().has_value())
{
if (nav_params->in_same_stack(*wp.graph_index(),
l.waypoint()) && !l.lane().has_value())
Expand All @@ -1073,7 +1085,7 @@ void EasyCommandHandle::follow_new_path(
{
const double dist =
(*l.location() - wp.position().block<2, 1>(0, 0)).norm();
if (dist <= nav_params->max_merge_lane_distance)
if (dist <= nav_params->max_merge_waypoint_distance)
{
found_connection = true;
i0 = 0;
Expand Down Expand Up @@ -1108,10 +1120,7 @@ void EasyCommandHandle::follow_new_path(
}
}
}
}
else
{
for (const auto& l : current_location)
else
{
Eigen::Vector2d p_l;
if (l.location().has_value())
Expand All @@ -1137,6 +1146,17 @@ void EasyCommandHandle::follow_new_path(
{
for (const auto& l : current_location)
{
if (!nav_params->skip_rotation_commands)
{
// If the waypoint and robot orientations are not aligned with
// skip rotation command set to False, we will not consider this as
// a connection
if (std::abs(wp.position()[2] -
l.orientation())*180.0 / M_PI > 1e-2)
{
continue;
}
}
if (l.lane().has_value())
{
if (lane == *l.lane())
Expand Down

0 comments on commit aa2f88f

Please sign in to comment.