Merge branch 'main' into autoware_behavior_velocity_planner_common

.github/CODEOWNERS

@@ -14,8 +14,9 @@ demos/autoware_test_node/** mfc@autoware.org @autowarefoundation/autoware-core-g
 localization/autoware_ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 localization/autoware_localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp lxg19892021@gmail.com zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-core-global-codeowners
-planning/autoware_path_generator/** mitsuhiro.sakamoto@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners
+planning/autoware_path_generator/** kosuke.takeuchi@tier4.jp mitsuhiro.sakamoto@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 sensing/autoware_gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp lxg19892021@gmail.com masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @autowarefoundation/autoware-core-global-codeowners
+testing/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 testing/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 testing/autoware_test_utils/** egon.kang@autocore.ai kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp yukinari.hisaki.2@tier4.jp zulfaqar.azmi@tier4.jp @autowarefoundation/autoware-core-global-codeowners
 testing/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-core-global-codeowners

planning/autoware_path_generator/CMakeLists.txt

@@ -16,7 +16,7 @@ target_link_libraries(${PROJECT_NAME}
   path_generator_parameters
 )
 
-target_compile_options(${PROJECT_NAME} PRIVATE
+target_compile_options(${PROJECT_NAME} PUBLIC
   -Wno-error=deprecated-declarations
 )
 
@@ -25,4 +25,18 @@ rclcpp_components_register_node(${PROJECT_NAME}
   EXECUTABLE path_generator_node
 )
 
-ament_auto_package()
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_path_generator_node_interface.cpp
+    test/test_lanelet.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    test_route
+)

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

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
 

planning/autoware_path_generator/package.xml

@@ -21,12 +21,18 @@
   <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_test_manager</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_trajectory</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>generate_parameter_library</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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

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");

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;

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