Skip to content

Commit

Permalink
use trajectory class
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 committed Feb 27, 2025
1 parent e7faec5 commit 7e160a3
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 55 deletions.
12 changes: 5 additions & 7 deletions planning/autoware_path_generator/include/autoware/path_generator/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,27 +155,25 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
* @param current_vel current longitudinal velocity of ego vehicle
* @param search_distance base search distance
* @param search_time time to extend search distance
* @param resampling_interval resampling interval for required end point determination
* @param angle_threshold_deg angle threshold for required end point determination
* @param base_link_to_front distance from base link to front of ego vehicle
* @return turn signal
*/
TurnIndicatorsCommand get_turn_signal(
const PathWithLaneId & path, const PlannerData & planner_data,
const geometry_msgs::msg::Pose & current_pose, const double current_vel,
const double search_distance, const double search_time, const double resampling_interval,
const double angle_threshold_deg, const double base_link_to_front);
const double search_distance, const double search_time, const double angle_threshold_deg,
const double base_link_to_front);

/**
* @brief get required end point for turn signal activation
* @param lanelet target lanelet
* @param resampling_interval centerline resampling interval
* @param angle_threshold_deg yaw angle difference threshold
* @return required end point
*/
lanelet::ConstPoint2d get_turn_signal_required_end_point(
const lanelet::ConstLanelet & lanelet, const double resampling_interval,
const double angle_threshold_deg);

std::optional<lanelet::ConstPoint2d> get_turn_signal_required_end_point(
const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg);
} // namespace utils
} // namespace autoware::path_generator

Expand Down
4 changes: 2 additions & 2 deletions planning/autoware_path_generator/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,8 @@ void PathGenerator::run()
auto turn_signal = utils::get_turn_signal(
*path, planner_data_, input_data.odometry_ptr->pose.pose,
input_data.odometry_ptr->twist.twist.linear.x, param.turn_signal.search_distance,
param.turn_signal.search_time, param.turn_signal.resampling_interval,
param.turn_signal.angle_threshold_deg, vehicle_info_.max_longitudinal_offset_m);
param.turn_signal.search_time, param.turn_signal.angle_threshold_deg,
vehicle_info_.max_longitudinal_offset_m);
turn_signal.stamp = now();
turn_signal_publisher_->publish(turn_signal);

Expand Down
110 changes: 64 additions & 46 deletions planning/autoware_path_generator/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,13 +14,16 @@

#include "autoware/path_generator/utils.hpp"

#include "autoware/trajectory/utils/find_intervals.hpp"

#include <autoware/motion_utils/constants.hpp>
#include <autoware/motion_utils/resample/resample.hpp>
#include <autoware/motion_utils/trajectory/trajectory.hpp>
#include <autoware_utils/geometry/geometry.hpp>
#include <autoware_utils/math/unit_conversion.hpp>
#include <autoware/trajectory/path_point_with_lane_id.hpp>
#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
#include <autoware_lanelet2_extension/utility/utilities.hpp>
#include <autoware_utils/geometry/geometry.hpp>
#include <autoware_utils/math/unit_conversion.hpp>

#include <lanelet2_core/geometry/Lanelet.h>

Expand Down Expand Up @@ -302,8 +305,8 @@ std::vector<geometry_msgs::msg::Point> get_path_bound(
TurnIndicatorsCommand get_turn_signal(
const PathWithLaneId & path, const PlannerData & planner_data,
const geometry_msgs::msg::Pose & current_pose, const double current_vel,
const double search_distance, const double search_time, const double resampling_interval,
const double angle_threshold_deg, const double base_link_to_front)
const double search_distance, const double search_time, const double angle_threshold_deg,
const double base_link_to_front)
{
TurnIndicatorsCommand turn_signal;
turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
Expand All @@ -314,6 +317,11 @@ TurnIndicatorsCommand get_turn_signal(
std::vector<lanelet::Id> searched_lanelet_ids = {};
std::optional<double> arc_length_from_vehicle_front_to_lanelet_start = std::nullopt;

auto calc_arc_length =
[&](const lanelet::ConstLanelet & lanelet, const lanelet::BasicPoint2d & point) -> double {
return lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), point).length;
};

