Merge branch 'main' into 500-fix-behavior-velocity-add-missing-intepo…

…lated-last-point
autowarefoundation · Mar 10, 2022 · 56fd4ad · 56fd4ad
2 parents 3bcee0b + 6e14982
commit 56fd4ad
Show file tree

Hide file tree

Showing 13 changed files with 99 additions and 53 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -37,6 +37,7 @@
     <push-ros-namespace namespace="euclidean"/>
     <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
       <arg name="output_clusters" value="lidar/clusters"/>
+      <arg name="use_pointcloud_map" value="false"/>
     </include>
     <include file="$(find-pkg-share roi_cluster_fusion)/launch/roi_cluster_fusion.launch.xml">
       <arg name="input/camera_info0" value="$(var camera_info0)"/>
@@ -68,10 +69,8 @@
       <arg name="output" value="camera_lidar_fusion/clusters"/>
     </include>
     <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
-      <arg name="use_map_current" value="$(var use_vector_map)"/>
       <arg name="input/objects" value="camera_lidar_fusion/clusters" />
       <arg name="output/objects" value="camera_lidar_fusion/objects_with_feature" />
-      <arg name="orientation_reliable" value="false"/>
     </include>
     <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
     <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
@@ -86,25 +85,48 @@
     </include>
   </group>
 
+  <!-- CenterPoint -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='centerpoint'&quot;)">
     <push-ros-namespace namespace="centerpoint"/>
     <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
       <arg name="output/objects" value="objects" />
     </include>
   </group>
 
+  <!-- Lidar Apollo Instance Segmentation -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='apollo'&quot;)">
     <push-ros-namespace namespace="apollo"/>
-    <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
-    <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
-      <arg name="output/objects" value="objects"/>
-      <arg name="use_map_current" value="$(var use_vector_map)"/>
+    <group>
+      <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
+      <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
+        <arg name="output/objects" value="objects_with_feature"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
+      </include>
+      <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
+      <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
+        <arg name="input" value="objects_with_feature" />
+        <arg name="output" value="objects" />
+      </include>
+    </group>
+  </group>
+
+  <!-- DetectionByTracker -->
+  <group>
+    <push-ros-namespace namespace="detection_by_tracker"/>
+    <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
     </include>
   </group>
 
+  <!-- Merger -->
   <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
     <arg name="input/object0" value="$(var lidar_detection_model)/objects"/>
     <arg name="input/object1" value="euclidean/camera_lidar_fusion/short_range_objects"/>
+    <arg name="output/object" value="camera_lidar_fusion/objects"/>
+  </include>
+  <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
+    <arg name="input/object0" value="detection_by_tracker/objects"/>
+    <arg name="input/object1" value="camera_lidar_fusion/objects"/>
     <arg name="output/object" value="objects"/>
   </include>
+
 </launch>
diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -33,7 +33,7 @@
       <include file="$(find-pkg-share lidar_apollo_instance_segmentation)/launch/lidar_apollo_instance_segmentation.launch.xml" />
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="output/objects" value="objects_with_feature"/>
-        <arg name="use_map_current" value="$(var use_vector_map)"/>
+        <arg name="use_vehicle_reference_yaw" value="true"/>
       </include>
       <!-- convert DynamicObjectsWithFeatureArray to DynamicObjects -->
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">

diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/...nning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
@@ -45,3 +45,6 @@
       # not enabled yet
       longitudinal_collision_margin_min_distance: 0.0          # [m]
       longitudinal_collision_margin_time: 0.0
