Merge branch 'main' into feat/z_fitting_by_vectormap

Author: SakodaShintaro

control/joy_controller/config/joy_controller.param.yaml

@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    joy_type: DS4
+    joy_type: $(var joy_type)
     update_rate: 10.0
     accel_ratio: 3.0
     brake_ratio: 5.0

control/joy_controller/launch/joy_controller.launch.xml

@@ -30,8 +30,7 @@
     <remap from="service/emergency_stop" to="/api/autoware/set/emergency"/>
     <remap from="service/autoware_engage" to="/api/autoware/set/engage"/>
 
-    <param from="$(var config_file)"/>
-    <param name="joy_type" value="$(var joy_type)"/>
+    <param from="$(var config_file)" allow_substs="true"/>
   </node>
 
   <node pkg="joy" exec="joy_node" name="joy_node" output="screen">

control/trajectory_follower_node/CMakeLists.txt

@@ -62,4 +62,5 @@ ament_auto_package(
   param
   test
   launch
+  config
 )

control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    use_external_target_vel: $(var use_external_target_vel)
+    external_target_vel: $(var external_target_vel)
+    lateral_deviation: $(var lateral_deviation)

control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml

@@ -5,9 +5,7 @@
 
   <!-- engage_transition_manager -->
   <node pkg="trajectory_follower_node" exec="simple_trajectory_follower_exe" name="simple_trajectory_follower" output="screen">
-    <param name="use_external_target_vel" value="$(var use_external_target_vel)"/>
-    <param name="external_target_vel" value="$(var external_target_vel)"/>
-    <param name="lateral_deviation" value="$(var lateral_deviation)"/>
+    <param from="$(find-pkg-share trajectory_follower_node)/config/simple_trajectory_follower.param.yaml" allow_substs="true"/>
 
     <remap from="input/kinematics" to="/localization/kinematic_state"/>
     <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -3,7 +3,7 @@
     update_rate: 10.0
     system_emergency_heartbeat_timeout: 0.5
     use_emergency_handling: false
-    check_external_emergency_heartbeat: false
+    check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat)
     use_start_request: false
     enable_cmd_limit_filter: true
     filter_activated_count_threshold: 5

control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml

@@ -1,8 +1,6 @@
 <launch>
   <arg name="config_file" default="$(find-pkg-share vehicle_cmd_gate)/config/vehicle_cmd_gate.param.yaml"/>
-  <arg name="use_emergency_handling" default="false"/>
   <arg name="check_external_emergency_heartbeat" default="true"/>
-  <arg name="use_start_request" default="false"/>
 
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
@@ -44,10 +42,7 @@
     <remap from="~/service/external_emergency_stop" to="~/external_emergency_stop"/>
     <remap from="~/service/clear_external_emergency_stop" to="~/clear_external_emergency_stop"/>
 
-    <param from="$(var config_file)"/>
+    <param from="$(var config_file)" allow_substs="true"/>
     <param from="$(var vehicle_info_param_file)"/>
-    <param name="use_emergency_handling" value="$(var use_emergency_handling)"/>
-    <param name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
-    <param name="use_start_request" value="$(var use_start_request)"/>
   </node>
 </launch>

control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp

@@ -331,6 +331,9 @@ std::shared_ptr<VehicleCmdGate> generateNode()
     {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml",
      "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"});
 
+  node_options.append_parameter_override(
+    "check_external_emergency_heartbeat", true);  // This parameter has to be set when launching.
+
   const auto override = [&](const auto s, const std::vector<double> v) {
     node_options.append_parameter_override<std::vector<double>>(s, v);
   };

perception/object_range_splitter/src/node.cpp

@@ -20,7 +20,7 @@ ObjectRangeSplitterNode::ObjectRangeSplitterNode(const rclcpp::NodeOptions & nod
 : Node("object_range_splitter_node", node_options)
 {
   using std::placeholders::_1;
-  spilt_range_ = declare_parameter("split_range", 30.0);
+  spilt_range_ = declare_parameter<double>("split_range");
   sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "input/object", rclcpp::QoS{1}, std::bind(&ObjectRangeSplitterNode::objectCallback, this, _1));
   long_range_object_pub_ =

perception/object_velocity_splitter/README.md

@@ -3,21 +3,23 @@
 This package contains a object filter module 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).
 This package can split DetectedObjects into two messages by object's speed.
 
-## Input
+## Interface
 
-| Name              | Type                                                  | Description          |
-| ----------------- | ----------------------------------------------------- | -------------------- |
-| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected objects. |
+### Input
 
-## Output
+- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - 3D detected objects
 