for (const auto & point : path.points) {
for (const auto & lane_id : point.lane_ids) {
if (exists(searched_lanelet_ids, lane_id)) {
Expand Down Expand Up @@ -346,11 +354,12 @@ TurnIndicatorsCommand get_turn_signal(
}

// ego is inside lanelet
const auto required_end_point =
get_turn_signal_required_end_point(lanelet, resampling_interval, angle_threshold_deg);
const auto required_end_point_opt =
get_turn_signal_required_end_point(lanelet, angle_threshold_deg);
if (!required_end_point_opt) continue;
if (
lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), current_point).length <=
lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), required_end_point).length) {
calc_arc_length(lanelet, current_point) <=
calc_arc_length(lanelet, required_end_point_opt.value())) {
return turn_signal;
}
}
Expand All @@ -360,9 +369,7 @@ TurnIndicatorsCommand get_turn_signal(
*arc_length_from_vehicle_front_to_lanelet_start += lanelet_length;
} else {
arc_length_from_vehicle_front_to_lanelet_start =
lanelet_length -
lanelet::geometry::toArcCoordinates(lanelet.centerline2d(), current_point).length -
base_link_to_front;
lanelet_length - calc_arc_length(lanelet, current_point) - base_link_to_front;
}
break;
}
Expand All @@ -371,45 +378,56 @@ TurnIndicatorsCommand get_turn_signal(
return turn_signal;
}

lanelet::ConstPoint2d get_turn_signal_required_end_point(
const lanelet::ConstLanelet & lanelet, const double resampling_interval,
const double angle_threshold_deg)
std::optional<lanelet::ConstPoint2d> get_turn_signal_required_end_point(
const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg)
{
std::vector<geometry_msgs::msg::Pose> centerline(lanelet.centerline().size());
for (size_t i = 0; i < lanelet.centerline().size(); ++i) {
centerline.at(i).position = lanelet::utils::conversion::toGeomMsgPt(lanelet.centerline()[i]);
}
autoware::motion_utils::insertOrientation(centerline, true);

// Create resampling intervals
const auto lanelet_length = autoware::motion_utils::calcArcLength(centerline);
std::vector<double> resampled_arclength;
for (double s = 0.0; s < lanelet_length; s += resampling_interval) {
resampled_arclength.push_back(s);
}

// Insert terminal point
if (lanelet_length - resampled_arclength.back() < autoware::motion_utils::overlap_threshold) {
resampled_arclength.back() = lanelet_length;
} else {
resampled_arclength.push_back(lanelet_length);
}

const auto resampled_centerline =
autoware::motion_utils::resamplePoseVector(centerline, resampled_arclength);
const double terminal_yaw = tf2::getYaw(resampled_centerline.back().orientation);

auto required_end_point = resampled_centerline.back().position;
for (size_t i = 0; i < resampled_centerline.size(); ++i) {
const auto yaw = tf2::getYaw(resampled_centerline.at(i).orientation);
const auto yaw_diff = autoware_utils::normalize_radian(yaw - terminal_yaw);
if (std::fabs(yaw_diff) < autoware_utils::deg2rad(angle_threshold_deg)) {
required_end_point = resampled_centerline.at(i).position;
break;
std::vector<geometry_msgs::msg::Pose> centerline_poses(lanelet.centerline().size());
std::transform(
lanelet.centerline().begin(), lanelet.centerline().end(), centerline_poses.begin(),
[](const auto & point) {
geometry_msgs::msg::Pose pose{};
pose.position = lanelet::utils::conversion::toGeomMsgPt(point);
return pose;
});

// NOTE: Trajectory does not support less than 4 points, so resample if less than 4.
// This implementation should be replaced in the future
if (centerline_poses.size() < 4) {
const auto lanelet_length = autoware::motion_utils::calcArcLength(centerline_poses);
const auto resampling_interval = lanelet_length / 4.0;
std::vector<double> resampled_arclength;
for (double s = 0.0; s < lanelet_length; s += resampling_interval) {
resampled_arclength.push_back(s);
}
if (lanelet_length - resampled_arclength.back() < autoware::motion_utils::overlap_threshold) {
resampled_arclength.back() = lanelet_length;
} else {
resampled_arclength.push_back(lanelet_length);
}
centerline_poses =
autoware::motion_utils::resamplePoseVector(centerline_poses, resampled_arclength);
if (centerline_poses.size() < 4) return std::nullopt;
}

return lanelet::utils::conversion::toLaneletPoint(required_end_point);
auto centerline =
autoware::trajectory::Trajectory<geometry_msgs::msg::Pose>::Builder{}.build(centerline_poses);
if (!centerline) {
return std::nullopt;
}
centerline->align_orientation_with_trajectory_direction();

const auto terminal_yaw = tf2::getYaw(centerline->compute(centerline->length()).orientation);
const auto intervals = autoware::trajectory::find_intervals(
centerline.value(),
[terminal_yaw, angle_threshold_deg](const geometry_msgs::msg::Pose & point) {
const auto yaw = tf2::getYaw(point.orientation);
return std::fabs(autoware_utils::normalize_radian(yaw - terminal_yaw)) <
autoware_utils::deg2rad(angle_threshold_deg);
});
if (intervals.empty()) return std::nullopt;

return lanelet::utils::conversion::toLaneletPoint(
centerline->compute(intervals.front().start).position);
}
} // namespace utils
} // namespace autoware::path_generator

0 comments on commit 7e160a3

Please sign in to comment.