Merge branch 'main' into support-new-traffic-signal-interface

Resolved conflicts in planning/behavior_velocity_planner_common/package.xml

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg

@@ -1,8 +1,9 @@
 builtin_interfaces/Time stamp
 
 # states
-string previous_state
-string current_state
+string status
+bool in_autoware_control
+bool in_transition
 
 # flags for engage permission
 bool is_all_ok

control/operation_mode_transition_manager/src/node.cpp

@@ -270,10 +270,19 @@ void OperationModeTransitionManager::publishData()
     pub_operation_mode_->publish(state);
   }
 
+  const auto status_str = [&]() {
+    if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")";
+    if (transition_)
+      return toString(current_mode_) + " (in transition from " + toString(transition_->previous) +
+             ")";
+    return toString(current_mode_);
+  }();
+
   ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo();
   debug_info.stamp = time;
-  debug_info.current_state = toString(current_mode_);
-  debug_info.previous_state = transition_ ? toString(transition_->previous) : "NONE";
+  debug_info.status = status_str;
+  debug_info.in_autoware_control = current_control;
+  debug_info.in_transition = transition_ ? true : false;
   pub_debug_info_->publish(debug_info);
 }
 

control/shift_decider/include/shift_decider/shift_decider.hpp

@@ -50,6 +50,7 @@ class ShiftDecider : public rclcpp::Node
   autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
   autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_;
   autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
+  uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK;
 
   bool park_on_goal_;
 };

control/shift_decider/src/shift_decider.cpp

@@ -83,7 +83,7 @@ void ShiftDecider::updateCurrentShiftCmd()
     } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
       shift_cmd_.command = GearCommand::REVERSE;
     } else {
-      shift_cmd_.command = current_gear_ptr_->report;
+      shift_cmd_.command = prev_shift_command;
     }
   } else {
     if (
@@ -95,6 +95,7 @@ void ShiftDecider::updateCurrentShiftCmd()
       shift_cmd_.command = current_gear_ptr_->report;
     }
   }
+  prev_shift_command = shift_cmd_.command;
 }
 
 void ShiftDecider::initTimer(double period_s)

perception/radar_object_clustering/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(radar_object_clustering)
+
+# Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+# Targets
+ament_auto_add_library(radar_object_clustering_node_component SHARED
+  src/radar_object_clustering_node/radar_object_clustering_node.cpp
+)
+
+rclcpp_components_register_node(radar_object_clustering_node_component
+  PLUGIN "radar_object_clustering::RadarObjectClusteringNode"
+  EXECUTABLE radar_object_clustering_node
+)
+
+# Tests
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Package
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

perception/radar_object_clustering/README.md

@@ -0,0 +1,78 @@
+# radar_object_clustering
+
+This package contains a radar object clustering for [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) input.
+
+This package can make clustered objects from radar DetectedObjects, the objects which is converted from RadarTracks by [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_tracks_msgs_converter) and is processed by noise filter.
+In other word, this package can combine multiple radar detections from one object into one and adjust class and size.
+
+![radar_clustering](docs/radar_clustering.drawio.svg)
+
+## Algorithm
+
+### Background
+
+In radars with object output, there are cases that multiple detection results are obtained from one object, especially for large vehicles such as trucks and trailers.
+Its multiple detection results cause separation of objects in tracking module.
+Therefore, by this package the multiple detection results are clustered into one object in advance.
+
+### Detail Algorithm
+
+- Sort by distance from `base_link`
+
+At first, to prevent changing the result from depending on the order of objects in DetectedObjects, input objects are sorted by distance from `base_link`.
+In addition, to apply matching in closeness order considering occlusion, objects are sorted in order of short distance in advance.
+
+- Clustering
+
+If two radar objects are near, and yaw angle direction and velocity between two radar objects is similar (the degree of these is defined by parameters), then these are clustered.
+Note that radar characteristic affect parameters for this matching.
+For example, if resolution of range distance or angle is low and accuracy of velocity is high, then `distance_threshold` parameter should be bigger and should set matching that strongly looks at velocity similarity.
+
+![clustering](docs/clustering.drawio.svg)
+
+After grouping for all radar objects, if multiple radar objects are grouping, the kinematics of the new clustered object is calculated from average of that and label and shape of the new clustered object is calculated from top confidence in radar objects.
+
+- Fixed label correction
+
+When the label information from radar outputs lack accuracy, `is_fixed_label` parameter is recommended to set `true`.
+If the parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter.
+If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `VEHICLE`.
+
+- Fixed size correction
+
+When the size information from radar outputs lack accuracy, `is_fixed_size` parameter is recommended to set `true`.
+If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters.
+If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to
+`size_x`, `size_y`, `size_z`, as average of vehicle size.
+Note that to use for [multi_objects_tracker](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/multi_object_tracker), the size parameters need to exceed `min_area_matrix` parameters of it.
+
+### Limitation
+
+For now, size estimation for clustered object is not implemented.
+So `is_fixed_size` parameter is recommended to set `true`, and size parameters is recommended to set to value near to average size of vehicles.
+
+## Input
+
+| Name              | Type                                                  | Description    |
+| ----------------- | ----------------------------------------------------- | -------------- |
+| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. |
+
+## Output
+
+| Name               | Type                                                  | Description    |
+| ------------------ | ----------------------------------------------------- | -------------- |
+| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Output objects |
+
+## Parameters
+
+| Name                 | Type   | Description                                                                                                                               | Default value |
+| :------------------- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `angle_threshold`    | double | Angle threshold to judge whether radar detections come from one object. [rad]                                                             | 0.174         |
+| `distance_threshold` | double | Distance threshold to judge whether radar detections come from one object. [m]                                                            | 4.0           |
+| `velocity_threshold` | double | Velocity threshold to judge whether radar detections come from one object. [m/s]                                                          | 2.0           |
+| `is_fixed_label`     | bool   | If this parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter.                    | false         |
+| `fixed_label`        | string | If `is_fixed_label` is true, the label of a clustered object is overwritten by this parameter.                                            | "UNKNOWN"     |
+| `is_fixed_size`      | bool   | If this parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. | false         |
+| `size_x`             | double | If `is_fixed_size` is true, the x-axis size of a clustered object is overwritten by this parameter. [m]                                   | 4.0           |
+| `size_y`             | double | If `is_fixed_size` is true, the y-axis size of a clustered object is overwritten by this parameter. [m]                                   | 1.5           |
+| `size_z`             | double | If `is_fixed_size` is true, the z-axis size of a clustered object is overwritten by this parameter. [m]                                   | 1.5           |