From 428ae199c9e55f65ca1c0a1705789deb72a2b78d Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 28 Feb 2025 14:26:53 +0900 Subject: [PATCH] feat(path_generator): add turn signal activation feature (#220) * add path_generator package Signed-off-by: mitukou1109 fix spell check error Signed-off-by: mitukou1109 include necessary headers Signed-off-by: mitukou1109 change package version to 0.0.0 Co-authored-by: Yutaka Kondo Signed-off-by: mitukou1109 fix include guard name Co-authored-by: Yutaka Kondo Signed-off-by: mitukou1109 replace flowchart uml with pre-generated image Co-authored-by: Yutaka Kondo Signed-off-by: mitukou1109 style(pre-commit): autofix Signed-off-by: mitukou1109 replace tier4_planning_msgs with autoware_internal_planning_msgs Signed-off-by: mitukou1109 style(pre-commit): autofix Signed-off-by: mitukou1109 use LaneletSequence instead of ConstLanelets Signed-off-by: mitukou1109 set orientation to path points Signed-off-by: mitukou1109 crop path bound to fit trajectory Signed-off-by: mitukou1109 offset path bound Signed-off-by: mitukou1109 no need to make return value optional Signed-off-by: mitukou1109 address deprecation warning Signed-off-by: mitukou1109 add doxygen comments Signed-off-by: mitukou1109 support multiple previous/next lanelets Signed-off-by: mitukou1109 fix path bound cut issue Signed-off-by: mitukou1109 group parameters Signed-off-by: mitukou1109 add turn signal activation feature Signed-off-by: mitukou1109 fix turn direction check process Signed-off-by: mitukou1109 consider required end point Signed-off-by: mitukou1109 keep turn signal activated until reaching desired end point if without conflicts Signed-off-by: mitukou1109 add missing parameters Signed-off-by: mitukou1109 * add include Signed-off-by: kosuke55 * use trajectory class Signed-off-by: kosuke55 * minor change Signed-off-by: kosuke55 --------- Signed-off-by: mitukou1109 Signed-off-by: kosuke55 Co-authored-by: mitukou1109 --- .../config/path_generator.param.yaml | 5 + .../include/autoware/path_generator/utils.hpp | 37 +++++ .../param/path_generator_parameters.yaml | 13 ++ planning/autoware_path_generator/src/node.cpp | 20 ++- planning/autoware_path_generator/src/node.hpp | 5 +- .../autoware_path_generator/src/utils.cpp | 133 ++++++++++++++++++ 6 files changed, 208 insertions(+), 5 deletions(-) diff --git a/planning/autoware_path_generator/config/path_generator.param.yaml b/planning/autoware_path_generator/config/path_generator.param.yaml index b6ed2383e3..7a4ed323d6 100644 --- a/planning/autoware_path_generator/config/path_generator.param.yaml +++ b/planning/autoware_path_generator/config/path_generator.param.yaml @@ -7,3 +7,8 @@ waypoint_group: separation_threshold: 1.0 interval_margin_ratio: 10.0 + turn_signal: + search_time: 3.0 + search_distance: 30.0 + resampling_interval: 1.0 + angle_threshold_deg: 15.0 diff --git a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp index 89eb19594f..0bebc1f51b 100644 --- a/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp +++ b/planning/autoware_path_generator/include/autoware/path_generator/utils.hpp @@ -18,8 +18,11 @@ #include "autoware/path_generator/common_structs.hpp" #include +#include #include +#include +#include #include #include @@ -27,6 +30,12 @@ namespace autoware::path_generator { using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + +const std::unordered_map turn_signal_command_map = { + {"left", TurnIndicatorsCommand::ENABLE_LEFT}, + {"right", TurnIndicatorsCommand::ENABLE_RIGHT}, + {"straight", TurnIndicatorsCommand::DISABLE}}; namespace utils { @@ -137,6 +146,34 @@ std::vector get_path_bound( const lanelet::CompoundLineString2d & lanelet_bound, const lanelet::CompoundLineString2d & lanelet_centerline, const double s_start, const double s_end); + +/** + * @brief get earliest turn signal based on turn direction specified for lanelets + * @param path target path + * @param planner_data planner data + * @param current_pose current pose of ego vehicle + * @param current_vel current longitudinal velocity of ego vehicle + * @param search_distance base search distance + * @param search_time time to extend search distance + * @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 angle_threshold_deg, + const double base_link_to_front); + +/** + * @brief get required end point for turn signal activation + * @param lanelet target lanelet + * @param angle_threshold_deg yaw angle difference threshold + * @return required end point + */ + +std::optional get_turn_signal_required_end_point( + const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg); } // namespace utils } // namespace autoware::path_generator diff --git a/planning/autoware_path_generator/param/path_generator_parameters.yaml b/planning/autoware_path_generator/param/path_generator_parameters.yaml index 2c7192427b..a3ac8231cd 100644 --- a/planning/autoware_path_generator/param/path_generator_parameters.yaml +++ b/planning/autoware_path_generator/param/path_generator_parameters.yaml @@ -15,3 +15,16 @@ path_generator: interval_margin_ratio: type: double + + turn_signal: + search_time: + type: double + + search_distance: + type: double + + resampling_interval: + type: double + + angle_threshold_deg: + type: double diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp index ada61f4f97..8712e0f0f2 100644 --- a/planning/autoware_path_generator/src/node.cpp +++ b/planning/autoware_path_generator/src/node.cpp @@ -51,6 +51,9 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options) path_publisher_ = create_publisher("~/output/path", 1); + turn_signal_publisher_ = + create_publisher("~/output/turn_indicators_cmd", 1); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const auto params = param_listener_->get_params(); @@ -67,12 +70,21 @@ void PathGenerator::run() return; } - const auto path = plan_path(input_data); + const auto param = param_listener_->get_params(); + const auto path = plan_path(input_data, param); if (!path) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); return; } + 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.angle_threshold_deg, + vehicle_info_.max_longitudinal_offset_m); + turn_signal.stamp = now(); + turn_signal_publisher_->publish(turn_signal); + path_publisher_->publish(*path); } @@ -182,10 +194,10 @@ bool PathGenerator::is_data_ready(const InputData & input_data) return true; } -std::optional PathGenerator::plan_path(const InputData & input_data) +std::optional PathGenerator::plan_path( + const InputData & input_data, const Params & params) { - const auto path = - generate_path(input_data.odometry_ptr->pose.pose, param_listener_->get_params()); + const auto path = generate_path(input_data.odometry_ptr->pose.pose, params); if (!path) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "output path is invalid"); diff --git a/planning/autoware_path_generator/src/node.hpp b/planning/autoware_path_generator/src/node.hpp index 7a7db18563..7f7fc40505 100644 --- a/planning/autoware_path_generator/src/node.hpp +++ b/planning/autoware_path_generator/src/node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ using autoware_internal_planning_msgs::msg::PathPointWithLaneId; using autoware_internal_planning_msgs::msg::PathWithLaneId; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::Odometry; using ::path_generator::Params; using Trajectory = autoware::trajectory::Trajectory; @@ -64,6 +66,7 @@ class PathGenerator : public rclcpp::Node // publisher rclcpp::Publisher::SharedPtr path_publisher_; + rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::TimerBase::SharedPtr timer_; @@ -82,7 +85,7 @@ class PathGenerator : public rclcpp::Node bool is_data_ready(const InputData & input_data); - std::optional plan_path(const InputData & input_data); + std::optional plan_path(const InputData & input_data, const Params & params); std::optional generate_path( const geometry_msgs::msg::Pose & current_pose, const Params & params) const; diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp index 3947a7a466..1e1c7a81fe 100644 --- a/planning/autoware_path_generator/src/utils.cpp +++ b/planning/autoware_path_generator/src/utils.cpp @@ -14,9 +14,16 @@ #include "autoware/path_generator/utils.hpp" +#include "autoware/trajectory/utils/find_intervals.hpp" + +#include +#include +#include +#include #include #include #include +#include #include @@ -294,5 +301,131 @@ std::vector get_path_bound( return 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 angle_threshold_deg, + const double base_link_to_front) +{ + TurnIndicatorsCommand turn_signal; + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + + const lanelet::BasicPoint2d current_point{current_pose.position.x, current_pose.position.y}; + const auto base_search_distance = search_distance + current_vel * search_time; + + std::vector searched_lanelet_ids = {}; + std::optional 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)) { + continue; + } + searched_lanelet_ids.push_back(lane_id); + + const auto lanelet = planner_data.lanelet_map_ptr->laneletLayer.get(lane_id); + if (!get_next_lanelet_within_route(lanelet, planner_data)) { + continue; + } + + if ( + !arc_length_from_vehicle_front_to_lanelet_start && + !lanelet::geometry::inside(lanelet, current_point)) { + continue; + } + + if (lanelet.hasAttribute("turn_direction")) { + turn_signal.command = + turn_signal_command_map.at(lanelet.attribute("turn_direction").value()); + + if (arc_length_from_vehicle_front_to_lanelet_start) { // ego is in front of lanelet + if ( + *arc_length_from_vehicle_front_to_lanelet_start > + lanelet.attributeOr("turn_signal_distance", base_search_distance)) { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + } + return turn_signal; + } + + // ego is inside lanelet + const auto required_end_point_opt = + get_turn_signal_required_end_point(lanelet, angle_threshold_deg); + if (!required_end_point_opt) continue; + if ( + calc_arc_length(lanelet, current_point) <= + calc_arc_length(lanelet, required_end_point_opt.value())) { + return turn_signal; + } + } + + const auto lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); + if (arc_length_from_vehicle_front_to_lanelet_start) { + *arc_length_from_vehicle_front_to_lanelet_start += lanelet_length; + } else { + arc_length_from_vehicle_front_to_lanelet_start = + lanelet_length - calc_arc_length(lanelet, current_point) - base_link_to_front; + } + break; + } + } + + return turn_signal; +} + +std::optional get_turn_signal_required_end_point( + const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg) +{ + std::vector 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 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; + } + + auto centerline = + autoware::trajectory::Trajectory::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