43 {
45 hdmap::Id id;
46 id.set_id(parking_routing_task->routing_request()
47 .parking_info()
48 .parking_space_id());
49 auto parking_space_info = hdmap_->GetParkingSpaceById(id);
50 auto request_parking_info =
51 parking_routing_task->mutable_routing_request()->mutable_parking_info();
52 if (parking_space_info == nullptr) {
53 AERROR <<
"Can not find parking space" << id_ <<
"in map";
54 return false;
55 }
56 auto points = parking_space_info->polygon().points();
57
58
59 Vec2d center_point(0, 0);
60 for (size_t i = 0; i < points.size(); i++) {
61 center_point += points[i];
62 }
63 center_point /= 4.0;
64 request_parking_info->mutable_parking_point()->set_x(center_point.x());
65 request_parking_info->mutable_parking_point()->set_y(center_point.y());
66 request_parking_info->mutable_parking_point()->set_z(0);
68 center_enu.set_x(center_point.x());
69 center_enu.set_y(center_point.y());
71 double nearest_s;
72 double nearest_l;
73 hdmap_->GetNearestLane(center_enu, &nearest_lane, &nearest_s, &nearest_l);
74 double lane_heading = nearest_lane->Heading(nearest_s);
76 lane_heading, parking_space_info->parking_space().heading());
77
78 if (std::fabs(diff_angle) < M_PI / 3.0) {
79 AINFO <<
"Find a parallel parking" << id_ <<
"lane_heading" << lane_heading
80 << "parking heading" << parking_space_info->parking_space().heading();
81 request_parking_info->set_parking_space_type(
82 ParkingSpaceType::PARALLEL_PARKING);
83 auto left_down = request_parking_info->mutable_corner_point()->add_point();
84 left_down->set_x(points[0].x());
85 left_down->set_y(points[0].y());
86 auto right_down = request_parking_info->mutable_corner_point()->add_point();
87 right_down->set_x(points[1].x());
88 right_down->set_y(points[1].y());
89 auto right_up = request_parking_info->mutable_corner_point()->add_point();
90 right_up->set_x(points[2].x());
91 right_up->set_y(points[2].y());
92 auto left_up = request_parking_info->mutable_corner_point()->add_point();
93 left_up->set_x(points[3].x());
94 left_up->set_y(points[3].y());
95 } else {
96 AINFO <<
"Find a vertical parking";
97 request_parking_info->set_parking_space_type(
98 ParkingSpaceType::VERTICAL_PLOT);
99 auto left_down = request_parking_info->mutable_corner_point()->add_point();
100 left_down->set_x(points[0].x());
101 left_down->set_y(points[0].y());
102 auto right_down = request_parking_info->mutable_corner_point()->add_point();
103 right_down->set_x(points[1].x());
104 right_down->set_y(points[1].y());
105 auto right_up = request_parking_info->mutable_corner_point()->add_point();
106 right_up->set_x(points[2].x());
107 right_up->set_y(points[2].y());
108 auto left_up = request_parking_info->mutable_corner_point()->add_point();
109 left_up->set_x(points[3].x());
110 left_up->set_y(points[3].y());
111 }
112
113 auto last_waypoint = parking_routing_task->mutable_routing_request()
114 ->mutable_waypoint()
115 ->rbegin();
116 auto extend_waypoint = parking_routing_task->mutable_routing_request()
117 ->mutable_waypoint()
118 ->Add();
119 static constexpr double kExtendParkingLength = 20;
121 extend_point.set_x(last_waypoint->pose().x() +
122 kExtendParkingLength * std::cos(lane_heading));
123 extend_point.set_y(last_waypoint->pose().y() +
124 kExtendParkingLength * std::sin(lane_heading));
125 hdmap_->GetNearestLaneWithHeading(extend_point, 20, lane_heading, M_PI_2,
126 &nearest_lane, &nearest_s, &nearest_l);
127 extend_point = nearest_lane->GetSmoothPoint(nearest_s);
128 extend_waypoint->mutable_pose()->set_x(extend_point.
x());
129 extend_waypoint->mutable_pose()->set_y(extend_point.
y());
130 extend_waypoint->set_id(nearest_lane->id().id());
131 extend_waypoint->set_s(nearest_s);
132
133 return true;
134}
static const HDMap * BaseMapPtr()
double AngleDiff(const double from, const double to)
Calculate the difference between angle from and to
std::shared_ptr< const LaneInfo > LaneInfoConstPtr