113 const trajectory_planning_msgs::msg::Trajectory& reference_trajectory,
const route_planning_msgs::msg::Route& route) {
114 const double NO_BOUNDARY_DISTANCE = 1e6;
115 constexpr double MAX_ROUTE_S_DIFFERENCE = 20.0;
116 constexpr double LOOK_BEHIND_REMAINING_ROUTE_S = 2.0;
119 std::vector<std::pair<double, double>> min_normal_distances;
120 std::vector<Eigen::Vector2d> left_boundary_points;
121 std::vector<Eigen::Vector2d> right_boundary_points;
122 std::vector<double> left_boundary_route_s;
123 std::vector<double> right_boundary_route_s;
124 std::vector<Eigen::Vector2d> left_boundary_intersections;
125 std::vector<Eigen::Vector2d> right_boundary_intersections;
128 Boundaries boundaries;
129 auto route_elements = route_planning_msgs::route_access::getRemainingRouteElements(route,
true);
130 const int ref_sample_size = trajectory_planning_msgs::trajectory_access::getSamplePointSize(reference_trajectory);
132 if (route_elements.empty()) {
134 RCLCPP_WARN(get_logger(),
"Remaining route is empty. Do not constrain boundaries.");
136 for (
int i = 0; i < ref_sample_size; ++i) {
137 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
139 return boundaries.min_normal_distances;
142 const double current_route_s = route_elements.front().s;
143 const size_t current_route_index = std::min(
static_cast<size_t>(route.current_route_element_idx), route.route_elements.size());
144 size_t overlap_begin = current_route_index;
145 while (overlap_begin > 0 && current_route_s - route.route_elements[overlap_begin - 1].s <= LOOK_BEHIND_REMAINING_ROUTE_S) {
148 route_elements.insert(route_elements.begin(), route.route_elements.begin() +
static_cast<std::ptrdiff_t
>(overlap_begin),
149 route.route_elements.begin() +
static_cast<std::ptrdiff_t
>(current_route_index));
151 boundaries.left_boundary_points.reserve(route_elements.size());
152 boundaries.right_boundary_points.reserve(route_elements.size());
153 boundaries.left_boundary_route_s.reserve(route_elements.size());
154 boundaries.right_boundary_route_s.reserve(route_elements.size());
155 boundaries.min_normal_distances.reserve(ref_sample_size);
156 boundaries.left_boundary_intersections.reserve(ref_sample_size);
157 boundaries.right_boundary_intersections.reserve(ref_sample_size);
159 for (
const auto& route_element : route_elements) {
160 if (route_element.is_enriched) {
162 route_planning_msgs::msg::LaneElement suggested_lane =
163 route_planning_msgs::route_access::getSuggestedLaneElement(route_element);
164 boundaries.left_boundary_points.emplace_back(suggested_lane.left_boundary.point.x, suggested_lane.left_boundary.point.y);
165 boundaries.right_boundary_points.emplace_back(suggested_lane.right_boundary.point.x,
166 suggested_lane.right_boundary.point.y);
167 boundaries.left_boundary_route_s.push_back(route_element.s);
168 boundaries.right_boundary_route_s.push_back(route_element.s);
170 const auto& lane_elements = route_element.lane_elements;
171 if (!lane_elements.empty()) {
172 boundaries.left_boundary_points.emplace_back(lane_elements.front().left_boundary.point.x,
173 lane_elements.front().left_boundary.point.y);
174 boundaries.right_boundary_points.emplace_back(lane_elements.back().right_boundary.point.x,
175 lane_elements.back().right_boundary.point.y);
176 boundaries.left_boundary_route_s.push_back(route_element.s);
177 boundaries.right_boundary_route_s.push_back(route_element.s);
180 boundaries.left_boundary_points.emplace_back(route_element.left_boundary.x, route_element.left_boundary.y);
181 boundaries.right_boundary_points.emplace_back(route_element.right_boundary.x, route_element.right_boundary.y);
182 boundaries.left_boundary_route_s.push_back(route_element.s);
183 boundaries.right_boundary_route_s.push_back(route_element.s);
189 auto findIntersection = [&](
const Eigen::Vector2d& ref_pos,
double sin_yaw,
double cos_yaw,
double expected_route_s,
190 const std::vector<Eigen::Vector2d>& boundary_points,
const std::vector<double>& boundary_route_s,
191 bool isLeft) -> std::pair<double, Eigen::Vector2d> {
192 std::pair<double, Eigen::Vector2d> intersection_result = {
193 std::numeric_limits<double>::infinity(),
194 Eigen::Vector2d(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity())};
195 double best_route_s_difference = std::numeric_limits<double>::infinity();
197 const Eigen::Vector2d normal_dir = isLeft ? Eigen::Vector2d(-sin_yaw, cos_yaw) : Eigen::Vector2d(sin_yaw, -cos_yaw);
198 const auto cross2d = [](
const Eigen::Vector2d& u,
const Eigen::Vector2d& v) {
return u.x() * v.y() - u.y() * v.x(); };
200 for (
size_t i = 0; i + 1 < boundary_points.size(); ++i) {
201 const Eigen::Vector2d& a = boundary_points[i];
202 const Eigen::Vector2d& b = boundary_points[i + 1];
203 Eigen::Vector2d seg = b - a;
204 Eigen::Vector2d ap = ref_pos - a;
206 const double denom = cross2d(seg, normal_dir);
207 if (std::abs(denom) < 1e-9) {
211 const double s = cross2d(ap, normal_dir) / denom;
212 const double t = cross2d(ap, seg) / denom;
214 if (s >= 0.0 && s <= 1.0 && t >= 0.0) {
215 Eigen::Vector2d intersection = a + s * seg;
216 double euclidean_distance = (ref_pos - intersection).norm();
217 const double intersection_route_s = boundary_route_s[i] + s * (boundary_route_s[i + 1] - boundary_route_s[i]);
218 const double route_s_difference = std::abs(intersection_route_s - expected_route_s);
220 if (route_s_difference <= MAX_ROUTE_S_DIFFERENCE &&
221 (route_s_difference < best_route_s_difference ||
222 (std::abs(route_s_difference - best_route_s_difference) < 1e-9 && euclidean_distance < intersection_result.first))) {
223 best_route_s_difference = route_s_difference;
224 intersection_result = {euclidean_distance, intersection};
228 return intersection_result;
232 double reference_progress = 0.0;
233 Eigen::Vector2d previous_ref_pos = Eigen::Vector2d::Zero();
234 for (
int i = 0; i < ref_sample_size; ++i) {
235 Eigen::Vector2d ref_pos(trajectory_planning_msgs::trajectory_access::getX(reference_trajectory, i),
236 trajectory_planning_msgs::trajectory_access::getY(reference_trajectory, i));
237 reference_progress += (ref_pos - previous_ref_pos).norm();
238 previous_ref_pos = ref_pos;
239 const double expected_route_s = current_route_s + reference_progress;
241 double yaw = trajectory_planning_msgs::trajectory_access::getTheta(reference_trajectory, i);
242 const double sin_yaw = std::sin(yaw);
243 const double cos_yaw = std::cos(yaw);
245 auto left_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.left_boundary_points,
246 boundaries.left_boundary_route_s,
true);
247 auto right_intersection = findIntersection(ref_pos, sin_yaw, cos_yaw, expected_route_s, boundaries.right_boundary_points,
248 boundaries.right_boundary_route_s,
false);
249 if (left_intersection.first != std::numeric_limits<double>::infinity() &&
250 right_intersection.first != std::numeric_limits<double>::infinity()) {
251 boundaries.min_normal_distances.emplace_back(left_intersection.first, right_intersection.first);
252 boundaries.left_boundary_intersections.emplace_back(left_intersection.second);
253 boundaries.right_boundary_intersections.emplace_back(right_intersection.second);
254 RCLCPP_DEBUG(this->get_logger(),
"Minimum left boundary distance: %.2f m", left_intersection.first);
255 RCLCPP_DEBUG(this->get_logger(),
"Minimum right boundary distance: %.2f m", right_intersection.first);
257 boundaries.min_normal_distances.emplace_back(NO_BOUNDARY_DISTANCE, NO_BOUNDARY_DISTANCE);
258 RCLCPP_WARN(get_logger(),
259 "No boundary intersection found for trajectory point %d. Do not constrain boundaries at this point.", i);
263 vizBoundaryPoints(boundaries.left_boundary_intersections, boundaries.right_boundary_intersections);
266 return boundaries.min_normal_distances;