+
+      # ---------- advanced parameters ----------
+      avoidance_execution_lateral_threshold: 0.499
diff --git a/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp b/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp
@@ -47,7 +47,7 @@ Debugger::Debugger(rclcpp::Node * node, const int camera_num) : node_(node)
   for (int id = 0; id < camera_num; ++id) {
     auto sub = image_transport::create_subscription(
       node, "input/image_raw" + std::to_string(id),
-      boost::bind(&Debugger::imageCallback, this, _1, id), "raw");
+      boost::bind(&Debugger::imageCallback, this, _1, id), "raw", rmw_qos_profile_sensor_data);
     image_subs_.push_back(sub);
     if (node->has_parameter("format")) {
       node->undeclare_parameter("format");

diff --git a/perception/shape_estimation/launch/shape_estimation.launch.xml b/perception/shape_estimation/launch/shape_estimation.launch.xml
@@ -2,15 +2,15 @@
 
   <arg name="input/objects" default="labeled_clusters"/>
   <arg name="output/objects" default="shape_estimated_objects"/>
-  <arg name="use_map_current" default="true"/>
+  <arg name="use_filter" default="true"/>
   <arg name="use_corrector" default="true"/>
   <arg name="node_name" default="shape_estimation"/>
-  <arg name="use_vehicle_reference_yaw" default="true"/>
+  <arg name="use_vehicle_reference_yaw" default="false"/>
   <node pkg="shape_estimation" exec="shape_estimation" name="$(var node_name)" output="screen">
     <remap from="input" to="$(var input/objects)" />
     <remap from="objects" to="$(var output/objects)" />
+    <param name="use_filter" value="$(var use_filter)" />
     <param name="use_corrector" value="$(var use_corrector)" />
-    <param name="use_map_current" value="$(var use_map_current)" />
     <param name="use_vehicle_reference_yaw" value="$(var use_vehicle_reference_yaw)"/>
   </node>
 </launch>
diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md
@@ -388,24 +388,27 @@ TODO
 
 ### Avoidance path generation
 
-| Name                                       | Unit   | Type   | Description                                                                                                 | Default value |
-| :----------------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
-| resample_interval_for_planning             | [m]    | double | Path resample interval for avoidance planning path.                                                         | 0.3           |
-| resample_interval_for_output               | [m]    | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 3.0           |
-| lateral_collision_margin                   | [m]    | double | The lateral distance between ego and avoidance targets.                                                     | 1.5           |
-| lateral_collision_safety_buffer            | [m]    | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle                | 0.5           |
-| longitudinal_collision_margin_min_distance | [m]    | double | when complete avoidance motion, there is a distance margin with the object for longitudinal direction.      | 0.0           |
-| longitudinal_collision_margin_time         | [s]    | double | when complete avoidance motion, there is a time margin with the object for longitudinal direction.          | 0.0           |
-| prepare_time                               | [s]    | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.               | 1.0           |
-| min_prepare_distance                       | [m]    | double | Minimum distance for "prepare_time" x "ego_speed".                                                          | 1.0           |
-| nominal_lateral_jerk                       | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego.                          | 0.3           |
-| max_lateral_jerk                           | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego.                 | 2.0           |
-| min_avoidance_distance                     | [m]    | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low)      | 10.0          |
-| min_nominal_avoidance_speed                | [m/s]  | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                            | 5.0           |
-| min_sharp_avoidance_speed                  | [m/s]  | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                              | 1.0           |
-| max_right_shift_length                     | [m]    | double | Maximum shift length for right direction                                                                    | 5.0           |
-| max_left_shift_length                      | [m]    | double | Maximum shift length for left direction                                                                     | 5.0           |
-| road_shoulder_safety_margin                | [m]    | double | Prevents the generated path to come too close to the road shoulders.                                        | 0.5           |
+| Name                                       | Unit   | Type   | Description                                                                                                                 | Default value |
+| :----------------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| resample_interval_for_planning             | [m]    | double | Path resample interval for avoidance planning path.                                                                         | 0.3           |
+| resample_interval_for_output               | [m]    | double | Path resample interval for output path. Too short interval increases computational cost for latter modules.                 | 3.0           |
+| lateral_collision_margin                   | [m]    | double | The lateral distance between ego and avoidance targets.                                                                     | 1.5           |
+| lateral_collision_safety_buffer            | [m]    | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle                                | 0.5           |
+| longitudinal_collision_margin_min_distance | [m]    | double | when complete avoidance motion, there is a distance margin with the object for longitudinal direction.                      | 0.0           |
+| longitudinal_collision_margin_time         | [s]    | double | when complete avoidance motion, there is a time margin with the object for longitudinal direction.                          | 0.0           |
+| prepare_time                               | [s]    | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.                               | 1.0           |
+| min_prepare_distance                       | [m]    | double | Minimum distance for "prepare_time" x "ego_speed".                                                                          | 1.0           |
+| nominal_lateral_jerk                       | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego.                                          | 0.3           |
+| max_lateral_jerk                           | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego.                                 | 2.0           |
+| min_avoidance_distance                     | [m]    | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low)                      | 10.0          |
+| min_nominal_avoidance_speed                | [m/s]  | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                                            | 5.0           |
+| min_sharp_avoidance_speed                  | [m/s]  | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                                              | 1.0           |
+| max_right_shift_length                     | [m]    | double | Maximum shift length for right direction                                                                                    | 5.0           |
+| max_left_shift_length                      | [m]    | double | Maximum shift length for left direction                                                                                     | 5.0           |
+| road_shoulder_safety_margin                | [m]    | double | Prevents the generated path to come too close to the road shoulders.                                                        | 0.5           |
+| avoidance_execution_lateral_threshold      | [m]    | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.5           |
+
+(\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value.
 
 ### Speed limit modification
 

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -45,3 +45,6 @@
       # not enabled yet
       longitudinal_collision_margin_min_distance: 0.0          # [m]
       longitudinal_collision_margin_time: 0.0
+
+      # ---------- advanced parameters ----------
+      avoidance_execution_lateral_threshold: 0.499
diff --git a/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -140,6 +140,12 @@ struct AvoidanceParameters
   // turn signal is not turned on.
   double avoidance_search_distance;
 
+  // The avoidance path generation is performed when the shift distance of the
+  // avoidance points is greater than this threshold.
+  // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new
+  // avoidance path will be generated unless their lateral margin difference exceeds this value.
+  double avoidance_execution_lateral_threshold;
+
   // debug
   bool publish_debug_marker = false;
   bool print_debug_info = false;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -244,6 +244,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.publish_debug_marker = dp("publish_debug_marker", false);
   p.print_debug_info = dp("print_debug_info", false);
 
+  p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499);
+
   return p;
 }
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2249,9 +2249,6 @@ boost::optional<AvoidPointArray> AvoidanceModule::findNewShiftPoint(
       ap.getRelativeLength(), ap.getRelativeLongitudinal(), getSharpAvoidanceEgoSpeed());
   };
 
