chore: sync upstream

.github/CODEOWNERS

@@ -85,22 +85,23 @@ perception/detected_object_validation/** yukihiro.saito@tier4.jp @autowarefounda
 perception/detection_by_tracker/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/euclidean_cluster/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
-perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
+perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/heatmap_visualizer/** kotaro.uetake@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners
 perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com @autowarefoundation/autoware-global-codeowners
 perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
+perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/map_based_prediction/** yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/object_merger/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/object_range_splitter/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
-perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
-perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
+perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners
+perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/shape_estimation/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp @autowarefoundation/autoware-global-codeowners
 perception/traffic_light_classifier/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -115,9 +116,9 @@ planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autow
 planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
-planning/obstacle_stop_planner/** satoshi.ota@tier4.jp @autowarefoundation/autoware-global-codeowners
+planning/obstacle_stop_planner/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners
 planning/planning_error_monitor/** yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners
@@ -136,7 +137,7 @@ sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp @autowarefoundat
 sensing/imu_corrector/** yamato.ando@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
-sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners
+sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners
 sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp @autowarefoundation/autoware-global-codeowners
 simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -20,7 +20,7 @@
 #include <autoware_adapi_v1_msgs/msg/route_state.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
-#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <autoware_planning_msgs/srv/set_route.hpp>
 
 namespace planning_interface
@@ -55,7 +55,7 @@ struct RouteState
 
 struct Route
 {
-  using Message = autoware_auto_planning_msgs::msg::HADMapRoute;
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
   static constexpr char name[] = "/planning/mission_planning/route";
   static constexpr size_t depth = 1;
   static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;

control/lane_departure_checker/README.md

@@ -49,7 +49,7 @@ This package includes the following features:
 
 - /localization/kinematic_state [`nav_msgs::msg::Odometry`]
 - /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`]
-- /planning/mission_planning/route [`autoware_auto_planning_msgs::msg::HADMapRoute`]
+- /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`]
 - /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`]
 - /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`]
 

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -21,10 +21,10 @@
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
-#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
@@ -41,10 +41,10 @@
 
 namespace lane_departure_checker
 {
-using autoware_auto_planning_msgs::msg::HADMapRoute;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::PoseDeviation;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
@@ -68,7 +68,7 @@ struct Input
   geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose{};
   nav_msgs::msg::Odometry::ConstSharedPtr current_odom{};
   lanelet::LaneletMapPtr lanelet_map{};
-  HADMapRoute::ConstSharedPtr route{};
+  LaneletRoute::ConstSharedPtr route{};
   lanelet::ConstLanelets route_lanelets{};
   Trajectory::ConstSharedPtr reference_trajectory{};
   Trajectory::ConstSharedPtr predicted_trajectory{};

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

@@ -26,8 +26,8 @@
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
-#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -59,7 +59,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_lanelet_map_bin_;
-  rclcpp::Subscription<HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_reference_trajectory_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_predicted_trajectory_;
 
@@ -69,17 +69,17 @@ class LaneDepartureCheckerNode : public rclcpp::Node
   lanelet::LaneletMapPtr lanelet_map_;
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_;
   lanelet::routing::RoutingGraphPtr routing_graph_;
-  HADMapRoute::ConstSharedPtr route_;
+  LaneletRoute::ConstSharedPtr route_;
   geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr cov_;
-  HADMapRoute::ConstSharedPtr last_route_;
+  LaneletRoute::ConstSharedPtr last_route_;
   lanelet::ConstLanelets route_lanelets_;
   Trajectory::ConstSharedPtr reference_trajectory_;
   Trajectory::ConstSharedPtr predicted_trajectory_;
 
   // Callback
   void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
   void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg);
-  void onRoute(const HADMapRoute::ConstSharedPtr msg);
+  void onRoute(const LaneletRoute::ConstSharedPtr msg);
   void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg);
   void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg);
 

control/lane_departure_checker/package.xml

@@ -15,6 +15,7 @@
 
   <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>boost</depend>
   <depend>diagnostic_updater</depend>
   <depend>eigen</depend>

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -21,7 +21,7 @@
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
-#include <autoware_auto_mapping_msgs/msg/had_map_segment.hpp>
+#include <autoware_planning_msgs/msg/lanelet_segment.hpp>
 
 #include <map>
 #include <memory>
@@ -33,7 +33,7 @@ using tier4_autoware_utils::rad2deg;
 
 namespace
 {
-using autoware_auto_mapping_msgs::msg::HADMapSegment;
+using autoware_planning_msgs::msg::LaneletSegment;
 
 std::array<geometry_msgs::msg::Point, 3> triangle2points(
   const geometry_msgs::msg::Polygon & triangle)
@@ -54,7 +54,7 @@ std::array<geometry_msgs::msg::Point, 3> triangle2points(
 lanelet::ConstLanelets getRouteLanelets(
   const lanelet::LaneletMapPtr & lanelet_map,
   const lanelet::routing::RoutingGraphPtr & routing_graph,
-  const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr & route_ptr,
+  const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
   const double vehicle_length)
 {
   lanelet::ConstLanelets route_lanelets;
@@ -159,7 +159,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   sub_lanelet_map_bin_ = this->create_subscription<HADMapBin>(
     "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(),
     std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1));
-  sub_route_ = this->create_subscription<HADMapRoute>(
+  sub_route_ = this->create_subscription<LaneletRoute>(
     "~/input/route", rclcpp::QoS{1}.transient_local(),
     std::bind(&LaneDepartureCheckerNode::onRoute, this, _1));
   sub_reference_trajectory_ = this->create_subscription<Trajectory>(
@@ -199,7 +199,7 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m
   lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_);
 }
 
-void LaneDepartureCheckerNode::onRoute(const HADMapRoute::ConstSharedPtr msg) { route_ = msg; }
+void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; }
 
 void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg)
 {

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -49,6 +49,8 @@
       longitudinal_collision_margin_min_distance: 0.0          # [m]
       longitudinal_collision_margin_time: 0.0
 
+      turn_signal_on_swerving: true # turn signal on while swerving
+
       # avoidance is performed for the object type with true
       target_object:
         car: true

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -39,6 +39,9 @@
       num_fix_points_for_extending: 50 # number of fixing points when extending
       max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m]
 
+      enable_clipping_fixed_traj: false
+      non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory
+
     object:  # avoiding object
       max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s]
       max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s]

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -209,7 +209,6 @@ def launch_setup(context, *args, **kwargs):
             obstacle_stop_planner_param,
             obstacle_stop_planner_acc_param,
             vehicle_info_param,
-            {"enable_slow_down": False},
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )

launch/tier4_system_launch/config/component_state_monitor/topics.yaml

@@ -82,7 +82,7 @@
   args:
     node_name_suffix: mission_planning_route
     topic: /planning/mission_planning/route
-    topic_type: autoware_auto_planning_msgs/msg/HADMapRoute
+    topic_type: autoware_planning_msgs/msg/LaneletRoute
     best_effort: false
     transient_local: true
     warn_rate: 0.0

localization/gyro_odometer/src/gyro_odometer_core.cpp

@@ -89,17 +89,17 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
     return;
   }
 
-  geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr =
+  geometry_msgs::msg::TransformStamped::SharedPtr tf_imu2base_ptr =
     std::make_shared<geometry_msgs::msg::TransformStamped>();
-  getTransform(output_frame_, imu_msg_ptr_->header.frame_id, tf_base2imu_ptr);
+  getTransform(output_frame_, imu_msg_ptr_->header.frame_id, tf_imu2base_ptr);
 
   geometry_msgs::msg::Vector3Stamped angular_velocity;
   angular_velocity.header = imu_msg_ptr_->header;
   angular_velocity.vector = imu_msg_ptr_->angular_velocity;
 
   geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
-  transformed_angular_velocity.header = tf_base2imu_ptr->header;
-  tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr);
+  transformed_angular_velocity.header = tf_imu2base_ptr->header;
+  tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr);
 
   // TODO(YamatoAndo) move code
   geometry_msgs::msg::TwistStamped twist;

perception/crosswalk_traffic_light_estimator/README.md

@@ -11,7 +11,7 @@
 | Name                                 | Type                                                     | Description        |
 | ------------------------------------ | -------------------------------------------------------- | ------------------ |
 | `~/input/vector_map`                 | `autoware_auto_mapping_msgs::msg::HADMapBin`             | vector map         |
-| `~/input/route`                      | `autoware_auto_planning_msgs::msg::HADMapRoute`          | route              |
+| `~/input/route`                      | `autoware_planning_msgs::msg::LaneletRoute`              | route              |
 | `~/input/classified/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | classified signals |
 
 ### Output

perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp

@@ -25,7 +25,7 @@
 #include <autoware_auto_perception_msgs/msg/traffic_light.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_signal.hpp>
 #include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
-#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <lanelet2_core/Attribute.h>
@@ -45,7 +45,7 @@ using autoware_auto_mapping_msgs::msg::HADMapBin;
 using autoware_auto_perception_msgs::msg::TrafficLight;
 using autoware_auto_perception_msgs::msg::TrafficSignal;
 using autoware_auto_perception_msgs::msg::TrafficSignalArray;
-using autoware_auto_planning_msgs::msg::HADMapRoute;
+using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_autoware_utils::DebugPublisher;
 using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::Float64Stamped;
@@ -58,7 +58,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
 
 private:
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
-  rclcpp::Subscription<HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
   rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_light_array_;
   rclcpp::Publisher<TrafficSignalArray>::SharedPtr pub_traffic_light_array_;
 
@@ -70,7 +70,7 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node
   lanelet::ConstLanelets conflicting_crosswalks_;
 
   void onMap(const HADMapBin::ConstSharedPtr msg);
-  void onRoute(const HADMapRoute::ConstSharedPtr msg);
+  void onRoute(const LaneletRoute::ConstSharedPtr msg);
   void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg);
 
   void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals);

perception/crosswalk_traffic_light_estimator/src/node.cpp

@@ -86,7 +86,7 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode(
   sub_map_ = create_subscription<HADMapBin>(
     "~/input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1));
-  sub_route_ = create_subscription<HADMapRoute>(
+  sub_route_ = create_subscription<LaneletRoute>(
     "~/input/route", rclcpp::QoS{1}.transient_local(),
     std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1));
   sub_traffic_light_array_ = create_subscription<TrafficSignalArray>(
@@ -118,7 +118,7 @@ void CrosswalkTrafficLightEstimatorNode::onMap(const HADMapBin::ConstSharedPtr m
   RCLCPP_INFO(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded");
 }
 
-void CrosswalkTrafficLightEstimatorNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
+void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg)
 {
   if (lanelet_map_ptr_ == nullptr) {
     RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map");

perception/lidar_centerpoint_tvm/.gitignore

@@ -0,0 +1 @@
+data/