-| Name                          | Type                                                  | Description             |
-| ----------------------------- | ----------------------------------------------------- | ----------------------- |
-| `~/output/low_speed_objects`  | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Objects with low speed  |
-| `~/output/high_speed_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Objects with high speed |
+### Output
 
-## Parameters
+- `~/output/low_speed_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - Objects with low speed
+- `~/output/high_speed_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - Objects with high speed
 
-| Name                 | Type   | Description                                         | Default value |
-| :------------------- | :----- | :-------------------------------------------------- | :------------ |
-| `velocity_threshold` | double | Velocity threshold parameter to split objects [m/s] | 3.0           |
+### Parameters
+
+- `velocity_threshold` (double) [m/s]
+  - Default parameter is 3.0
+
+This parameter is velocity threshold to split objects

perception/radar_fusion_to_detected_object/README.md

@@ -1,46 +1,100 @@
 # radar_fusion_to_detected_object
 
-This package contains a sensor fusion module for radar-detected objects and 3D detected objects. The fusion node can:
+This package contains a sensor fusion module for radar-detected objects and 3D detected objects.
+
+The fusion node can:
 
 - Attach velocity to 3D detections when successfully matching radar data. The tracking modules use the velocity information to enhance the tracking results while planning modules use it to execute actions like adaptive cruise control.
 - Improve the low confidence 3D detections when corresponding radar detections are found.
 
 ![process_low_confidence](docs/radar_fusion_to_detected_object_6.drawio.svg)
 
-## Core algorithm
+## Design
+
+### Background
+
+This package is the fusion with LiDAR-based 3D detection output and radar data.
+LiDAR based 3D detection can estimate position and size of objects with high precision, but it cannot estimate velocity of objects.
+Radar data can estimate doppler velocity of objects, but it cannot estimate position and size of objects with high precision
+This fusion package is aim to fuse these characteristic data, and to estimate position, size, velocity of objects with high precision.
+
+### Algorithm
 
 The document of core algorithm is [here](docs/algorithm.md)
 
+## Interface for core algorithm
+
+The parameters for core algorithm can be set as `core_params`.
+
 ### Parameters for sensor fusion
 
-| Name                     | Type   | Description                                                                                                                                                                                                                    | Default value |
-| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| bounding_box_margin      | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]                                                     | 2.0           |
-| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]                                                                                                    | 5.0           |
-| threshold_yaw_diff       | double | The yaw orientation threshold. If ∣ θ_ob − θ_ra ∣ < threshold × yaw_diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,\*θ_ra is yaw angle from radar object. [rad] | 0.35          |
+- `bounding_box_margin` (double) [m]
+  - Default parameter is 2.0.
+
+This parameter is the distance to extend the 2D bird's-eye view bounding box on each side.
+This parameter is used as a threshold to find radar centroids falling inside the extended box.
+
+- `split_threshold_velocity` (double) [m/s]
+  - Default parameter is 5.0.
+
+This parameter is the object's velocity threshold to decide to split for two objects from radar information.
+Note that this feature is not currently implemented.
+
+- `threshold_yaw_diff` (double) [rad]
+  - Default parameter is 0.35.
+
+This parameter is the yaw orientation threshold.
+If the difference of yaw degree between from a LiDAR-based detection object and radar velocity, radar information is attached to output objects.
 
 ### Weight parameters for velocity estimation
 
 To tune these weight parameters, please see [document](docs/algorithm.md) in detail.
 
-| Name                                 | Type   | Description                                                                                                                                                                             | Default value |
-| :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| velocity_weight_average              | double | The twist coefficient of average twist of radar data in velocity estimation.                                                                                                            | 0.0           |
-| velocity_weight_median               | double | The twist coefficient of median twist of radar data in velocity estimation.                                                                                                             | 0.0           |
-| velocity_weight_min_distance         | double | The twist coefficient of radar data nearest to the center of bounding box in velocity estimation.                                                                                       | 1.0           |
-| velocity_weight_target_value_average | double | The twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. |
-| 0.0                                  |
-| velocity_weight_target_value_top     | double | The twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.   | 0.0           |
+- `velocity_weight_average` (double)
+- Default parameter is 0.0.
+
+This parameter is the twist coefficient of average twist of radar data in velocity estimation.
+
+- `velocity_weight_median` (double)
+- Default parameter is 0.0.
+
+This parameter is the twist coefficient of median twist of radar data in velocity estimation.
+
+- `velocity_weight_min_distance` (double)
+- Default parameter is 1.0.
+
+This parameter is the twist coefficient of radar data nearest to the center of bounding box in velocity estimation.
+
+- `velocity_weight_target_value_average` (double)
+- Default parameter is 0.0.
+
+This parameter is the twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.
+
+- `velocity_weight_target_value_top` (double)
+- Default parameter is 0.0.
+
+This parameter is the twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.
 
 ### Parameters for fixed object information
 
-| Name                     | Type  | Description                                                                                                                                            | Default value |
-| :----------------------- | :---- | :----------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| convert_doppler_to_twist | bool  | Convert doppler velocity to twist using the yaw information of a detected object.                                                                      | false         |
-| threshold_probability    | float | If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object. | 0.4           |
-| compensate_probability   | bool  | If this parameter is true, compensate probability of objects to threshold probability.                                                                 | false         |
+- `convert_doppler_to_twist` (bool)
+  - Default parameter is false.
+
+This parameter is the flag whether convert doppler velocity to twist using the yaw information of a detected object.
 
-## radar_object_fusion_to_detected_object
+- `threshold_probability` (float)
+  - Default parameter is 0.4.
+
+This parameter is the threshold to filter output objects.
+If the probability of an output object is lower than this parameter, and the output object does not have radar points/objects, then delete the object.
+
+- `compensate_probability` (bool)
+  - Default parameter is false.
+
+This parameter is the flag to use probability compensation.
+If this parameter is true, compensate probability of objects to threshold probability.
+
+## Interface for radar_object_fusion_to_detected_object
 
 Sensor fusion with radar objects and a detected object.
 
@@ -56,24 +110,28 @@ ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.laun
 
 ### Input
 
-| Name                    | Type                                                  | Description                                                            |
-| ----------------------- | ----------------------------------------------------- | ---------------------------------------------------------------------- |
-| `~/input/objects`       | autoware_auto_perception_msgs/msg/DetectedObject.msg  | 3D detected objects.                                                   |
-| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` |
+- `~/input/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - 3D detected objects.
+- `~/input/radar_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - Radar objects. Note that frame_id need to be same as `~/input/objects`
 
 ### Output
 
