merge branch main into autoware_msg

Signed-off-by: liu cui <[email protected]>

.github/PULL_REQUEST_TEMPLATE/small-change.md

@@ -15,6 +15,10 @@ Not applicable.
 
 Not applicable.
 
+## Interface changes
+
+<!-- Describe any changed interfaces, such as topics, services, or parameters, including debugging interfaces -->
+
 ## Pre-review checklist for the PR author
 
 The PR author **must** check the checkboxes below when creating the PR.

.github/PULL_REQUEST_TEMPLATE/standard-change.md

@@ -18,6 +18,19 @@
 
 <!-- Describe any changed interfaces, such as topics, services, or parameters. -->
 
+### ROS Topic Changes
+
+<!-- | Topic Name       | Type                | Direction | Update Description                                            | -->
+<!-- | ---------------- | ------------------- | --------- | ------------------------------------------------------------- | -->
+<!-- | `/example_topic` | `std_msgs/String`   | Subscribe | Description of what the topic is used for in the system       | -->
+<!-- | `/another_topic` | `sensor_msgs/Image` | Publish   | Also explain if it is added / modified / deleted with the PR | -->
+
+### ROS Parameter Changes
+
+<!-- | Parameter Name       | Default Value | Update Description                                  | -->
+<!-- | -------------------- | ------------- | --------------------------------------------------- | -->
+<!-- | `example_parameters` | `1.0`         | Describe the parameter and also explain the updates | -->
+
 ## Effects on system behavior
 
 <!-- Describe how this PR affects the system behavior. -->

common/autoware_auto_common/include/autoware_auto_common/common/types.hpp

@@ -37,7 +37,9 @@ namespace types
 // We don't currently require code to comply to MISRA, but we should try to where it is
 // easily possible.
 using bool8_t = bool;
+#if __cplusplus < 201811L || !__cpp_char8_t
 using char8_t = char;
+#endif
 using uchar8_t = unsigned char;
 // If we ever compile on a platform where this is not true, float32_t and float64_t definitions
 // need to be adjusted.

common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -57,6 +57,9 @@ struct ObjectPropertyValues
   float alpha{0.999F};
 };
 
+// Control object marker visualization
+enum class ObjectFillType { Skeleton, Fill };
+
 // Map defining colors according to value of label field in ObjectClassification msg
 const std::map<
   autoware_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues>
@@ -85,7 +88,8 @@ get_shape_marker_ptr(
   const autoware_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
-  const bool & is_orientation_available = true);
+  const bool & is_orientation_available = true,
+  const ObjectFillType fill_type = ObjectFillType::Skeleton);
 
 AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_2d_shape_marker_ptr(

common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

@@ -112,6 +112,12 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_confidence_interval_property->addOption("95%", 2);
     m_confidence_interval_property->addOption("99%", 3);
 
+    m_object_fill_type_property = new rviz_common::properties::EnumProperty(
+      "Object Fill Type", "skeleton", "Change object fill type in visualization", this);
+    m_object_fill_type_property->addOption(
+      "skeleton", static_cast<int>(detail::ObjectFillType::Skeleton));
+    m_object_fill_type_property->addOption("Fill", static_cast<int>(detail::ObjectFillType::Fill));
+
     // iterate over default values to create and initialize the properties.
     for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
       const auto & class_property_values = map_property_it.second;
@@ -189,9 +195,13 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     const bool & is_orientation_available) const
   {
     const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
+    const auto fill_type =
+      static_cast<detail::ObjectFillType>(m_object_fill_type_property->getOptionInt());
+
     if (m_display_type_property->getOptionInt() == 0) {
       return detail::get_shape_marker_ptr(
-        shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
+        shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available,
+        fill_type);
     } else if (m_display_type_property->getOptionInt() == 1) {
       return detail::get_2d_shape_marker_ptr(
         shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
@@ -526,6 +536,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   rviz_common::properties::EnumProperty * m_simple_visualize_mode_property;
   // Property to set confidence interval of state estimations
   rviz_common::properties::EnumProperty * m_confidence_interval_property;
+  // Property to set visualization type
+  rviz_common::properties::EnumProperty * m_object_fill_type_property;
   // Property to enable/disable label visualization
   rviz_common::properties::BoolProperty m_display_label_property;
   // Property to enable/disable uuid visualization

common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -495,23 +495,37 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   const autoware_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
-  const bool & is_orientation_available)
+  const bool & is_orientation_available, const ObjectFillType fill_type)
 {
   auto marker_ptr = std::make_shared<Marker>();
   marker_ptr->ns = std::string("shape");
+  marker_ptr->color = color_rgba;
+  marker_ptr->scale.x = line_width;
 
   using autoware_perception_msgs::msg::Shape;
   if (shape_msg.type == Shape::BOUNDING_BOX) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_bounding_box_line_list(shape_msg, marker_ptr->points);
+    if (fill_type == ObjectFillType::Skeleton) {
+      marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+      calc_bounding_box_line_list(shape_msg, marker_ptr->points);
+    } else if (fill_type == ObjectFillType::Fill) {
+      marker_ptr->type = visualization_msgs::msg::Marker::CUBE;
+      marker_ptr->scale = shape_msg.dimensions;
+      marker_ptr->color.a = 0.75f;
+    }
     if (is_orientation_available) {
       calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points);
     } else {
       calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points);
     }
   } else if (shape_msg.type == Shape::CYLINDER) {
-    marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
-    calc_cylinder_line_list(shape_msg, marker_ptr->points);
+    if (fill_type == ObjectFillType::Skeleton) {
+      marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
+      calc_cylinder_line_list(shape_msg, marker_ptr->points);
+    } else if (fill_type == ObjectFillType::Fill) {
+      marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER;
+      marker_ptr->scale = shape_msg.dimensions;
+      marker_ptr->color.a = 0.75f;
+    }
   } else if (shape_msg.type == Shape::POLYGON) {
     marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
     calc_polygon_line_list(shape_msg, marker_ptr->points);
@@ -523,8 +537,6 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
   marker_ptr->pose = to_pose(centroid, orientation);
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
-  marker_ptr->scale.x = line_width;
-  marker_ptr->color = color_rgba;
 
   return marker_ptr;
 }

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -33,6 +33,8 @@ Planning:
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: behavior_path_planner.path_shifter
 
   behavior_path_planner_avoidance:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner

