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

Insert in-place rotation waypoint if missing #111

Merged
merged 8 commits into from
Dec 20, 2024
Merged

Insert in-place rotation waypoint if missing #111

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
3120bb6
Insert in-place rotation if missing
xiyuoh Nov 22, 2024
20ac53a
Fix typo
xiyuoh Dec 9, 2024
53908a2
Use references
xiyuoh Dec 11, 2024
67ae677
Remove same stack requirement
xiyuoh Dec 11, 2024
349d583
Merge branch 'main' into xiyu/insert_rotation
mxgrey Dec 19, 2024
344559c
Fix orientation test and checkpoint identification for manual turn-in…
mxgrey Dec 19, 2024
a756cfe
Tweak test debugging functions
mxgrey Dec 19, 2024
bd86f2d
Merge branch 'main' into xiyu/insert_rotation
mxgrey Dec 19, 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
2 changes: 1 addition & 1 deletion rmf_traffic/include/rmf_traffic/agv/Graph.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,7 @@ class Graph
std::optional<double> merge_radius() const;

/// Set the merge radius specific to this waypoint.
Waypoint& set_merge_radius(std::optional<double> valeu);
Waypoint& set_merge_radius(std::optional<double> value);

class Implementation;
private:
Expand Down
27 changes: 27 additions & 0 deletions rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -551,6 +551,33 @@ reconstruct_waypoints(
{
const auto& node = node_sequence.at(i);
const auto yaw = node->yaw;

// Insert in-place rotation waypoint if the current and previous waypoints
// are disconnected
const auto prev_candidate = candidates.back();
const auto prev_wp = prev_candidate.waypoint;
const auto current_wp = node->waypoint;
Copy link
Member

@luca-della-vedova luca-della-vedova Dec 10, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Nit, we should use references for more complex types to avoid copying const auto&

Copy link
Member Author

Choose a reason for hiding this comment

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

53908a2

const bool same_stack =
prev_wp.graph_index.has_value() && current_wp.has_value()
&& *prev_wp.graph_index == *current_wp;
Copy link
Member

Choose a reason for hiding this comment

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

Isn't this condition a subset of the position check? Would two waypoints that are exactly the same always return true for:

const bool same_pos = (prev_pos - node->position).norm() < 1e-3;

Copy link
Member Author

Choose a reason for hiding this comment

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

I think same graph index always mean same position, but same (or sufficiently close) positions don't always mean same graph index. Having !(same_stack || same_pos) was to include any locations that either have the same graph index, or a different graph index but sufficiently close within the threshold of 0.001m. I can see cases where two separate waypoints are added close to one another to manage the lane direction and traffic.

Copy link
Member

Choose a reason for hiding this comment

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

Yap that's what I meant, if two waypoints have the same index they will always have the exact same position, so would checking only for the position be sufficient?

Copy link
Member Author

Choose a reason for hiding this comment

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

Ah I see what you mean, gotcha removed in 67ae677

const Eigen::Vector2d prev_pos =
Eigen::Vector2d{prev_wp.position[0], prev_wp.position[1]};
const bool same_pos = (prev_pos - node->position).norm() < 1e-3;
const bool same_ori =
std::abs(prev_wp.position[2] - node->yaw)*180.0 / M_PI < 1e-2;
if (!(same_stack || same_pos) && !same_ori)
{
candidates.push_back(WaypointCandidate{
true,
Plan::Waypoint::Implementation{
Eigen::Vector3d{prev_pos[0], prev_pos[1], node->yaw},
prev_wp.time, prev_wp.graph_index,
prev_wp.approach_lanes, {}, {}, prev_wp.event, {}
},
prev_candidate.velocity
});
}

if (node->approach_lanes.empty())
{
// This means we are doing an in-place rotation or a wait
Expand Down