Skip to content

Commit

Permalink
Insert in-place rotation waypoint if missing (#111)
Browse files Browse the repository at this point in the history 
* Insert in-place rotation if missing

Signed-off-by: Xiyu Oh <[email protected]>

* Fix typo

Signed-off-by: Xiyu Oh <[email protected]>

* Use references

Signed-off-by: Xiyu Oh <[email protected]>

* Remove same stack requirement

Signed-off-by: Xiyu Oh <[email protected]>

* Fix orientation test and checkpoint identification for manual turn-in-place insertions

Signed-off-by: Michael X. Grey <[email protected]>

* Tweak test debugging functions

Signed-off-by: Michael X. Grey <[email protected]>

---------

Signed-off-by: Xiyu Oh <[email protected]>
Signed-off-by: Michael X. Grey <[email protected]>
Co-authored-by: Grey <[email protected]>
  • Loading branch information
@xiyuoh @mxgrey
xiyuoh and mxgrey authored Dec 20, 2024
1 parent dc6688d commit 982bf13
Show file tree
Hide file tree
Showing 3 changed files with 140 additions and 8 deletions.
2 changes: 1 addition & 1 deletion rmf_traffic/include/rmf_traffic/agv/Graph.hpp
View file
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
94 changes: 94 additions & 0 deletions rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -280,6 +280,12 @@ void stream_trajectory(
ss << "(finished)\n";
}

//==============================================================================
inline bool same_orientation(double yaw0, double yaw1)
{
return std::abs(rmf_utils::wrap_to_pi(yaw0 - yaw1))*180.0 / M_PI < 1e-2;
}

//==============================================================================
std::vector<Plan::Waypoint> find_dependencies(
std::vector<Route>& itinerary,
Expand Down Expand Up @@ -473,6 +479,72 @@ std::vector<Plan::Waypoint> find_dependencies(
}
}

std::size_t c_last = 0;
std::size_t c = 1;
std::size_t c_next = 2;
for (; c_next < candidates.size(); ++c_last, ++c, ++c_next)
{
auto& candidate = candidates[c];
if (candidate.waypoint.arrival.empty())
{
// This was a manually inserted turn-in-place, so let's try to match it
// with a trajectory checkpoint
const auto p_candidate = candidate.waypoint.position;
for (std::size_t r = 0; r < itinerary.size(); ++r)
{
std::optional<std::size_t> floor_opt;
for (const auto& checkpoint : candidates[c_last].waypoint.arrival)
{
if (checkpoint.route_id != r)
continue;

const auto id = checkpoint.checkpoint_id;
if (!floor_opt.has_value())
floor_opt = id;
else if (*floor_opt < id)
floor_opt = id;
}

if (!floor_opt.has_value())
continue;
const auto floor_id = floor_opt.value();

std::optional<std::size_t> ceil_opt;
for (const auto& checkpoint : candidates[c_next].waypoint.arrival)
{
if (checkpoint.route_id != r)
continue;

const auto id = checkpoint.checkpoint_id;
if (!ceil_opt.has_value())
ceil_opt = id;
else if (id < *ceil_opt)
ceil_opt = id;
}

if (!ceil_opt.has_value())
continue;
const auto ceil_id = ceil_opt.value();

// Between the floor and the ceiling, try to find a waypoint in this
// trajectory that matches the position of the inserted waypoint.
for (std::size_t id = floor_id; id < ceil_id; ++id)
{
const auto& trajectory_checkpoint = itinerary[r].trajectory()[id];
const auto p_trajectory = trajectory_checkpoint.position();
const bool same_pos = (p_candidate.block<2,1>(0,0) - p_trajectory.block<2,1>(0,0)).norm() < 1e-2;
const bool same_ori = same_orientation(p_candidate[2], p_trajectory[2]);
if (same_pos && same_ori)
{
// We have a matching checkpoint
candidate.waypoint.arrival.push_back(Plan::Checkpoint { r, id });
candidate.waypoint.time = trajectory_checkpoint.time();
}
}
}
}
}

std::vector<Plan::Waypoint> waypoints;
waypoints.reserve(candidates.size());
for (const auto& c : candidates)
Expand Down Expand Up @@ -551,6 +623,28 @@ 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 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 = same_orientation(prev_wp.position[2], node->yaw);
if (!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.event, {}
},
prev_candidate.velocity
});
}

if (node->approach_lanes.empty())
{
// This means we are doing an in-place rotation or a wait
Expand Down
52 changes: 45 additions & 7 deletions rmf_traffic/test/unit/agv/test_Planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,21 +82,52 @@ void print_trajectory_info(
rmf_traffic::Time time)
{
display_path(plan);
std::cout << "Plan waypoint count: " << plan->get_waypoints().size() << std::endl;
int wp_count = 0;
for (const auto& wp : plan->get_waypoints())
{
std::cout << " wp" << wp_count++ << ": t=" << rmf_traffic::time::to_seconds(wp.time() - time)
<< "s {" << wp.position().transpose() << "}";
if (wp.graph_index())
{
std::cout << " [graph index " << *wp.graph_index() << "]";
}
else
{
std::cout << " [no graph index]";
}

std::cout << " checkpoints:";
if (wp.arrival_checkpoints().empty())
{
std::cout << " none";
}
else
{
for (const auto c : wp.arrival_checkpoints())
{
std::cout << " [" << c.route_id << ":" << c.checkpoint_id << "]";
}
}

std::cout << std::endl;
}


std::cout << "Itinerary count: " << plan->get_itinerary().size()
<< std::endl;
int trajectory_count = 1;
int trajectory_count = 0;
for (const auto& r : plan->get_itinerary())
{
int waypoint_count = 1;
int waypoint_count = 0;
const auto& t = r.trajectory();
std::cout << "Trajectory [" << trajectory_count << "] in " << r.map()
<< " with " << t.size() << " waypoints\n";
for (auto it = t.begin(); it != t.end(); it++)
{
auto position = it->position();
std::cout << " Waypoint "<< waypoint_count << ": {" << position[0]
<< ","<<position[1] << "," << position[2] << "} "
<< rmf_traffic::time::to_seconds(it->time() - time) << "s"
std::cout << " x"<< waypoint_count << ": t=" << rmf_traffic::time::to_seconds(it->time() - time)
<< "s" << " {" << position[0] << ","<<position[1] << "," << position[2] << "} "
<< std::endl;
waypoint_count++;
}
Expand Down Expand Up @@ -230,7 +261,8 @@ rmf_traffic::Trajectory test_with_obstacle(

void test_ignore_obstacle(
const rmf_traffic::agv::Planner::Result& original_result,
const rmf_traffic::schedule::Version database_version)
const rmf_traffic::schedule::Version database_version,
const bool print_info = false)
{
REQUIRE(database_version > 0);
const auto& start = original_result->get_start();
Expand All @@ -239,6 +271,12 @@ void test_ignore_obstacle(

const auto new_plan = original_result.replan(start, std::move(options));

if (print_info)
{
print_trajectory_info(original_result, original_result->get_start().time());
print_trajectory_info(new_plan, original_result->get_start().time());
}

// The new plan which ignores the conflicts should be the same as the original
REQUIRE(new_plan->get_itinerary().size() == 1);
const auto new_duration =
Expand Down Expand Up @@ -1505,7 +1543,7 @@ SCENARIO("Test planning")
}
}

SCENARIO("DP1 Graph")
SCENARIO("DP1 Graph", "[dp1_graph]")
{
using namespace std::chrono_literals;

Expand Down

0 comments on commit 982bf13

Please sign in to comment.