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

Fix VTS segment indices #12

Merged
merged 1 commit into from
Feb 14, 2024
Merged

Fix VTS segment indices #12

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
64 changes: 39 additions & 25 deletions trajectory_native/src/trajectory_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,41 +92,51 @@ class VehicleTrajectoryService final
const int last_waypoint_idx = segment.waypoints_size() - 1;
const vts::Waypoint *prev_waypoint;

if (segment.waypoints_size() == 0) {
response->mutable_error()->set_reason("Empty segment");
return grpc::Status::OK;
}

for (int waypoint_idx = 0; waypoint_idx < segment.waypoints_size(); waypoint_idx++) {
const vts::Waypoint &waypoint = segment.waypoints(waypoint_idx);
int total_waypoint_idx = segment_start_offset + waypoint_idx;
if (segment_start_offset > 0) {
total_waypoint_idx++;
}

if (waypoint.has_heading_constraint()) {
fmt::print("Adding pose waypoint {} ({}, {}, {})\n", segment_start_offset + waypoint_idx,
fmt::print("Adding pose waypoint {} ({}, {}, {})\n", total_waypoint_idx,
waypoint.x(), waypoint.y(), waypoint.heading_constraint());
builder.PoseWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y(),
builder.PoseWpt(total_waypoint_idx, waypoint.x(), waypoint.y(),
waypoint.heading_constraint());
} else {
fmt::print("Adding translation waypoint {} ({}, {})\n", segment_start_offset + waypoint_idx,
fmt::print("Adding translation waypoint {} ({}, {})\n", total_waypoint_idx,
waypoint.x(), waypoint.y());
builder.TranslationWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y());
builder.TranslationWpt(total_waypoint_idx, waypoint.x(), waypoint.y());
}

switch (waypoint.velocity_constraint_case()) {
case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET:
// If this is the first or last waypoint, add an implicit stop if no other constraint is added
if (waypoint_idx == 0 || waypoint_idx == last_waypoint_idx) {
if ((segment_idx == 0 && waypoint_idx == 0) ||
(segment_idx == request->segments_size() - 1 && waypoint_idx == last_waypoint_idx)) {
fmt::print("Adding implicit zero velocity constraint to waypoint {}\n",
segment_start_offset + waypoint_idx);
builder.WptZeroVelocity(segment_start_offset + waypoint_idx);
builder.WptZeroAngularVelocity(segment_start_offset + waypoint_idx);
total_waypoint_idx);
builder.WptZeroVelocity(total_waypoint_idx);
builder.WptZeroAngularVelocity(total_waypoint_idx);
}
break;
case vts::Waypoint::kZeroVelocity:
fmt::print("Adding zero velocity constraint to waypoint {}\n",
segment_start_offset + waypoint_idx);
builder.WptZeroVelocity(segment_start_offset + waypoint_idx);
builder.WptZeroAngularVelocity(segment_start_offset + waypoint_idx);
total_waypoint_idx);
builder.WptZeroVelocity(total_waypoint_idx);
builder.WptZeroAngularVelocity(total_waypoint_idx);
break;
case vts::Waypoint::kVehicleVelocity:
fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n",
waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(),
waypoint.vehicle_velocity().omega(), segment_start_offset + waypoint_idx);
builder.WptConstraint(segment_start_offset + waypoint_idx,
waypoint.vehicle_velocity().omega(), total_waypoint_idx);
builder.WptConstraint(total_waypoint_idx,
trajopt::HolonomicVelocityConstraint{
trajopt::RectangularSet2d{
{waypoint.vehicle_velocity().vx(),
Expand All @@ -136,7 +146,7 @@ class VehicleTrajectoryService final
},
trajopt::CoordinateSystem::kField
});
builder.WptConstraint(segment_start_offset + waypoint_idx,
builder.WptConstraint(total_waypoint_idx,
trajopt::AngularVelocityConstraint{
trajopt::IntervalSet1d{
waypoint.vehicle_velocity().omega(),
Expand All @@ -151,8 +161,8 @@ class VehicleTrajectoryService final
: guess_control_interval_count(waypoint,
*prev_waypoint,
request->model());
fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, waypoint_idx - 1,
waypoint_idx);
fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, total_waypoint_idx - 1,
total_waypoint_idx);
control_intervals.push_back(sample_count);
}

Expand All @@ -162,31 +172,35 @@ class VehicleTrajectoryService final
// Segment constraints
if (segment.has_max_velocity()) {
fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(),
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx);
builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx,
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
segment.max_velocity());
}

if (segment.has_max_omega()) {
fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(),
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx);
builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx,
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
trajopt::AngularVelocityConstraint{segment.max_omega()});
}

if (segment.straight_line()) {
if (segment.waypoints_size() > 2) {
response->mutable_error()->set_reason("More than two waypoints in a straight segment");
return grpc::Status::OK;
}
double x1 = segment.waypoints(0).x();
double x2 = segment.waypoints(last_waypoint_idx).x();
double x2 = segment.waypoints(1).x();
double y1 = segment.waypoints(0).y();
double y2 = segment.waypoints(last_waypoint_idx).y();
double y2 = segment.waypoints(1).y();
double angle = atan2(y2 - y1, x2 - x1);
fmt::print("Adding straight line constraint with angle {} to segment {} (waypoints {}-{})\n", angle,
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx);
builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx,
segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx + 1);
builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx + 1,
angle);
}

segment_start_offset += last_waypoint_idx + 1;
segment_start_offset += last_waypoint_idx;
}

builder.ControlIntervalCounts(std::move(control_intervals));
Expand Down
Loading