common/tier4_simulated_clock_rviz_plugin/README.md

@@ -10,18 +10,37 @@ This plugin allows publishing and controlling the simulated ROS time.
 | -------- | --------------------------- | -------------------------- |
 | `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time |
 
-## HowToUse
+## How to use the plugin
+
+1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`.
+
+   ```shell
+   ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true
+   ```
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses.
+
+2. Start rviz and select panels/Add new panel.
 
-1. Start rviz and select panels/Add new panel.
    ![select_panel](./images/select_panels.png)
-2. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
+3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
    ![select_clock_plugin](./images/select_clock_plugin.png)
-3. Use the added panel to control how the simulated clock is published.
+
+4. Use the added panel to control how the simulated clock is published.
+
    ![use_clock_plugin](./images/use_clock_plugin.png)
 
-   - Pause button: pause/resume the clock.
-   - Speed: speed of the clock relative to the system clock.
-   - Rate: publishing rate of the clock.
-   - Step button: advance the clock by the specified time step.
-   - Time step: value used to advance the clock when pressing the step button d).
-   - Time unit: time unit associated with the value from e).
+   <ol type="a">
+     <li>Pause button: pause/resume the clock.</li>
+     <li>Speed: speed of the clock relative to the system clock.</li>
+     <li>Rate: publishing rate of the clock.</li>
+     <li>Step button: advance the clock by the specified time step.</li>
+     <li>Time step: value used to advance the clock when pressing the step button d).</li>
+     <li>Time unit: time unit associated with the value from e).</li>
+   </ol>
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you set the time step too large, your simulation will go haywire.

control/autonomous_emergency_braking/README.md

@@ -108,6 +108,33 @@ After Noise filtering, it performs a geometric collision check to determine whet
 
 Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe.
 
+#### Obstacle velocity estimation
+
+Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations:
+
+$$
+d_{t} = o_{time stamp} - prev_{time stamp}
+$$
+
+$$
+d_{pos} = norm(o_{pos} - prev_{pos})
+$$
+
+$$
+v_{norm} = d_{pos} / d_{t}
+$$
+
+Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively.
+
+Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$:
+
+$$
+v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego}
+$$
+
+where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object.
+All these equations are performed disregarding the z axis (in 2D).
+
 ### 4. Collision check with target obstacles
 
 In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as:

control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -1,26 +1,38 @@
 /**:
   ros__parameters:
-    publish_debug_pointcloud: false
+    # Ego path calculation
     use_predicted_trajectory: true
-    use_imu_path: true
-    path_footprint_extra_margin: 1.0
+    use_imu_path: false
+    min_generated_path_length: 0.5
+    imu_prediction_time_horizon: 1.5
+    imu_prediction_time_interval: 0.1
+    mpc_prediction_time_horizon: 1.5
+    mpc_prediction_time_interval: 0.1
+
+    # Debug
+    publish_debug_pointcloud: false
+
+    # Point cloud partitioning
     detection_range_min_height: 0.0
     detection_range_max_height_margin: 0.0
     voxel_grid_x: 0.05
     voxel_grid_y: 0.05
     voxel_grid_z: 100000.0
-    min_generated_path_length: 0.5
+
+    # Point cloud cropping
     expand_width: 0.1
+    path_footprint_extra_margin: 4.0
+
+    # Point cloud clustering
+    cluster_tolerance: 0.1 #[m]
+    minimum_cluster_size: 10
+    maximum_cluster_size: 10000
+
+    # RSS distance collision check
     longitudinal_offset: 2.0
     t_response: 1.0
     a_ego_min: -3.0
     a_obj_min: -1.0
-    cluster_tolerance: 0.1 #[m]
-    minimum_cluster_size: 10
-    maximum_cluster_size: 10000
-    imu_prediction_time_horizon: 1.5
-    imu_prediction_time_interval: 0.1
-    mpc_prediction_time_horizon: 1.5
-    mpc_prediction_time_interval: 0.1
-    collision_keeping_sec: 0.0
+    collision_keeping_sec: 2.0
+    previous_obstacle_keep_time: 1.0
     aeb_hz: 10.0