Skip to content

Commit

Permalink
Merge pull request #12 from Mechanical-Advantage/fix-vts-segment-indices
Browse files Browse the repository at this point in the history 
Fix VTS segment indices
  • Loading branch information
@camearle20
camearle20 authored Feb 14, 2024
2 parents 67f0108 + d5a56b1 commit 1ae9f51
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 1ae9f51

Please sign in to comment.