c.h(pr.h());
c.sp(-0.1);
c.st(0.0);
- std::vector<bcar::Pose> ispath;
+ std::vector<bcar::BicycleCar> ispath;
if (s.right()) {
while (!c.rf().on_right_side_of(s.entry())) {
c.next();
}
double elapsed = p.scnt();
auto jvo = p.json();
- unsigned int i = 0;
- for (auto p: ispath) {
- jvo["ispath"][i][0] = p.x();
- jvo["ispath"][i][1] = p.y();
- jvo["ispath"][i][2] = p.h();
- i += 1;
- }
jvo["init"] = jvi["init"];
jvo["slot"] = jvi["slot"];
jvo["obst"] = jvi["obst"];
jvo["ncnt"] = ncnt;
jvo["bcnt"] = bcnt;
jvo["path"] = best_path;
+ unsigned int best_path_size = best_path.size();
+ double best_path_gx = best_path[best_path_size - 1][0].asDouble();
+ double best_path_gy = best_path[best_path_size - 1][1].asDouble();
+ double best_path_gh = best_path[best_path_size - 1][2].asDouble();
+ // FIXME for perpendicular parking.
+ //assert(std::abs(best_path_gx - ispath[0].x()) < 1e-3
+ // && std::abs(best_path_gy - ispath[0].y()) < 1e-3
+ // && std::abs(best_path_gh - ispath[0].h()) < 1e-3);
+ unsigned int i = 0;
+ for (auto p: ispath) {
+ jvo["ispath"][i][0] = p.x();
+ jvo["ispath"][i][1] = p.y();
+ jvo["ispath"][i][2] = p.h();
+ if (i == 0) {
+ // The first pose of in-slot path is entry
+ // configuration, i.e. the last pose of the path.
+ // We just need to check if the last pose of the path
+ // (goal) is cusp.
+ int best_path_goal_sgn = bcar::sgn(
+ jvo["path"][best_path_size - 1][3].asDouble());
+ int ispath_goal_sgn = bcar::sgn(ispath[0].sp());
+ if (best_path_goal_sgn != ispath_goal_sgn) {
+ jvo["path"][best_path_size - 1][5] = true;
+ }
+ i++;
+ continue;
+ }
+ jvo["path"][best_path_size + i - 1][0] = p.x();
+ jvo["path"][best_path_size + i - 1][1] = p.y();
+ jvo["path"][best_path_size + i - 1][2] = p.h();
+ jvo["path"][best_path_size + i - 1][3] = p.sp();
+ int segment_type = 0;
+ if (p.st() > 0) {
+ segment_type = 1;
+ } else if (p.st() < 0) {
+ segment_type = -1;
+ }
+ jvo["path"][best_path_size + i - 1][4] = segment_type;
+ bool is_cusp = false;
+ if (bcar::sgn(p.sp()) != bcar::sgn(ispath[i - 1].sp())) {
+ is_cusp = true;
+ }
+ jvo["path"][best_path_size + i - 1][5] = is_cusp;
+ i++;
+ }
jvo["goal_cc"] = cost;
std::cout << jvo << std::endl;
return 0;