-  // TODO(Horibe) maybe this value must be same with trimSmallShift's.
-  constexpr double NEW_POINT_THRESHOLD = 0.5 - 1.0e-3;
-
   for (size_t i = 0; i < candidates.size(); ++i) {
     const auto & candidate = candidates.at(i);
     std::stringstream ss;
@@ -2279,7 +2276,8 @@ boost::optional<AvoidPointArray> AvoidanceModule::findNewShiftPoint(
     // TODO(Horibe) test fails with this print. why?
     // DEBUG_PRINT("%s, shift current: %f, candidate: %f", pfx, current_shift, candidate.length);
 
-    if (std::abs(candidate.length - current_shift) > NEW_POINT_THRESHOLD) {
+    const auto new_point_threshold = parameters_.avoidance_execution_lateral_threshold;
+    if (std::abs(candidate.length - current_shift) > new_point_threshold) {
       DEBUG_PRINT(
         "%s, New shift point is found!!! shift change: %f -> %f", pfx, current_shift,
         candidate.length);

diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
@@ -34,18 +34,19 @@ double quantize(const double val, const double resolution)
 }
 
 void updateMinMaxPosition(
-  const Eigen::Vector2d & point, double & min_x, double & min_y, double & max_x, double & max_y)
+  const Eigen::Vector2d & point, boost::optional<double> & min_x, boost::optional<double> & min_y,
+  boost::optional<double> & max_x, boost::optional<double> & max_y)
 {
-  min_x = std::min(min_x, point.x());
-  min_y = std::min(min_y, point.y());
-  max_x = std::max(max_x, point.x());
-  max_y = std::max(max_y, point.y());
+  min_x = min_x ? std::min(min_x.get(), point.x()) : point.x();
+  min_y = min_y ? std::min(min_y.get(), point.y()) : point.y();
+  max_x = max_x ? std::max(max_x.get(), point.x()) : point.x();
+  max_y = max_y ? std::max(max_y.get(), point.y()) : point.y();
 }
 
 bool sumLengthFromTwoPoints(
   const Eigen::Vector2d & base_point, const Eigen::Vector2d & target_point,
-  const double length_threshold, double & sum_length, double & min_x, double & min_y,
-  double & max_x, double & max_y)
+  const double length_threshold, double & sum_length, boost::optional<double> & min_x,
+  boost::optional<double> & min_y, boost::optional<double> & max_x, boost::optional<double> & max_y)
 {
   const double norm_length = (base_point - target_point).norm();
   sum_length += norm_length;
@@ -80,10 +81,10 @@ std::array<double, 4> getLaneletScope(
       }};
 
   // calculate min/max x and y
-  double min_x = current_pose.position.x;
-  double min_y = current_pose.position.y;
-  double max_x = current_pose.position.x;
-  double max_y = current_pose.position.y;
+  boost::optional<double> min_x;
+  boost::optional<double> min_y;
+  boost::optional<double> max_x;
+  boost::optional<double> max_y;
 
   for (const auto & get_bound_func : get_bound_funcs) {
     // search nearest point index to current pose
@@ -199,7 +200,13 @@ std::array<double, 4> getLaneletScope(
     }
   }
 
-  return {min_x, min_y, max_x, max_y};
+  if (!min_x || !min_y || !max_x || !max_y) {
+    const double x = current_pose.position.x;
+    const double y = current_pose.position.y;
+    return {x, y, x, y};
+  }
+
+  return {min_x.get(), min_y.get(), max_x.get(), max_y.get()};
 }
 }  // namespace drivable_area_utils
 

diff --git a/sensing/image_transport_decompressor/CMakeLists.txt b/sensing/image_transport_decompressor/CMakeLists.txt
@@ -39,4 +39,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_auto_package()
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml b/sensing/image_transport_decompressor/launch/image_transport_decompressor.launch.xml
@@ -1,9 +1,9 @@
 <launch>
-    <arg name="input_topic_name" default="/input/compressed_image" />
-    <arg name="output_topic_name" default="/output/raw_image" />
+  <arg name="input_topic_name" default="/input/compressed_image" />
+  <arg name="output_topic_name" default="/output/raw_image" />
 
-    <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="image_transport_decompressor_node">
-      <remap from="input" to="$(var input_topic_name)" />
-      <remap from="output" to="$(var output_topic_name)" />
-    </node>
-  </launch>
+  <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="$(anon image_transport_decompressor_node)">
+    <remap from="~/input/compressed_image" to="$(var input_topic_name)" />
+    <remap from="~/output/raw_image" to="$(var output_topic_name)" />
+  </node>
+</launch>