-| Name                             | Type                                                  | Description                                                                            |
-| -------------------------------- | ----------------------------------------------------- | -------------------------------------------------------------------------------------- |
-| `~/output/objects`               | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist.                                                         |
-| `~/debug/low_confidence_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object that doesn't output as `~/output/objects` because of low confidence |
+- `~/output/objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - 3D detected object with twist.
+- `~/debug/low_confidence_objects` (`autoware_auto_perception_msgs/msg/DetectedObjects.msg`)
+  - 3D detected object that doesn't output as `~/output/objects` because of low confidence
 
 ### Parameters
 
-| Name           | Type   | Description           | Default value |
-| :------------- | :----- | :-------------------- | :------------ |
-| update_rate_hz | double | The update rate [hz]. | 20.0          |
+The parameters for core algorithm can be set as `node_params`.
+
+- `update_rate_hz` (double) [hz]
+  - Default parameter is 20.0
+
+This parameter is update rate for the `onTimer` function.
+This parameter should be same as the frame rate of input topics.
 
-## radar_scan_fusion_to_detected_object (TBD)
+## Interface for radar_scan_fusion_to_detected_object (TBD)
 
-TBD
+Under implement

planning/obstacle_avoidance_planner/launch/obstacle_avoidance_planner.launch.xml

@@ -2,13 +2,11 @@
   <arg name="input_path_topic" default="input/path"/>
   <arg name="output_path_topic" default="output/path"/>
   <arg name="enable_debug_info" default="false"/>
-  <arg name="param_path" default="$(find-pkg-share obstacle_avoidance_planner)/param/obstacle_avoidance_planner.param.yaml"/>
-
+  <arg name="param_path" default="$(find-pkg-share obstacle_avoidance_planner)/config/obstacle_avoidance_planner.param.yaml"/>
   <node pkg="obstacle_avoidance_planner" exec="obstacle_avoidance_planner_node" name="obstacle_avoidance_planner" output="screen">
     <remap from="~/input/path" to="$(var input_path_topic)"/>
     <remap from="~/input/odometry" to="/localization/kinematic_state"/>
     <remap from="~/output/path" to="$(var output_path_topic)"/>
-
     <param from="$(var param_path)"/>
     <param name="enable_debug_info" value="$(var enable_debug_info)"/>
   </node>

planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml

@@ -6,7 +6,6 @@
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
 
-  <arg name="enable_slow_down" default="false"/>
   <arg name="input_objects" default="/perception/object_recognition/objects"/>
   <arg name="input_pointcloud" default="input/pointcloud"/>
   <arg name="input_trajectory" default="input/trajectory"/>
@@ -21,7 +20,6 @@
     <param from="$(var adaptive_cruise_control_param_path)"/>
     <param from="$(var obstacle_stop_planner_param_path)"/>
     <param from="$(var vehicle_info_param_file)"/>
-    <param name="enable_slow_down" value="$(var enable_slow_down)"/>
     <!-- remap topic name -->
     <remap from="~/output/stop_reason" to="/planning/scenario_planning/status/stop_reason"/>
     <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>

vehicle/external_cmd_converter/CMakeLists.txt

@@ -15,4 +15,5 @@ rclcpp_components_register_node(external_cmd_converter
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

vehicle/external_cmd_converter/config/external_cmd_converter.param.yaml

@@ -0,0 +1,9 @@
+/**:
+  ros__parameters:
+    csv_path_accel_map: $(var csv_path_accel_map)
+    csv_path_brake_map: $(var csv_path_brake_map)
+    ref_vel_gain: $(var ref_vel_gain)
+    timer_rate: $(var timer_rate)
+    wait_for_first_topic: $(var wait_for_first_topic)
+    control_command_timeout: $(var control_command_timeout)
+    emergency_stop_timeout: $(var emergency_stop_timeout)