Merge branch 'main' into feat/adapi-door

Author: isamu-takagi

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp

@@ -51,6 +51,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -59,6 +60,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <deque>
 #include <map>
 #include <memory>
 #include <string>
@@ -72,6 +74,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
   using Pose = geometry_msgs::msg::Pose;
   using PoseStamped = geometry_msgs::msg::PoseStamped;
   using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using PoseArray = geometry_msgs::msg::PoseArray;
   using TransformStamped = geometry_msgs::msg::TransformStamped;
   using MarkerArray = visualization_msgs::msg::MarkerArray;
   using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
@@ -83,23 +86,23 @@ class ArTagBasedLocalizer : public rclcpp::Node
 private:
   void map_bin_callback(const HADMapBin::ConstSharedPtr & msg);
   void image_callback(const Image::ConstSharedPtr & msg);
-  void cam_info_callback(const CameraInfo & msg);
-  void ekf_pose_callback(const PoseWithCovarianceStamped & msg);
-  void publish_pose_as_base_link(const PoseStamped & sensor_to_tag, const std::string & tag_id);
-  static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker);
+  void cam_info_callback(const CameraInfo::ConstSharedPtr & msg);
+  void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  std::vector<landmark_manager::Landmark> detect_landmarks(const Image::ConstSharedPtr & msg);
+  Pose calculate_new_self_pose(
+    const std::vector<landmark_manager::Landmark> & detected_landmarks, const Pose & self_pose);
 
   // Parameters
   float marker_size_{};
   std::vector<std::string> target_tag_ids_;
   std::vector<double> base_covariance_;
-  double distance_threshold_squared_{};
+  double distance_threshold_{};
   double ekf_time_tolerance_{};
   double ekf_position_tolerance_{};
 
   // tf
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 
   // image transport
   std::unique_ptr<image_transport::ImageTransport> it_;
@@ -112,6 +115,7 @@ class ArTagBasedLocalizer : public rclcpp::Node
 
   // Publishers
   rclcpp::Publisher<MarkerArray>::SharedPtr marker_pub_;
+  rclcpp::Publisher<PoseArray>::SharedPtr pose_array_pub_;
   image_transport::Publisher image_pub_;
   rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pose_pub_;
   rclcpp::Publisher<DiagnosticArray>::SharedPtr diag_pub_;
@@ -120,7 +124,8 @@ class ArTagBasedLocalizer : public rclcpp::Node
   aruco::MarkerDetector detector_;
   aruco::CameraParameters cam_param_;
   bool cam_info_received_;
-  PoseWithCovarianceStamped latest_ekf_pose_{};
+  std::mutex self_pose_array_mtx_;
+  std::deque<PoseWithCovarianceStamped::ConstSharedPtr> self_pose_msg_ptr_array_;
   std::map<std::string, Pose> landmark_map_;
 };
 

localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp

@@ -26,11 +26,11 @@ class TruckCorrector : public VehicleCorrector
   {
     corrector_utils::CorrectionBBParameters params;
     params.min_width = 1.5;
-    params.max_width = 3.2;
+    params.max_width = 3.5;
     params.default_width = (params.min_width + params.max_width) * 0.5;
     params.min_length = 4.0;
-    params.max_length = 7.9;
-    params.default_length = (params.min_length + params.max_length) * 0.5;
+    params.max_length = 18.0;
+    params.default_length = 7.0;  // 7m is the most common length of a truck in Japan
     setParams(params);
   }
 

perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp

@@ -19,7 +19,7 @@ bool TruckFilter::filter(
   [[maybe_unused]] const geometry_msgs::msg::Pose & pose)
 {
   constexpr float min_width = 1.5;
-  constexpr float max_width = 3.2;
-  constexpr float max_length = 7.9;  // upto 12m in japanese law
+  constexpr float max_width = 3.5;
+  constexpr float max_length = 18.0;  // upto 12m in japanese law
   return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length);
 }

perception/shape_estimation/lib/filter/truck_filter.cpp

@@ -34,6 +34,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/path_safety_checker/safety_check.cpp
   src/utils/path_safety_checker/objects_filtering.cpp
   src/utils/start_goal_planner_common/utils.cpp
+  src/utils/avoidance/shift_line_generator.cpp
   src/utils/avoidance/utils.cpp
   src/utils/lane_change/utils.cpp
   src/utils/side_shift/util.cpp

planning/behavior_path_planner/CMakeLists.txt

@@ -11,14 +11,14 @@
       enable_bound_clipping: false
       enable_yield_maneuver: true
       enable_yield_maneuver_during_shifting: false
-      enable_cancel_maneuver: false
+      enable_cancel_maneuver: true
       disable_path_update: false
 
       # drivable area setting
       use_adjacent_lane: true
       use_opposite_lane: true
-      use_intersection_areas: false
-      use_hatched_road_markings: false
+      use_intersection_areas: true
+      use_hatched_road_markings: true
 
       # for debug
       publish_debug_marker: false
@@ -27,7 +27,6 @@
       # avoidance is performed for the object type with true
       target_object:
         car:
-          is_target: true                              # [-]
           execute_num: 1                               # [-]
           moving_speed_threshold: 1.0                  # [m/s]
           moving_time_threshold: 1.0                   # [s]
@@ -38,9 +37,8 @@
           safety_buffer_longitudinal: 0.0              # [m]
           use_conservative_buffer_longitudinal: true   # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
         truck:
-          is_target: true
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
@@ -49,9 +47,8 @@
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         bus:
-          is_target: true
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
@@ -60,9 +57,8 @@
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         trailer:
-          is_target: true
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
@@ -71,9 +67,8 @@
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         unknown:
-          is_target: false
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
@@ -82,9 +77,8 @@
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         bicycle:
-          is_target: false
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
@@ -93,9 +87,8 @@
           safety_buffer_longitudinal: 1.0
           use_conservative_buffer_longitudinal: true
         motorcycle:
-          is_target: false
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
@@ -104,9 +97,8 @@
           safety_buffer_longitudinal: 1.0
           use_conservative_buffer_longitudinal: true
         pedestrian:
-          is_target: false
           execute_num: 1
-          moving_speed_threshold: 1.0
+          moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
@@ -119,6 +111,16 @@
 
       # For target object filtering
       target_filtering:
+        # avoidance target type
+        target_type:
+          car: true                                      # [-]
+          truck: true                                    # [-]
+          bus: true                                      # [-]
+          trailer: true                                  # [-]
+          unknown: true                                  # [-]
+          bicycle: true                                  # [-]
+          motorcycle: true                               # [-]
+          pedestrian: true                               # [-]
         # detection range
         object_check_goal_distance: 20.0                # [m]
         # filtering parking objects
@@ -130,7 +132,7 @@
 
         # detection area generation parameters
         detection_area:
-          static: true                                   # [-]
+          static: false                                  # [-]
           min_forward_distance: 50.0                     # [m]
           max_forward_distance: 150.0                    # [m]
           backward_distance: 10.0                        # [m]
@@ -148,10 +150,20 @@
 
         # params for filtering objects that are in intersection
         intersection:
-          yaw_deviation: 0.175                           # [rad] (default: 10.0deg)
+          yaw_deviation: 0.349                           # [rad] (default 20.0deg)
 
       # For safety check
       safety_check:
+        # safety check target type
+        target_type:
+          car: true                                      # [-]
+          truck: true                                    # [-]
+          bus: true                                      # [-]
+          trailer: true                                  # [-]
+          unknown: false                                 # [-]
+          bicycle: true                                  # [-]
+          motorcycle: true                               # [-]
+          pedestrian: true                               # [-]
         # safety check configuration
         enable: true                                     # [-]
         check_current_lane: false                        # [-]
@@ -162,32 +174,32 @@
         # collision check parameters
         check_all_predicted_path: false                  # [-]
         safety_check_backward_distance: 100.0            # [m]
-        hysteresis_factor_expand_rate: 2.0               # [-]
+        hysteresis_factor_expand_rate: 1.5               # [-]
         hysteresis_factor_safe_count: 10                 # [-]
         # predicted path parameters
         min_velocity: 1.38                               # [m/s]
         max_velocity: 50.0                               # [m/s]
         time_resolution: 0.5                             # [s]
-        time_horizon_for_front_object: 10.0              # [s]
+        time_horizon_for_front_object: 3.0               # [s]
         time_horizon_for_rear_object: 10.0               # [s]
         delay_until_departure: 0.0                       # [s]
         # rss parameters
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]
         rear_vehicle_reaction_time: 2.0                  # [s]
         rear_vehicle_safety_time_margin: 1.0             # [s]
-        lateral_distance_max_threshold: 2.0              # [m]
+        lateral_distance_max_threshold: 0.75             # [m]
         longitudinal_distance_min_threshold: 3.0         # [m]
         longitudinal_velocity_delta_time: 0.8            # [s]
 
       # For avoidance maneuver
       avoidance:
         # avoidance lateral parameters
         lateral:
-          lateral_execution_threshold: 0.499            # [m]
+          lateral_execution_threshold: 0.09             # [m]
           lateral_small_shift_threshold: 0.101          # [m]
           lateral_avoid_check_threshold: 0.1            # [m]
-          soft_road_shoulder_margin: 0.8                # [m]
+          soft_road_shoulder_margin: 0.3                # [m]
           hard_road_shoulder_margin: 0.3                # [m]
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
@@ -246,13 +258,13 @@
         longitudinal:
           nominal_deceleration: -1.0                    # [m/ss]
           nominal_jerk: 0.5                             # [m/sss]
-          max_deceleration: -2.0                        # [m/ss]
+          max_deceleration: -1.5                        # [m/ss]
           max_jerk: 1.0                                 # [m/sss]
           max_acceleration: 1.0                         # [m/ss]
 
       shift_line_pipeline:
         trim:
-          quantize_filter_threshold: 0.2
+          quantize_filter_threshold: 0.1
           same_grad_filter_1_threshold: 0.1
           same_grad_filter_2_threshold: 0.2
           same_grad_filter_3_threshold: 0.5

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -7,7 +7,6 @@
       # avoidance is performed for the object type with true
       target_object:
         car:
-          is_target: true                              # [-]
           execute_num: 2                               # [-]
           moving_speed_threshold: 1.0                  # [m/s]
           moving_time_threshold: 1.0                   # [s]
@@ -16,7 +15,6 @@
           avoid_margin_lateral: 1.0                    # [m]
           safety_buffer_lateral: 0.7                   # [m]
         truck:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -25,7 +23,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         bus:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -34,7 +31,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         trailer:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -43,7 +39,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         unknown:
-          is_target: true
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -52,7 +47,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.0
         bicycle:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -61,7 +55,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
         motorcycle:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -70,7 +63,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
         pedestrian:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -80,3 +72,16 @@
           safety_buffer_lateral: 1.0
         lower_distance_for_polygon_expansion: 0.0      # [m]
         upper_distance_for_polygon_expansion: 1.0      # [m]
+
+      # For target object filtering
+      target_filtering:
+        # avoidance target type
+        target_type:
+          car: true                                    # [-]
+          truck: true                                  # [-]
+          bus: true                                    # [-]
+          trailer: true                                # [-]
+          unknown: true                                # [-]
+          bicycle: false                               # [-]
+          motorcycle: false                            # [-]
+          pedestrian: false                            # [-]