feat(path_generator): add turn signal activation feature (#220)

* add path_generator package Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix spell check error Signed-off-by: mitukou1109 <mitukou1109@gmail.com> include necessary headers Signed-off-by: mitukou1109 <mitukou1109@gmail.com> change package version to 0.0.0 Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix include guard name Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> replace flowchart uml with pre-generated image Co-authored-by: Yutaka Kondo <yutaka.kondo@youtalk.jp> Signed-off-by: mitukou1109 <mitukou1109@gmail.com> style(pre-commit): autofix Signed-off-by: mitukou1109 <mitukou1109@gmail.com> replace tier4_planning_msgs with autoware_internal_planning_msgs Signed-off-by: mitukou1109 <mitukou1109@gmail.com> style(pre-commit): autofix Signed-off-by: mitukou1109 <mitukou1109@gmail.com> use LaneletSequence instead of ConstLanelets Signed-off-by: mitukou1109 <mitukou1109@gmail.com> set orientation to path points Signed-off-by: mitukou1109 <mitukou1109@gmail.com> crop path bound to fit trajectory Signed-off-by: mitukou1109 <mitukou1109@gmail.com> offset path bound Signed-off-by: mitukou1109 <mitukou1109@gmail.com> no need to make return value optional Signed-off-by: mitukou1109 <mitukou1109@gmail.com> address deprecation warning Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add doxygen comments Signed-off-by: mitukou1109 <mitukou1109@gmail.com> support multiple previous/next lanelets Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix path bound cut issue Signed-off-by: mitukou1109 <mitukou1109@gmail.com> group parameters Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add turn signal activation feature Signed-off-by: mitukou1109 <mitukou1109@gmail.com> fix turn direction check process Signed-off-by: mitukou1109 <mitukou1109@gmail.com> consider required end point Signed-off-by: mitukou1109 <mitukou1109@gmail.com> keep turn signal activated until reaching desired end point if without conflicts Signed-off-by: mitukou1109 <mitukou1109@gmail.com> add missing parameters Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add include Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * use trajectory class Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * minor change Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> --------- Signed-off-by: mitukou1109 <mitukou1109@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: mitukou1109 <mitukou1109@gmail.com>
autowarefoundation · Feb 28, 2025 · 428ae19 · 428ae19
1 parent 1860a40
commit 428ae19
Show file tree

Hide file tree

Showing 6 changed files with 208 additions and 5 deletions.
diff --git 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
@@ -18,15 +18,24 @@
 #include "autoware/path_generator/common_structs.hpp"
 
 #include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 
 #include <optional>
+#include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
 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<std::string, uint8_t> turn_signal_command_map = {
+  {"left", TurnIndicatorsCommand::ENABLE_LEFT},
+  {"right", TurnIndicatorsCommand::ENABLE_RIGHT},
+  {"straight", TurnIndicatorsCommand::DISABLE}};
 
 namespace utils
 {
@@ -137,6 +146,34 @@ std::vector<geometry_msgs::msg::Point> 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<lanelet::ConstPoint2d> 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
@@ -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
@@ -51,6 +51,9 @@ PathGenerator::PathGenerator(const rclcpp::NodeOptions & node_options)
 
   path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
 
+  turn_signal_publisher_ =
+    create_publisher<TurnIndicatorsCommand>("~/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<PathWithLaneId> PathGenerator::plan_path(const InputData & input_data)
+std::optional<PathWithLaneId> 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
@@ -25,6 +25,7 @@
 #include <autoware_internal_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <autoware_vehicle_msgs/msg/turn_indicators_command.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
 #include <memory>
@@ -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<PathPointWithLaneId>;
@@ -64,6 +66,7 @@ class PathGenerator : public rclcpp::Node
 
   // publisher
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
+  rclcpp::Publisher<TurnIndicatorsCommand>::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<PathWithLaneId> plan_path(const InputData & input_data);
+  std::optional<PathWithLaneId> plan_path(const InputData & input_data, const Params & params);
 
   std::optional<PathWithLaneId> 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
@@ -14,9 +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/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>
 
@@ -294,5 +301,131 @@ std::vector<geometry_msgs::msg::Point> 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<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)) {
+        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<lanelet::ConstPoint2d> get_turn_signal_required_end_point(
+  const lanelet::ConstLanelet & lanelet, const double angle_threshold_deg)
+{
+  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;
+  }
+
+  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