Skip to content

Commit

Permalink
Fix VTS segment indices to apply segment constraints to the last wayp…
Browse files Browse the repository at this point in the history 
…oint of previous segments
  • Loading branch information
@camearle20
camearle20 committed Feb 14, 2024
1 parent 67f0108 commit d5a56b1
Showing 1 changed file with 39 additions and 25 deletions.
64 changes: 39 additions & 25 deletions trajectory_native/src/trajectory_service.cpp
View file
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

0 comments on commit d5a56b1

Please sign in to comment.