diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index df8673cd68724..0e3032ddebc26 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -44,7 +44,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt index 7f38e4387b452..9f7a64c18da9a 100644 --- a/control/autoware_autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -12,19 +12,29 @@ include_directories( ${PCL_INCLUDE_DIRS} ) +ament_auto_add_library(autoware_autonomous_emergency_braking_helpers SHARED + include/autoware/autonomous_emergency_braking/utils.hpp + src/utils.cpp +) + set(AEB_NODE ${PROJECT_NAME}_node) ament_auto_add_library(${AEB_NODE} SHARED + include/autoware/autonomous_emergency_braking/node.hpp src/node.cpp ) +target_link_libraries(${AEB_NODE} autoware_autonomous_emergency_braking_helpers) rclcpp_components_register_node(${AEB_NODE} PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" EXECUTABLE ${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_aeb + test/test.cpp) + +target_link_libraries(test_aeb ${AEB_NODE}) + endif() ament_auto_package( diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 2f3f5f08d4156..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -8,11 +8,11 @@ This module has following assumptions. -- It is used when driving at low speeds (about 15 km/h). - - The predicted path of the ego vehicle can be made from either the path created from sensors or the path created from a control module, or both. -- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses point cloud as obstacles. +- The current speed and angular velocity can be obtained from the sensors of the ego vehicle, and it uses points as obstacles. + +- The AEBs target obstacles are 2D points that can be obtained from the input point cloud or by obtaining the intersection points between the predicted ego footprint path and a predicted object's shape. ### IMU path generation: steering angle vs IMU's angular velocity @@ -23,7 +23,7 @@ The pros and cons of both approaches are: IMU angular velocity: - (+) Usually, it has high accuracy -- (-)Vehicle vibration might introduce noise. +- (-) Vehicle vibration might introduce noise. Steering angle: @@ -40,11 +40,13 @@ AEB has the following steps before it outputs the emergency stop signal. 2. Generate a predicted path of the ego vehicle. -3. Get target obstacles from the input point cloud. +3. Get target obstacles from the input point cloud and/or predicted object data. + +4. Estimate the closest obstacle speed. -4. Collision check with target obstacles. +5. Collision check with target obstacles. -5. Send emergency stop signals to `/diagnostics`. +6. Send emergency stop signals to `/diagnostics`. We give more details of each section below. @@ -54,11 +56,11 @@ We do not activate AEB module if it satisfies the following conditions. - Ego vehicle is not in autonomous driving state -- When the ego vehicle is not moving (Current Velocity is very low) +- When the ego vehicle is not moving (Current Velocity is below a 0.1 m/s threshold) ### 2. Generate a predicted path of the ego vehicle -AEB generates a predicted path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: +AEB generates a predicted footprint path based on current velocity and current angular velocity obtained from attached sensors. Note that if `use_imu_path` is `false`, it skips this step. This predicted path is generated as: $$ x_{k+1} = x_k + v cos(\theta_k) dt \\ @@ -68,29 +70,55 @@ $$ where $v$ and $\omega$ are current longitudinal velocity and angular velocity respectively. $dt$ is time interval that users can define in advance. -### 3. Get target obstacles from the input point cloud +On the other hand, if `use_predicted_trajectory` is set to true, the AEB module will use the predicted path from the MPC as a base to generate a footprint path. Both the IMU footprint path and the MPC footprint path can be used at the same time. + +### 3. Get target obstacles + +After generating the ego footprint path(s), the target obstacles are identified. There are two methods to find target obstacles: using the input point cloud, or using the predicted object information coming from perception modules. -After generating the ego predicted path, we select target obstacles from the input point cloud. This obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. +#### Pointcloud obstacle filtering -#### Rough filtering +The AEB module can filter the input pointcloud to find target obstacles with which the ego vehicle might collide. This method can be enable if the `use_pointcloud_data` parameter is set to true. The pointcloud obstacle filtering has three major steps, which are rough filtering, noise filtering with clustering and rigorous filtering. -In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The image of the rough filtering is illustrated below. +##### Rough filtering + +In rough filtering step, we select target obstacle with simple filter. Create a search area up to a certain distance (default is half of the ego vehicle width plus the `path_footprint_extra_margin` parameter) away from the predicted path of the ego vehicle and ignore the point cloud that are not within it. The rough filtering step is illustrated below. ![rough_filtering](./image/obstacle_filtering_1.drawio.svg) -#### Noise filtering with clustering and convex hulls +##### Noise filtering with clustering and convex hulls -To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: . +To prevent the AEB from considering noisy points, euclidean clustering is performed on the filtered point cloud. The points in the point cloud that are not close enough to other points to form a cluster are discarded. Furthermore, each point in a cluster is compared against the `cluster_minimum_height` parameter, if no point inside a cluster has a height/z value greater than `cluster_minimum_height`, the whole cluster of points is discarded. The parameters `cluster_tolerance`, `minimum_cluster_size` and `maximum_cluster_size` can be used to tune the clustering and the size of objects to be ignored, for more information about the clustering method used by the AEB module, please check the official documentation on euclidean clustering of the PCL library: . Furthermore, a 2D convex hull is created around each detected cluster, the vertices of each hull represent the most extreme/outside points of the cluster. These vertices are then checked in the next step. -#### Rigorous filtering +##### Rigorous filtering -After Noise filtering, the module performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept. 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. +After Noise filtering, the module performs a geometric collision check to determine whether the filtered obstacles/hull vertices actually have possibility to collide with the ego vehicle. In this check, the ego vehicle is represented as a rectangle, and the point cloud obstacles are represented as points. Only the vertices with a possibility of collision are kept. ![rigorous_filtering](./image/obstacle_filtering_2.drawio.svg) -#### Obstacle velocity estimation +#### Using predicted objects to get target obstacles + +If the `use_predicted_object_data` parameter is set to true, the AEB can use predicted object data coming from the perception modules, to get target obstacle points. This is done by obtaining the 2D intersection points between the ego's predicted footprint path and each of the predicted objects enveloping polygon or bounding box. + +![predicted_object_and_path_intersection](./image/using-predicted-objects.drawio.svg) + +### Finding the closest target obstacle + +Once all target obstacles have been identified, the AEB module chooses the point that is closest to the ego vehicle as the candidate for collision checking. Only the closest point is considered because RSS distance is used to judge if a collision will happen or not, and if the closest vertex to the ego is deemed to be safe from collision, the rest of the target obstacles will also be safe. + +![closest_object](./image/closest-point.drawio.svg) + +### 4. Obstacle velocity estimation + +To begin calculating the target point's velocity, the point must enter the speed calculation area, +which is defined by the `speed_calculation_expansion_margin` parameter. +Depending on the operational environment, +this margin can reduce unnecessary autonomous emergency braking +caused by velocity miscalculations during the initial calculation steps. + +![speed_calculation_expansion](./image/speed_calculation_expansion.drawio.svg) 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 relative speed using the following equations: @@ -110,7 +138,9 @@ Where $t_{1}$ and $t_{0}$ are the timestamps of the point clouds used to detect ![relative_speed](./image/object_relative_speed.drawio.svg) -Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: +Note that, when the closest obstacle/point comes from using predicted object data, $v_{norm}$ is calculated by directly computing the norm of the predicted object's velocity in the x and y axes. + +The velocity vector is then compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$: $$ v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego} @@ -118,21 +148,23 @@ $$ 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 current 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). -Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity is set to 0 m/s. That is because the RSS distance calculation assumes the ego and the object move in the same direction and it cannot deal with negative velocities. +Note that, the object velocity is calculated against the ego's current movement direction. If the object moves in the opposite direction to the ego's movement, the object velocity will be negative, which will reduce the rss distance on the next step. + +The resulting estimated object speed is added to a queue of speeds with timestamps. The AEB then checks for expiration of past speed estimations and eliminates expired speed measurements from the queue, the object expiration is determined by checking if the time elapsed since the speed was first added to the queue is larger than the parameter `previous_obstacle_keep_time`. Finally, the median speed of the queue is calculated. The median speed will be used to calculate the RSS distance used for collision checking. -### 4. Collision check with target obstacles using RSS distance +### 5. Collision check with target obstacles using RSS distance In the fourth step, it checks the collision with the closest obstacle point using RSS distance. RSS distance is formulated as: $$ -d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) - v_{obj}^2/(2*a_{obj_{min}}) + offset +d = v_{ego}*t_{response} + v_{ego}^2/(2*a_{min}) -(sign(v_{obj})) * v_{obj}^2/(2*a_{obj_{min}}) + offset $$ where $v_{ego}$ and $v_{obj}$ is current ego and obstacle velocity, $a_{min}$ and $a_{obj_{min}}$ is ego and object minimum acceleration (maximum deceleration), $t_{response}$ is response time of the ego vehicle to start deceleration. Therefore the distance from the ego vehicle to the obstacle is smaller than this RSS distance $d$, the ego vehicle send emergency stop signals. This is illustrated in the following picture. ![rss_check](./image/rss_check.drawio.svg) -### 5. Send emergency stop signals to `/diagnostics` +### 6. Send emergency stop signals to `/diagnostics` If AEB detects collision with point cloud obstacles in the previous step, it sends emergency signal to `/diagnostics` in this step. Note that in order to enable emergency stop, it has to send ERROR level emergency. Moreover, AEB user should modify the setting file to keep the emergency level, otherwise Autoware does not hold the emergency state. @@ -160,41 +192,49 @@ The AEB module can also prevent collisions when the ego vehicle is moving backwa When vehicle odometry information is faulty, it is possible that the MPC fails to predict a correct path for the ego vehicle. If the MPC predicted path is wrong, collision avoidance will not work as intended on the planning modules. However, the AEB’s IMU path does not depend on the MPC and could be able to predict a collision when the other modules cannot. As an example you can see a figure of a hypothetical case in which the MPC path is wrong and only the AEB’s IMU path detects a collision. -![backward driving](./image/wrong-mpc.drawio.svg) +![wrong mpc](./image/wrong-mpc.drawio.svg) ## Parameters -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | -| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | -| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | -| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | -| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | +| check_autoware_state | [-] | bool | flag to enable or disable autoware state check. If set to false, the AEB module will run even when the ego vehicle is not in AUTONOMOUS state. | true | +| detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | +| detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| cluster tolerance | [m] | double | maximum allowable distance between any two points to be considered part of the same cluster | 0.15 | +| cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | +| minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | +| maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | +| speed_calculation_expansion_margin | [m] | double | expansion width of the ego vehicle for the beginning speed calculation | 0.1 | ## Limitations +- The distance required to stop after collision detection depends on the ego vehicle's speed and deceleration performance. To avoid collisions, it's necessary to increase the detection distance and set a higher deceleration rate. However, this creates a trade-off as it may also increase the number of unnecessary activations. Therefore, it's essential to consider what role this module should play and adjust the parameters accordingly. + - AEB might not be able to react with obstacles that are close to the ground. It depends on the performance of the pre-processing methods applied to the point cloud. - Longitudinal acceleration information obtained from sensors is not used due to the high amount of noise. - The accuracy of the predicted path created from sensor data depends on the accuracy of sensors attached to the ego vehicle. -- Currently, the module calculates thee closest object velocity if it goes in the same direction as the ego vehicle, otherwise the velocity is set to 0 m/s since RSS distance calculation does not use negative velocities. - ![aeb_range](./image/range.drawio.svg) diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 4233f8b6bebcb..01a03b5ae21e8 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,8 +3,12 @@ # Ego path calculation use_predicted_trajectory: true use_imu_path: false + use_pointcloud_data: true + use_predicted_object_data: true use_object_velocity_calculation: true - min_generated_path_length: 0.5 + check_autoware_state: true + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 1.5 @@ -12,6 +16,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 @@ -23,9 +28,11 @@ # Point cloud cropping expand_width: 0.1 path_footprint_extra_margin: 4.0 + speed_calculation_expansion_margin: 0.5 # Point cloud clustering cluster_tolerance: 0.1 #[m] + cluster_minimum_height: 0.0 minimum_cluster_size: 10 maximum_cluster_size: 10000 diff --git a/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg b/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg new file mode 100644 index 0000000000000..154bba1fc9152 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/closest-point.drawio.svg @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + +
+
+
+ Find the closest target obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg index 1d12fe233ad8e..137872daebace 100644 --- a/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg @@ -6,360 +6,249 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="616px" - height="534px" - viewBox="-0.5 -0.5 616 534" - content="<mxfile host="app.diagrams.net" modified="2024-06-13T07:23:40.787Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="VWopeiBdF3bKI_lRsSXP" version="24.5.4" type="google" scale="1" border="0"> <diagram name="Page-1" id="nTw8mlOSOdTxmldlJuHK"> <mxGraphModel dx="941" dy="648" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-1" value="" style="group;opacity=30;" vertex="1" connectable="0" parent="1"> <mxGeometry x="340" y="602" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-2" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-4" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-5" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-1"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-6" value="" style="group" vertex="1" connectable="0" parent="1"> <mxGeometry x="340" y="647" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-7" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-8" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-9" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-6"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-10" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" vertex="1" parent="1"> <mxGeometry x="180" y="618" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-11" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="456.7649999999999" y="677.5" as="targetPoint" /> <mxPoint x="350.0000000000002" y="677.4867690685733" as="sourcePoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-12" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="466.34" y="655" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-13" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="464" y="671" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-14" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="468.34000000000003" y="685" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-15" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="480" y="655" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-16" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="472" y="665" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-17" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" vertex="1" parent="1"> <mxGeometry x="235.47000000000003" y="651.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-18" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="482" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="478.34000000000003" y="695" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-20" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="492" y="683" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-21" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="392.66999999999996" y="782" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-22" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="468" y="676" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-23" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="683" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="481" y="684" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-25" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="473" y="689" as="sourcePoint" /> <mxPoint x="468.24800000000005" y="675.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-26" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="699" as="sourcePoint" /> <mxPoint x="473.068" y="689.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-27" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="491" y="709" as="sourcePoint" /> <mxPoint x="482.068" y="699.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-28" value="" style="group;opacity=30;" vertex="1" connectable="0" parent="1"> <mxGeometry x="334.84" y="872" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-29" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-30" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-31" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-32" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-28"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-33" value="" style="group" vertex="1" connectable="0" parent="1"> <mxGeometry x="334.84" y="917" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-34" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-35" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-36" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" vertex="1" parent="ty0ni0eQC3Mwg9WxKvAH-33"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-37" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" vertex="1" parent="1"> <mxGeometry x="174.84" y="888" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-38" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="461.17999999999995" y="925" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-39" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e3c800;strokeColor=#B09500;fontColor=#000000;" vertex="1" parent="1"> <mxGeometry x="458.84" y="941" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-40" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="463.18" y="955" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-41" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="474.84" y="925" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-42" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" vertex="1" parent="1"> <mxGeometry x="230.31000000000003" y="921.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-43" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="473.18" y="965" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-44" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" vertex="1" parent="1"> <mxGeometry x="486.84" y="953" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-45" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" vertex="1" parent="1"> <mxGeometry x="387.50999999999993" y="1052" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-46" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="462.84" y="946" as="sourcePoint" /> <mxPoint x="478.84" y="929" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-47" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-44"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="476.84" y="953" as="sourcePoint" /> <mxPoint x="478.84" y="929" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-48" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="467.84" y="959" as="sourcePoint" /> <mxPoint x="463.088" y="945.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-49" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="476.84" y="969" as="sourcePoint" /> <mxPoint x="467.90799999999996" y="959.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-50" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-44"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="485.84" y="979" as="sourcePoint" /> <mxPoint x="476.90799999999996" y="969.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-51" value="" style="endArrow=classic;startArrow=classic;html=1;rounded=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;exitX=0.008;exitY=0.543;exitDx=0;exitDy=0;exitPerimeter=0;" edge="1" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-42" target="ty0ni0eQC3Mwg9WxKvAH-39"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="353.5" y="949" as="sourcePoint" /> <mxPoint x="403.5" y="899" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-52" value="&lt;font style=&quot;font-size: 14px;&quot;&gt;Closest Point&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;fontColor=#000000;" vertex="1" connectable="0" parent="ty0ni0eQC3Mwg9WxKvAH-51"> <mxGeometry x="-0.084" y="1" relative="1" as="geometry"> <mxPoint x="3" y="-7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-53" value="Filter out points outside clusters and get a 2D convex hull of each cluster" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" vertex="1" parent="1"> <mxGeometry x="159" y="564" width="600" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-54" value="Find the closest vertex of the convex hull(s)" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" vertex="1" parent="1"> <mxGeometry x="301" y="832" width="370" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-55" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="535" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-56" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.x;fillColor=#FF0000;opacity=40;" vertex="1" parent="1"> <mxGeometry x="529" y="668" width="20" height="19" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + height="265px" + viewBox="-0.5 -0.5 616 265" + content="<mxfile host="app.diagrams.net" modified="2024-06-21T02:42:01.489Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="3pgNd0JrTgvgM2V0BwGE" version="24.6.0" type="google" scale="1" border="0"> <diagram name="Page-1" id="nTw8mlOSOdTxmldlJuHK"> <mxGraphModel dx="1137" dy="783" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-1" value="" style="group;opacity=30;" parent="1" connectable="0" vertex="1"> <mxGeometry x="340" y="602" width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-2" value="" style="group;fillColor=#dae8fc;strokeColor=#6c8ebf;container=0;" parent="ty0ni0eQC3Mwg9WxKvAH-1" connectable="0" vertex="1"> <mxGeometry width="420" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry x="280" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-4" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry x="140" width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-5" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-1" vertex="1"> <mxGeometry width="140" height="155" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-6" value="" style="group" parent="1" connectable="0" vertex="1"> <mxGeometry x="340" y="647" width="420" height="85" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-7" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry x="280" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-8" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry x="140" width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-9" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;opacity=40;strokeWidth=5;" parent="ty0ni0eQC3Mwg9WxKvAH-6" vertex="1"> <mxGeometry width="140" height="60" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-10" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/svg+xml,PHN2ZyB4bWxuczp4bGluaz0iaHR0cDovL3d3dy53My5vcmcvMTk5OS94bGluayIgeG1sbnM9Imh0dHA6Ly93d3cudzMub3JnLzIwMDAvc3ZnIiB2aWV3Qm94PSIwIDAgMTE5LjQ2IDQwIiBpZD0iTGF5ZXJfMiI+PGRlZnM+PHN0eWxlPi5jbHMtMXtjbGlwLXBhdGg6dXJsKCNjbGlwcGF0aCk7fS5jbHMtMntmaWxsOm5vbmU7fS5jbHMtMiwuY2xzLTN7c3Ryb2tlLXdpZHRoOjBweDt9LmNscy0ze2ZpbGw6IzIzMWYyMDt9PC9zdHlsZT48Y2xpcFBhdGggaWQ9ImNsaXBwYXRoIj48cmVjdCBoZWlnaHQ9IjQwIiB3aWR0aD0iMTE1LjExIiB4PSIuNSIgY2xhc3M9ImNscy0yIi8+PC9jbGlwUGF0aD48L2RlZnM+PGcgaWQ9IlJvYWRzIj48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9Ii41IiBjbGFzcz0iY2xzLTMiLz48cGF0aCBkPSJtMTA0LjksMjAuNWgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFabS0xNC40MSwwaC03LjIxdi0xaDcuMjF2MVptLTE0LjQxLDBoLTcuMjF2LTFoNy4yMXYxWm0tMTQuNDEsMGgtNy4yMXYtMWg3LjIxdjFaIiBjbGFzcz0iY2xzLTMiLz48cmVjdCBoZWlnaHQ9IjEiIHdpZHRoPSIzLjUiIHk9IjE5LjUiIHg9IjExMi4xMSIgY2xhc3M9ImNscy0zIi8+PGcgY2xhc3M9ImNscy0xIj48cG9seWdvbiBwb2ludHM9IjAgMi41OSAwIDMuNTIgMS4wMiAzLjUyIDEuMDIgMy41OSAxMTguNDYgMy41OSAxMTguNDYgMzYuNDEgMSAzNi40MSAxIDM2LjQgMCAzNi40IDAgMzcuNDEgMTE5LjQ2IDM3LjQxIDExOS40NiAyLjU5IDAgMi41OSIgY2xhc3M9ImNscy0zIi8+PC9nPjwvZz48L3N2Zz4=;" parent="1" vertex="1"> <mxGeometry x="180" y="618" width="595" height="200" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-11" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="1" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="456.7649999999999" y="677.5" as="targetPoint" /> <mxPoint x="350.0000000000002" y="677.4867690685733" as="sourcePoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-12" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" parent="1" vertex="1"> <mxGeometry x="466.34" y="655" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-13" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="464" y="671" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-14" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="468.34000000000003" y="685" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-15" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="480" y="655" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-16" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="472" y="665" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-17" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=0;" parent="1" vertex="1"> <mxGeometry x="235.47000000000003" y="651.5" width="123.19" height="53" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-18" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="482" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="478.34000000000003" y="695" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-20" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#60a917;strokeColor=#2D7600;fontColor=#ffffff;" parent="1" vertex="1"> <mxGeometry x="492" y="683" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-21" value="" style="shape=image;aspect=fixed;image=data:image/svg+xml,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;points=[[0,0.25,0,0,0],[0,0.5,0,0,0],[0,0.75,0,0,0],[0.1,0,0,0,0],[0.1,1,0,0,0],[0.25,0,0,0,0],[0.25,1,0,0,0],[0.5,0,0,0,0],[0.5,1,0,0,0],[0.75,0,0,0,0],[0.75,1,0,0,0],[0.89,0,0,0,0],[0.89,1,0,0,0],[1,0.25,0,0,0],[1,0.5,0,0,0],[1,0.75,0,0,0]];imageBackground=none;imageAspect=0;perimeter=none;" parent="1" vertex="1"> <mxGeometry x="392.66999999999996" y="782" width="107.33" height="46" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-22" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="468" y="676" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-23" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0;entryY=0.875;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.369;exitY=0.304;exitDx=0;exitDy=0;exitPerimeter=0;" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="683" as="sourcePoint" /> <mxPoint x="484" y="659" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="481" y="684" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-25" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.406;entryY=0.732;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="473" y="689" as="sourcePoint" /> <mxPoint x="468.24800000000005" y="675.856" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-26" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="482" y="699" as="sourcePoint" /> <mxPoint x="473.068" y="689.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-27" value="" style="endArrow=none;html=1;rounded=0;fillColor=#60a917;strokeColor=#2D7600;entryX=0.716;entryY=0.513;entryDx=0;entryDy=0;entryPerimeter=0;exitX=0.366;exitY=0.246;exitDx=0;exitDy=0;exitPerimeter=0;" parent="1" source="ty0ni0eQC3Mwg9WxKvAH-20" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="491" y="709" as="sourcePoint" /> <mxPoint x="482.068" y="699.104" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-53" value="Filter out points outside clusters and get a 2D convex hull of each cluster" style="text;html=1;align=center;verticalAlign=middle;resizable=0;points=[];autosize=1;strokeColor=none;fillColor=none;fontSize=18;" parent="1" vertex="1"> <mxGeometry x="159" y="564" width="600" height="40" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-55" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="535" y="673" width="8" height="8" as="geometry" /> </mxCell> <mxCell id="ty0ni0eQC3Mwg9WxKvAH-56" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.x;fillColor=#FF0000;opacity=40;" parent="1" vertex="1"> <mxGeometry x="529" y="668" width="20" height="19" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " style="background-color: rgb(255, 255, 255);" > - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+ + -
-
- Closest Point -
-
-
- - - - - - - - - - - - -
-
-
- Filter out points outside clusters and get a 2D convex hull of each cluster -
-
-
- - - - - - - - - - - - -
-
-
- Find the closest vertex of the convex hull(s) -
-
-
- - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Filter out points outside clusters and get a 2D convex hull of each cluster +
+
+
+ + + + + + + + + + + + + + + + - - - - - - diff --git a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg index 2d1716519631d..1d3dd824df5f0 100644 --- a/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg @@ -1,134 +1,73 @@ - - - - - - - + + + + + + + + + + + + + + + + +
+
+ + Closest point distance +
+ +
+
+ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - Closest Point Distance -
-
-
- - - - - - - - - - - - - - - - - -
-
-
RSS distance
-
-
- - - - + + + + + + +
+
+ RSS distance +
+
+ diff --git a/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg new file mode 100644 index 0000000000000..37728253370e4 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/speed_calculation_expansion.drawio.svg @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ speed_calculation_expansion_margin +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Closest Point +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg b/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg new file mode 100644 index 0000000000000..244b609c7f009 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/image/using-predicted-objects.drawio.svg @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego path footprint and object polygon intersection points +
+
+
+ + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg index a829cb60ec7d6..282d2fff9b9a8 100644 --- a/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg +++ b/control/autoware_autonomous_emergency_braking/image/wrong-mpc.drawio.svg @@ -5,203 +5,240 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="366px" - height="428px" - viewBox="-0.5 -0.5 366 428" - content="<mxfile host="app.diagrams.net" modified="2024-06-13T09:19:00.668Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/121.0.0.0 Safari/537.36" etag="zoMk_8KlhUSMgrp7g-GF" version="24.5.4" type="google" scale="5" border="0"> <diagram name="Page-1" id="Er80bHjigqJgTDkSixCH"> <mxGraphModel dx="183" dy="126" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <UserObject label="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" link="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" id="PcQBwLoKRl0nhIK2ECRR-1"> <mxCell style="text;whiteSpace=wrap;html=1;" parent="1" vertex="1"> <mxGeometry x="400" y="215" width="70" height="75" as="geometry" /> </mxCell> </UserObject> <mxCell id="PcQBwLoKRl0nhIK2ECRR-22" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="417" y="216" width="30" height="10" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-5" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1"> <mxGeometry x="407" y="269" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-3" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="412" y="250" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-4" value="&lt;font style=&quot;font-size: 5px;&quot;&gt;IMU&lt;/font&gt;" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="417" y="231" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-7" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;entryX=0.69;entryY=0.13;entryDx=0;entryDy=0;entryPerimeter=0;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="410.9999999999999" y="276" as="sourcePoint" /> <mxPoint x="466.0000000000001" y="230" as="targetPoint" /> <Array as="points"> <mxPoint x="427" y="242" /> </Array> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-13" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" parent="PcQBwLoKRl0nhIK2ECRR-7" connectable="0" vertex="1"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint y="-3" as="offset" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-12" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.5;entryY=0;entryDx=0;entryDy=0;fillColor=#0050ef;strokeColor=#001DBC;" parent="1" target="PcQBwLoKRl0nhIK2ECRR-3" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="414.87" y="269" as="sourcePoint" /> <mxPoint x="414.87" y="259" as="targetPoint" /> <Array as="points"> <mxPoint x="414.87" y="269" /> </Array> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-18" value="" style="rounded=0;whiteSpace=wrap;html=1;rotation=15;fillColor=#dae8fc;strokeColor=#6c8ebf;opacity=40;" parent="1" vertex="1"> <mxGeometry x="422.24" y="211" width="10" height="20" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-19" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="425" y="222.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-24" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="430.5" y="222.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-25" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1"> <mxGeometry x="420.24" y="225.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-29" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="415.24" y="280" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-30" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="407" y="276.97" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-2" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,iVBORw0KGgoAAAANSUhEUgAAAKwAAABKCAYAAAAmLL9HAAAACXBIWXMAADT9AAA1CgHHClLTAAAJN0lEQVR4nO2dQW7bOBSGfxezLJDMbnbJADWjXdwTVD1B3BNEPcGkJxjNCeKeYJQT1DlBlBPU2QlEgbF33U0MdJ9Z8KmRnkhTylgSJfMDhIC2adHML/LxkXycPD09oQmBEHMAIYAZXUeNvsBz6GwBrOhKMymXTTJP6gg2EOIUQAxgDi9Qz37ZAlgCiDMp17YP7xRsIMQxlFD/2FPhPJ5dfIYS7qPpA0bBBkLMoJR/0k7ZPB4tGwDzTMqV7k2tYAMhIgAL6Lv/vAlPAaxMX+wpEwhRquhMyklfZekTaghnUOMgk4m5BXCVSZnwNyqCJbH+bfiSBYDFribbo8cLtgqZnFd06YT7kYu2JFhSf6rJfAsg8kJ9OV6wZki4CYAL9tYWQFjsxX8KljKtULVZbzIpo7YKeyh4wdoJhEgAXLKXNwBmeWP5S+GNGAMUK/UKIYBj+jsIAiHSvsvQEo8o+1lr98qZlFEgBFAW7QmUyRAD1MKSn/Uflv82k3L+8nK3RyBECCCC9wsPgVsAS90AykQgxBJV8+D3TMr1K0rE7M0tlCCcIhAiCoRYA7iDegq9WN3nAsDfgRBrmiWtQwSlwSIxAOSC5V/klCcgEGIWCLGC8l54v/AwOQHwJRBiSeMlI6S9BXt5DgCTs+l0DuBL4Y0tgFNXBBsIcQXg2vKxeyjvxpqutnkD5Ut8DeA3umycs/TDvgvVAt/p+gFlk36rkafoZzU1Lg9QXiejD59EvUa5F/0wOZtOFyhPvToz0DKMGnPuASRNbKP/WZYQ3m7OJ42STMrU9mGqsxjAO8N3hRbRJij//z9PzqbTlH1hxVnbBzSK1v3Qe6j55rSjcsyguiddWQ6Ze6jZKOtMJwk3QbXFLbmsNPkilCex7l9BNd9Fep9qpSdLJ5BPmZRhh2JdAPhqKMuh8w7AV6qjndD/a4aqGXQC1WKb4FqcTc6mU6cc2oEQMYA/2cvW7mPPZTiGsom53Vkkt5tzv+MYCQt/dz20D1D/H+u4x9Bzfsqk1AqfT7g4JVjqOu7Yyy6JdQNlky1dGZR2BdXLHPoJJqCmaA31axzoc8G+4h/omYSlXRHrFsq2P82kTA5NrIByNdFvPwXwEVU/6TmAtKbLKmIvH0HNZllxRrBkCvAnt5ZRv0cW0LufZi4MRF2B6kJnk56j6j/V5V8BuGEvR3Xu7YRgC8vMitx2KRKaheEutLybW3dVjqFAdRKiKtrLmjNaMUufkEdmJ04IFnrfZq0uYo/wliE3Rw6u+68L1U2IqnlQp5VdQ60zKGIVuiuC5eK86bJVI38fN0fmXqx2qI640E6oTm2kLB3aMvQuWFopxu1G6xO6Z2KWvunK1zsGqK64TRrXyMrHJ6e2DL0LFtWn6qHLgRbZTbx1jbu6/4iIWdpqk2oaBevCJhcFm3Z8f96dPfhBVnOozvgAbO/rqV0Q7ClLpx3fn7cCScf3HxMJS1tH/U1xQbD8R607vv8pS491mrULGtukTfnF/pHWKbmzeohzwAd8Tgu2sMwxxLPNt4HqmWot+2sRXne71mK8CBdaWKdw1ZUVCHFMe53y7UHFAcoJvXZXZ0V/W3RRd16wA6CwxoFvzNNxgRpz+kPFC3YYJGjWvZ5jpINHL1jHIZuVt6wbqNVjEwC/Qq2e2rDPXFDeUeEF6z4RS+fbShLgedkflLeFi5bnHTxesO4TsrQ2fiq9FlvyDh4vWPfh05XGPVCa5Ziji+HgBTswdrmOxuoZKOIF6z4lu9SybI/P3XObdvB4wbpPytKxriUtnEexK+/gcWFq1rObBNXwkyvaA5fbs6bdrEm7Rese38I6Dq0N4FtJTqAiovxLly5I3u0YF6F7wQ6DCM2Cxz1ghD5YwAt2EBQ2+/GWVsctRrx50tuwAyHf7Of48sLW8YIdGCTItOdi9IY3CRiH4Hxviy7qzgXBloIw1In+sWf4YKbr+48JXnd7jzLugmBb3wdkYc3SXrAvp/X9eS4Ids3SYcf35w9M1PH9x0TE0nvfH+eCYFOWDju+P1/9dE7RaDwNMETw2RVd+0W4KNjzLu1Y2qXLF4nEXd1/RMQsvbHtgNbsiLAu1uldsIaIIV1HLoxZ+nKM20vaguqKhyqNa2RtbPP2LliCB3+77LJbpoXP/Onubbv0kKA64l3/pmZs35ClU1sGVwS7xAtijO4Z3qofYcTbpfdBYft549i+1CDxzZVWm9cJwRqOaryoGWN0X2VYohoyMo/bf9pVOYYC1UmK6kDrhurSRszSVpsXcESwAJBJGaPaLS86nki4gj5u/4qOEPXg53GqK+jPg6jTus5QtXmTOvd2RrBExNJ5t9yJaAurorhojwBc04nU0SGaCRQqKT9N/RpVM6DJsUcJe3mLmiagU+d0AYM8WA4Y72KUGYBjOHaw3CPKT8vbHiIIlthxKLLxh7VUjgXKB0d7qnzOpKxjBpgagftMytCQZwZ1dGrO9hU053nWLmpL0Gni95q3rgMh0q58pPSPeGsoy6FzD9W41RFrCL3Nu8HuKN1ciyudYENbAbqAnjo+agdUd3JHwo06KMeKyvKeysPdb4fEFqoO3tMh1daZLDIB7lDdc7aF/aSekKVXk7PpdA7gC/si7bmffUAj0mvLx3J7co1uIni/gXr6XwP4jS4buhG163yn6wdUw/atRp4ZXSHMkWceAES7BE8mxBplc/XD5OnpCYEQ3I79i9xMTkC2TIIWIjp7OucWSqw2b0KM8uB7m0l5nLu1uKP3yiXXDXXLM+jDSnqGwQbAh0xK64F9hqNcl8CzHzZmbx7BwSAMhdOkvT05HG7xfBJ63eWGCap+3hgAJk9Pys1lcOHc0IjdWchcCPHsL3QV7nscq+fhEcreXQFIm46FDC7Nn66zomCP6SbcUHZetEOAO8D7nqBxEYNY8wDOj0BharZwyC3vZi/7PJnEM34KJ+RwsVZcX6W1BORm0DmCLwCsAyG0kfM8npdAQo2h3Fe6E3KuuOvrp0nAviiCWozADV9AqX4J5fdc9T2NOxS8SaCgMUc+7pjDrLEr3SJwrWALX7zECMOOe5xmA2UGaBtC4/JCyjAD8Lmlgnk8nL+gBljGXtvYwhah1eUxzE24x/NSchMzpg2pO6kl2CKBEHMo+yO3RbyAPU3YouynbRS74D+7OeKwBtsqIwAAAABJRU5ErkJggg==;direction=west;imageBackground=default;rotation=-75;" parent="1" vertex="1"> <mxGeometry x="400" y="274.97" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-31" value="" style="verticalLabelPosition=bottom;verticalAlign=top;html=1;shape=mxgraph.basic.flash;fillColor=#fff2cc;strokeColor=#d6b656;" parent="1" vertex="1"> <mxGeometry x="407.24" y="275" width="4.76" height="8" as="geometry" /> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-32" value="" style="endArrow=none;html=1;rounded=0;entryX=0.3;entryY=0.35;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="429" y="259" as="sourcePoint" /> <mxPoint x="421.24" y="252" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-34" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="449.88" y="241" as="sourcePoint" /> <mxPoint x="446" y="234" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-35" value="" style="endArrow=none;html=1;rounded=0;entryX=0.3;entryY=0.35;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#f8cecc;strokeColor=#b85450;" parent="1" edge="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="438.26" y="250" as="sourcePoint" /> <mxPoint x="430.5" y="243" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-36" value="" style="endArrow=classic;html=1;rounded=0;strokeColor=default;fillColor=#d5e8d4;endSize=2;startSize=2;" parent="1" edge="1"> <mxGeometry x="0.0036" width="50" height="50" relative="1" as="geometry"> <mxPoint x="460" y="269" as="sourcePoint" /> <mxPoint x="440" y="249" as="targetPoint" /> <mxPoint as="offset" /> </mxGeometry> </mxCell> <mxCell id="PcQBwLoKRl0nhIK2ECRR-37" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC path is wrong&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" parent="1" connectable="0" vertex="1"> <mxGeometry x="456.00037936571215" y="268.99956650554554" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + width="968px" + height="723px" + viewBox="-0.5 -0.5 968 723" + content="<mxfile host="app.diagrams.net" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/128.0.0.0 Safari/537.36" version="24.7.8" scale="9" border="1"> <diagram name="Page-1" id="Y1UgVOXVdMQA1kQdro1d"> <mxGraphModel dx="205" dy="118" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0"> <root> <mxCell id="0" /> <mxCell id="1" parent="0" /> <UserObject label="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" link="&lt;b style=&quot;font-weight:normal;&quot; id=&quot;docs-internal-guid-16187b7b-7fff-5157-edfc-59831d7d53d4&quot;&gt;&lt;img width=&quot;70px;&quot; height=&quot;70px;&quot; src=&quot;https://lh7-us.googleusercontent.com/slidesz/AGV_vUdVGwxdOkKFcsQrdo4YfXPg44pOoqQMen0V7cwCo5A-EblTaGze8RnoOq4sRbU0dERW216f171P2PPyWaYIAbyRELgVnzp53eZ4CBNd4GxjeA-rJhh3uL90_2279dcwFv3jrbIDnd5vY8-aBZdQ5_7UEH6qcUie=s2048?key=ntUvc0_pIcS5A532hIN96g&quot;&gt;&lt;/b&gt;" id="2"> <mxCell style="text;whiteSpace=wrap;html=1;" vertex="1" parent="1"> <mxGeometry x="210" y="220" width="70" height="75" as="geometry" /> </mxCell> </UserObject> <mxCell id="3" value="" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#fff2cc;strokeColor=#d6b656;" vertex="1" parent="1"> <mxGeometry x="235" y="221" width="30" height="10" as="geometry" /> </mxCell> <mxCell id="4" value="" style="endArrow=none;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="237.9999999999999" y="279.5" as="sourcePoint" /> <mxPoint x="290" y="250" as="targetPoint" /> <Array as="points"> <mxPoint x="254" y="245.5" /> </Array> </mxGeometry> </mxCell> <mxCell id="5" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;MPC&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" connectable="0" vertex="1" parent="4"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint y="-1" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="249.12" y="229.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="7" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="260" y="225.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="8" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="260" y="230.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="9" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=-75;opacity=50;" vertex="1" parent="1"> <mxGeometry x="224.88" y="279.97" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="10" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#e1d5e7;strokeColor=#9673a6;" vertex="1" parent="1"> <mxGeometry x="255.12" y="227.5" width="3" height="3" as="geometry" /> </mxCell> <mxCell id="11" value="" style="shape=image;verticalLabelPosition=bottom;labelBackgroundColor=default;verticalAlign=top;aspect=fixed;imageAspect=0;image=data:image/png,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;direction=west;imageBackground=default;rotation=-75;" vertex="1" parent="1"> <mxGeometry x="217.26" y="264" width="23.24" height="10" as="geometry" /> </mxCell> <mxCell id="12" value="" style="endArrow=none;html=1;rounded=0;fillColor=#dae8fc;strokeColor=#6c8ebf;strokeWidth=9;opacity=40;exitX=0.342;exitY=0.718;exitDx=0;exitDy=0;exitPerimeter=0;curved=1;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="231.2399999999999" y="259.5" as="sourcePoint" /> <mxPoint x="283.24" y="230.00000000000003" as="targetPoint" /> <Array as="points"> <mxPoint x="247.24" y="225.50000000000003" /> </Array> </mxGeometry> </mxCell> <mxCell id="13" value="&lt;font style=&quot;font-size: 4px;&quot;&gt;IMU&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];labelBackgroundColor=none;" connectable="0" vertex="1" parent="12"> <mxGeometry x="0.2768" y="-4" relative="1" as="geometry"> <mxPoint x="-11" y="6" as="offset" /> </mxGeometry> </mxCell> <mxCell id="14" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.529;entryY=0.366;entryDx=0;entryDy=0;entryPerimeter=0;startSize=5;endSize=1;fillColor=#0050ef;strokeColor=#001DBC;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="234" y="294" as="sourcePoint" /> <mxPoint x="227.49990692359" y="274.9978128903232" as="targetPoint" /> </mxGeometry> </mxCell> <mxCell id="15" value="&lt;font style=&quot;font-size: 3px;&quot;&gt;Localization error&lt;/font&gt;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="14"> <mxGeometry x="0.0166" relative="1" as="geometry"> <mxPoint x="-32" y="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="16" value="" style="endArrow=classic;html=1;rounded=0;endSize=1;entryX=0.286;entryY=0.8;entryDx=0;entryDy=0;entryPerimeter=0;" edge="1" parent="1"> <mxGeometry width="50" height="50" relative="1" as="geometry"> <mxPoint x="212" y="294" as="sourcePoint" /> <mxPoint x="229.01999999999992" y="284" as="targetPoint" /> </mxGeometry> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " style="background-color: rgb(255, 255, 255);" > - - - - - - - - -
-
-
- - - -
-
-
- + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + +
+
+
+ MPC +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ IMU +
+
+
+ + + + + + + + + + + + + + + + + +
+
+
+ Localization error +
+
+
+ + + + + + + + + + + + - - - - - - - - - - - - - - - - - - -
-
-
- IMU -
-
-
- - - - - - - - - - - - -
-
-
- MPC -
-
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- MPC path is wrong -
-
-
- - - diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 7e06af73b0f11..32bedd15c5c53 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ #define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include "autoware/universe_utils/system/time_keeper.hpp" + #include #include #include @@ -23,6 +25,7 @@ #include #include +#include #include #include #include @@ -30,6 +33,7 @@ #include #include #include +#include #include #include @@ -38,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +52,7 @@ #include #include #include +#include #include #include namespace autoware::motion::control::autonomous_emergency_braking @@ -59,8 +65,8 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; -using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; @@ -68,6 +74,13 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using colorTuple = std::tuple; + +/** + * @brief Struct to store object data + */ struct ObjectData { rclcpp::Time stamp; @@ -75,24 +88,47 @@ struct ObjectData double velocity{0.0}; double rss{0.0}; double distance_to_object{0.0}; + bool is_target{true}; }; +/** + * @brief Class to manage collision data + */ class CollisionDataKeeper { public: + /** + * @brief Constructor for CollisionDataKeeper + * @param clock Shared pointer to the clock + */ explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + /** + * @brief Set timeout values for collision and obstacle data + * @param collision_keep_time Time to keep collision data + * @param previous_obstacle_keep_time Time to keep previous obstacle data + */ void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) { collision_keep_time_ = collision_keep_time; previous_obstacle_keep_time_ = previous_obstacle_keep_time; } + /** + * @brief Get timeout values for collision and obstacle data + * @return Pair of collision and obstacle data timeout values + */ std::pair getTimeout() { return {collision_keep_time_, previous_obstacle_keep_time_}; } + /** + * @brief Check if object data has expired + * @param data Optional reference to the object data + * @param timeout Timeout value to check against + * @return True if object data has expired, false otherwise + */ bool checkObjectDataExpired(std::optional & data, const double timeout) { if (!data.has_value()) return true; @@ -106,38 +142,67 @@ class CollisionDataKeeper return false; } + /** + * @brief Check if collision data has expired + * @return True if collision data has expired, false otherwise + */ bool checkCollisionExpired() { return this->checkObjectDataExpired(closest_object_, collision_keep_time_); } + /** + * @brief Check if previous object data has expired + * @return True if previous object data has expired, false otherwise + */ bool checkPreviousObjectDataExpired() { return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); } - ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } - - ObjectData getPreviousObjectData() const + /** + * @brief Get the closest object data + * @return Object data of the closest object + */ + [[nodiscard]] std::optional get() const { return closest_object_; } + + /** + * @brief Get the previous closest object data + * @return Object data of the previous closest object + */ + [[nodiscard]] ObjectData getPreviousObjectData() const { return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); } + /** + * @brief Set collision data + * @param data Object data to set + */ void setCollisionData(const ObjectData & data) { closest_object_ = std::make_optional(data); } + /** + * @brief Set previous object data + * @param data Object data to set + */ void setPreviousObjectData(const ObjectData & data) { prev_closest_object_ = std::make_optional(data); } + /** + * @brief Reset the obstacle velocity history + */ void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + /** + * @brief Update the velocity history with current object velocity + * @param current_object_velocity Current object velocity + * @param current_object_velocity_time_stamp Timestamp of the current object velocity + */ void updateVelocityHistory( const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) { @@ -152,20 +217,25 @@ class CollisionDataKeeper }), obstacle_velocity_history_.end()); obstacle_velocity_history_.emplace_back( - std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + current_object_velocity, current_object_velocity_time_stamp); } - std::optional getMedianObstacleVelocity() const + /** + * @brief Get the median obstacle velocity from history + * @return Optional median obstacle velocity + */ + [[nodiscard]] std::optional getMedianObstacleVelocity() const { if (obstacle_velocity_history_.empty()) return std::nullopt; std::vector raw_velocities; + raw_velocities.reserve(obstacle_velocity_history_.size()); for (const auto & vel_time_pair : obstacle_velocity_history_) { raw_velocities.emplace_back(vel_time_pair.first); } const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 - : (raw_velocities.size()) / 2.0; - const size_t med2 = (raw_velocities.size()) / 2.0; + : (raw_velocities.size()) / 2; + const size_t med2 = (raw_velocities.size()) / 2; std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); const double vel1 = raw_velocities.at(med1); std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); @@ -173,9 +243,23 @@ class CollisionDataKeeper return (vel1 + vel2) / 2.0; } + /** + * @brief Calculate object speed from velocity history + * @param closest_object Closest object data + * @param path Ego vehicle path + * @param current_ego_speed Current ego vehicle speed + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed) { + // in case the object comes from predicted objects info, we reuse the speed. + if (std::abs(closest_object.velocity) > std::numeric_limits::epsilon()) { + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(closest_object.velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + if (this->checkPreviousObjectDataExpired()) { this->setPreviousObjectData(closest_object); this->resetVelocityHistory(); @@ -207,7 +291,7 @@ class CollisionDataKeeper p_vel * std::cos(p_yaw - traj_yaw) + std::abs(current_ego_speed); // Current RSS distance calculation does not account for negative velocities - return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; + return estimated_velocity; }); if (!estimated_velocity_opt.has_value()) { @@ -230,9 +314,16 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +/** + * @brief Autonomous Emergency Braking (AEB) node + */ class AEB : public rclcpp::Node { public: + /** + * @brief Constructor for AEB + * @param node_options Options for the node + */ explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber @@ -243,61 +334,206 @@ class AEB : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; autoware::universe_utils::InterProcessPollingSubscriber sub_predicted_traj_{ this, "~/input/predicted_trajectory"}; + autoware::universe_utils::InterProcessPollingSubscriber predicted_objects_sub_{ + this, "~/input/objects"}; autoware::universe_utils::InterProcessPollingSubscriber sub_autoware_state_{ this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; - rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug - + rclcpp::Publisher::SharedPtr debug_marker_publisher_; + rclcpp::Publisher::SharedPtr virtual_wall_publisher_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_pub_; + rclcpp::Publisher::SharedPtr debug_rss_distance_publisher_; // timer rclcpp::TimerBase::SharedPtr timer_; + mutable std::shared_ptr time_keeper_{nullptr}; // callback + /** + * @brief Callback for point cloud messages + * @param input_msg Shared pointer to the point cloud message + */ void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + + /** + * @brief Callback for IMU messages + * @param input_msg Shared pointer to the IMU message + */ void onImu(const Imu::ConstSharedPtr input_msg); + + /** + * @brief Timer callback function + */ void onTimer(); + + /** + * @brief Callback for parameter updates + * @param parameters Vector of updated parameters + * @return Set parameters result + */ rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); + /** + * @brief Fetch the latest data from subscribers + * @return True if data fetch was successful, false otherwise + */ bool fetchLatestData(); - // main function + /** + * @brief Diagnostic check for collisions + * @param stat Diagnostic status wrapper + */ void onCheckCollision(DiagnosticStatusWrapper & stat); + + /** + * @brief Check for collisions + * @param debug_markers Marker array for debugging + * @return True if a collision is detected, false otherwise + */ bool checkCollision(MarkerArray & debug_markers); + + /** + * @brief Check if there is a collision with the closest object + * @param current_v Current velocity of the ego vehicle + * @param closest_object Data of the closest object + * @return True if a collision is detected, false otherwise + */ bool hasCollision(const double current_v, const ObjectData & closest_object); + /** + * @brief Generate the ego vehicle path + * @param curr_v Current velocity of the ego vehicle + * @param curr_w Current angular velocity of the ego vehicle + * @return Generated ego path + */ Path generateEgoPath(const double curr_v, const double curr_w); + + /** + * @brief Generate the ego vehicle path from the predicted trajectory + * @param predicted_traj Predicted trajectory of the ego vehicle + * @return Optional generated ego path + */ std::optional generateEgoPath(const Trajectory & predicted_traj); + + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @param polygons vector to be filled with the polygons + * @return Vector of polygons representing the path footprint + */ + void generatePathFootprint( + const Path & path, const double extra_width_margin, std::vector & polygons); + + /** + * @brief Generate the footprint of the path with extra width margin + * @param path Ego vehicle path + * @param extra_width_margin Extra width margin for the footprint + * @return Vector of polygons representing the path footprint + */ std::vector generatePathFootprint(const Path & path, const double extra_width_margin); - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, - const pcl::PointCloud::Ptr obstacle_points_ptr); + /** + * @brief Create object data using point cloud clusters + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param speed_calc_ego_polys Polygons representing the expanded ego vehicle footprint for speed + * calculation area + * @param stamp Timestamp of the data + * @param objects Vector to store the created object data + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + */ + void getClosestObjectsOnPath( + const Path & ego_path, const rclcpp::Time & stamp, + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects); + + /** + * @brief Create object data using point cloud clusters + * @param obstacle_points_ptr Pointer to the point cloud of obstacles + * @param points_belonging_to_cluster_hulls output: pointer to the point cloud of points belonging + * to cluster hulls + */ + void getPointsBelongingToClusterHulls( + const PointCloud::Ptr obstacle_points_ptr, + const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers); + + /** + * @brief Create object data using predicted objects + * @param ego_path Ego vehicle path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param objects Vector to store the created object data + */ + void createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & objects); + /** + * @brief Crop the point cloud with the ego vehicle footprint path + * @param ego_polys Polygons representing the ego vehicle footprint + * @param filtered_objects Pointer to the filtered point cloud of obstacles + */ void cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); - void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - + /** + * @brief Add a marker for debugging + * @param current_time Current time + * @param path Ego vehicle path + * @param polygons Polygons representing the ego vehicle footprint + * @param objects Vector of object data + * @param closest_object Optional data of the closest object + * @param debug_colors Tuple of RGBA colors + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ void addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, const std::vector & objects, const std::optional & closest_object, - const double color_r, const double color_g, const double color_b, const double color_a, - const std::string & ns, MarkerArray & debug_markers); - + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); + + /** + * @brief Add a marker of convex hulls for debugging + * @param current_time Current time + * @param hulls vector of polygons of the convex hulls + * @param debug_colors Tuple of RGBA colors + * @param ns Namespace for the marker + * @param debug_markers Marker array for debugging + */ + void addClusterHullMarkers( + const rclcpp::Time & current_time, const std::vector & hulls, + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers); + + /** + * @brief Add a collision marker for debugging + * @param data Data of the collision object + * @param debug_markers Marker array for debugging + */ void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + /** + * @brief Add an info marker stop wall in front of the ego vehicle + * @param markers Data of the closest object + */ + void addVirtualStopWallMarker(MarkerArray & markers); + + /** + * @brief Calculate object speed from history + * @param closest_object Data of the closest object + * @param path Ego vehicle path + * @param current_ego_speed Current speed of the ego vehicle + * @return Optional calculated object speed + */ std::optional calcObjectSpeedFromHistory( const ObjectData & closest_object, const Path & path, const double current_ego_speed); + // Member variables PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + PredictedObjects::ConstSharedPtr predicted_objects_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; tf2_ros::Buffer tf_buffer_{get_clock()}; @@ -309,24 +545,32 @@ class AEB : public rclcpp::Node // diag Updater updater_{this}; - // member variables + // Member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; + bool publish_debug_time_; bool use_predicted_trajectory_; bool use_imu_path_; + bool use_pointcloud_data_; + bool use_predicted_object_data_; bool use_object_velocity_calculation_; + bool check_autoware_state_; double path_footprint_extra_margin_; + double speed_calculation_expansion_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; double a_ego_min_; double a_obj_min_; double cluster_tolerance_; + double cluster_minimum_height_; int minimum_cluster_size_; int maximum_cluster_size_; double imu_prediction_time_horizon_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp new file mode 100644 index 0000000000000..18f3716b755ee --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -0,0 +1,119 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ +#define AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ + +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#include + +#endif + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware::universe_utils::Polygon2d; +using autoware::universe_utils::Polygon3d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; + +/** + * @brief Apply a transform to a predicted object + * @param input the predicted object + * @param transform_stamped the tf2 transform + */ +PredictedObject transformObjectFrame( + const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped); + +/** + * @brief Get the predicted objects polygon as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects cylindrical shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); + +/** + * @brief Get the predicted objects bounding box shape as a geometry polygon + * @param current_pose the predicted object's pose + * @param obj_shape the object's shape + */ +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param obj the object + */ +Polygon2d convertObjToPolygon(const PredictedObject & obj); + +/** + * @brief Get the transform from source to target frame + * @param target_frame target frame + * @param source_frame source frame + * @param tf_buffer buffer of tf transforms + * @param logger node logger + */ +std::optional getTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon2d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); + +/** + * @brief Get the predicted object's shape as a geometry polygon + * @param polygons vector of Polygon3d + * @param polygon_marker marker to be filled with polygon points + */ +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker); +} // namespace autoware::motion::control::autonomous_emergency_braking::utils + +#endif // AUTOWARE__AUTONOMOUS_EMERGENCY_BRAKING__UTILS_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index 75b1a9dcf822d..34d0492799577 100644 --- a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -3,8 +3,8 @@ - + @@ -15,5 +15,6 @@ + diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..a4adb7087e448 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,16 +8,22 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 ament_cmake autoware_cmake + ament_cmake_ros + ament_lint_auto + autoware_lint_common + autoware_test_utils autoware_control_msgs autoware_motion_utils autoware_planning_msgs autoware_system_msgs + autoware_test_utils autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -35,11 +41,9 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_debug_msgs visualization_msgs - ament_lint_auto - autoware_lint_common - ament_cmake diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 90cf3f900673e..c785ab661060d 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -12,16 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/autonomous_emergency_braking/node.hpp" - +#include +#include +#include +#include #include #include #include #include #include #include +#include + +#include #include +#include +#include #include #include @@ -33,8 +40,13 @@ #include #include #include -#include #include + +#include +#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -46,6 +58,9 @@ namespace autoware::motion::control::autonomous_emergency_braking { +using autoware::motion::control::autonomous_emergency_braking::utils::convertObjToPolygon; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Point3d; using diagnostic_msgs::msg::DiagnosticStatus; namespace bg = boost::geometry; @@ -58,6 +73,16 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +void appendPointToPolygon(Polygon3d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point3d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + point.z() = geom_point.z; + + bg::append(polygon.outer(), point); +} + Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) @@ -100,7 +125,7 @@ Polygon2d createPolygon( Polygon2d hull_polygon; bg::convex_hull(polygon, hull_polygon); - + bg::correct(hull_polygon); return hull_polygon; } @@ -113,7 +138,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) { pub_obstacle_pointcloud_ = this->create_publisher("~/debug/obstacle_pointcloud", 1); - debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + debug_marker_publisher_ = this->create_publisher("~/debug/markers", 1); + virtual_wall_publisher_ = this->create_publisher("~/virtual_wall", 1); + debug_rss_distance_publisher_ = + this->create_publisher("~/debug/rss_distance", 1); } // Diagnostics { @@ -122,17 +150,24 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); + use_predicted_object_data_ = declare_parameter("use_predicted_object_data"); use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); + check_autoware_state_ = declare_parameter("check_autoware_state"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); + speed_calculation_expansion_margin_ = + declare_parameter("speed_calculation_expansion_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = declare_parameter("detection_range_max_height_margin"); voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -140,6 +175,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) a_obj_min_ = declare_parameter("a_obj_min"); cluster_tolerance_ = declare_parameter("cluster_tolerance"); + cluster_minimum_height_ = declare_parameter("cluster_minimum_height"); minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); @@ -163,6 +199,12 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) const double aeb_hz = declare_parameter("aeb_hz"); const auto period_ns = rclcpp::Rate(aeb_hz).period(); timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); + + debug_processing_time_detail_pub_ = + create_publisher( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(debug_processing_time_detail_pub_); } rcl_interfaces::msg::SetParametersResult AEB::onParameter( @@ -170,18 +212,25 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); + updateParam(parameters, "use_predicted_object_data", use_predicted_object_data_); updateParam( parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); + updateParam(parameters, "check_autoware_state", check_autoware_state_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam( + parameters, "speed_calculation_expansion_margin", speed_calculation_expansion_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); updateParam(parameters, "voxel_grid_x", voxel_grid_x_); updateParam(parameters, "voxel_grid_y", voxel_grid_y_); updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -189,6 +238,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "a_obj_min", a_obj_min_); updateParam(parameters, "cluster_tolerance", cluster_tolerance_); + updateParam(parameters, "cluster_minimum_height", cluster_minimum_height_); updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); @@ -218,24 +268,18 @@ void AEB::onTimer() void AEB::onImu(const Imu::ConstSharedPtr input_msg) { // transform imu - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); - return; - } + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", input_msg->header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return; angular_velocity_ptr_ = std::make_shared(); - tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped.value()); } void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); PointCloud::Ptr pointcloud_ptr(new PointCloud); pcl::fromROSMsg(*input_msg, *pointcloud_ptr); @@ -244,22 +288,15 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) get_logger(), "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); - return; - } + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", input_msg->header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return; // transform by using eigen matrix PointCloud2 transformed_points{}; const Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); pcl::fromROSMsg(transformed_points, *pointcloud_ptr); } @@ -298,34 +335,61 @@ bool AEB::fetchLatestData() return missing("ego velocity"); } - const auto pointcloud_ptr = sub_point_cloud_.takeData(); - if (!pointcloud_ptr) { - return missing("object pointcloud message"); + if (use_pointcloud_data_) { + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + + onPointCloud(pointcloud_ptr); + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + } else { + obstacle_ros_pointcloud_ptr_.reset(); + } + + if (use_predicted_object_data_) { + predicted_objects_ptr_ = predicted_objects_sub_.takeData(); + if (!predicted_objects_ptr_) { + return missing("predicted objects"); + } + } else { + predicted_objects_ptr_.reset(); } - onPointCloud(pointcloud_ptr); - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); + + if (!obstacle_ros_pointcloud_ptr_ && !predicted_objects_ptr_) { + return missing("object detection method (pointcloud or predicted objects)"); } - const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { + const bool has_imu_path = std::invoke([&]() { + if (!use_imu_path_) return false; + const auto imu_ptr = sub_imu_.takeData(); if (!imu_ptr) { return missing("imu message"); } // imu_ptr is valid onImu(imu_ptr); - } - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } + return (!angular_velocity_ptr_) ? missing("imu") : true; + }); - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); + const bool has_predicted_path = std::invoke([&]() { + if (!use_predicted_trajectory_) { + return false; + } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + return (!predicted_traj_ptr_) ? missing("control predicted trajectory") : true; + }); + + if (!has_imu_path && !has_predicted_path) { + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, + "[AEB] At least one path (IMU or predicted trajectory) is required for operation"); + return false; } autoware_state_ = sub_autoware_state_.takeData(); - if (!autoware_state_) { + if (check_autoware_state_ && !autoware_state_) { return missing("autoware_state"); } @@ -335,6 +399,7 @@ bool AEB::fetchLatestData() void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) { MarkerArray debug_markers; + MarkerArray virtual_wall_marker; checkCollision(debug_markers); if (!collision_data_keeper_.checkCollisionExpired()) { @@ -342,10 +407,15 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) const auto diag_level = DiagnosticStatus::ERROR; stat.summary(diag_level, error_msg); const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); + if (data.has_value()) { + stat.addf("RSS", "%.2f", data.value().rss); + stat.addf("Distance", "%.2f", data.value().distance_to_object); + stat.addf("Object Speed", "%.2f", data.value().velocity); + if (publish_debug_markers_) { + addCollisionMarker(data.value(), debug_markers); + } + } + addVirtualStopWallMarker(virtual_wall_marker); } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -353,12 +423,13 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) } // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); + debug_marker_publisher_->publish(debug_markers); + virtual_wall_publisher_->publish(virtual_wall_marker); } bool AEB::checkCollision(MarkerArray & debug_markers) { - using colorTuple = std::tuple; + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // step1. check data if (!fetchLatestData()) { @@ -366,7 +437,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) } // if not driving, disable aeb - if (autoware_state_->state != AutowareState::DRIVING) { + if (check_autoware_state_ && autoware_state_->state != AutowareState::DRIVING) { return false; } @@ -377,118 +448,192 @@ bool AEB::checkCollision(MarkerArray & debug_markers) return false; } - auto check_collision = [&]( - const auto & path, const colorTuple & debug_colors, - const std::string & debug_ns, - pcl::PointCloud::Ptr filtered_objects) { - // Crop out Pointcloud using an extra wide ego path - const auto expanded_ego_polys = - generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); - cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + auto merge_expanded_path_polys = [&](const std::vector & paths) { + std::vector merged_expanded_path_polygons; + for (const auto & path : paths) { + generatePathFootprint( + path, expand_width_ + path_footprint_extra_margin_, merged_expanded_path_polygons); + } + return merged_expanded_path_polygons; + }; + auto get_objects_on_path = [&]( + const auto & path, PointCloud::Ptr points_belonging_to_cluster_hulls, + const colorTuple & debug_colors, const std::string & debug_ns) { // Check which points of the cropped point cloud are on the ego path, and get the closest one - std::vector objects_from_point_clusters; const auto ego_polys = generatePathFootprint(path, expand_width_); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + std::vector objects; + // Crop out Pointcloud using an extra wide ego path + if ( + use_pointcloud_data_ && points_belonging_to_cluster_hulls && + !points_belonging_to_cluster_hulls->empty()) { + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); + } + if (use_predicted_object_data_) { + createObjectDataUsingPredictedObjects(path, ego_polys, objects); + } + + // Add debug markers + if (publish_debug_markers_) { + addMarker( + this->get_clock()->now(), path, ego_polys, objects, collision_data_keeper_.get(), + debug_colors, debug_ns, debug_markers); + } + return objects; + }; - // Get only the closest object and calculate its speed + auto check_collision = [&](const Path & path, std::vector & objects) { + time_keeper_->start_track("has_collision"); const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = std::min_element( - objects_from_point_clusters.begin(), objects_from_point_clusters.end(), - [](const auto & o1, const auto & o2) { + // Attempt to find the closest object + const auto closest_object_itr = + std::min_element(objects.begin(), objects.end(), [](const auto & o1, const auto & o2) { + // target objects have priority + if (o1.is_target != o2.is_target) { + return o1.is_target; + } return o1.distance_to_object < o2.distance_to_object; }); - if (closest_object_point_itr == objects_from_point_clusters.end()) { - return std::nullopt; + if (closest_object_itr != objects.end()) { + // Calculate speed for the closest object + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_itr, path, current_v) + : std::make_optional(0.0); + + if (closest_object_speed.has_value()) { + closest_object_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_itr); + } } - const auto closest_object_speed = (use_object_velocity_calculation_) - ? collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v) - : std::make_optional(0.0); - if (!closest_object_speed.has_value()) { - return std::nullopt; - } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); + + return std::nullopt; }); - // Add debug markers - { - const auto [color_r, color_g, color_b, color_a] = debug_colors; - addMarker( - this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, - closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); - } + const bool has_collision = + (closest_object_point.has_value() && closest_object_point.value().is_target) + ? hasCollision(current_v, closest_object_point.value()) + : false; + + time_keeper_->end_track("has_collision"); // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; + return has_collision; }; // step3. make function to check collision with ego path created with sensor data - const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_imu_path_) return false; - const double current_w = angular_velocity_ptr_->z; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "ego"; - const auto ego_path = generateEgoPath(current_v, current_w); - - return check_collision(ego_path, debug_color, ns, filtered_objects); - }; - - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_predicted = - [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; - const auto predicted_traj_ptr = predicted_traj_ptr_; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); + const auto ego_imu_path = (!use_imu_path_ || !angular_velocity_ptr_) + ? Path{} + : generateEgoPath(current_v, angular_velocity_ptr_->z); + + const auto ego_mpc_path = (!use_predicted_trajectory_ || !predicted_traj_ptr_) + ? std::nullopt + : generateEgoPath(*predicted_traj_ptr_); + + PointCloud::Ptr filtered_objects = pcl::make_shared(); + if (use_pointcloud_data_) { + const std::vector paths = [&]() { + std::vector paths; + if (use_imu_path_) paths.push_back(ego_imu_path); + if (ego_mpc_path.has_value()) { + paths.push_back(ego_mpc_path.value()); + } + return paths; + }(); - if (!predicted_path_opt) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "predicted"; - const auto & predicted_path = predicted_path_opt.value(); + if (paths.empty()) return false; + const std::vector merged_path_polygons = merge_expanded_path_polys(paths); + // Data of filtered point cloud + cropPointCloudWithEgoFootprintPath(merged_path_polygons, filtered_objects); + } - return check_collision(predicted_path, debug_color, ns, filtered_objects); + PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); + getPointsBelongingToClusterHulls( + filtered_objects, points_belonging_to_cluster_hulls, debug_markers); + + const auto imu_path_objects = (!use_imu_path_ || !angular_velocity_ptr_) + ? std::vector{} + : get_objects_on_path( + ego_imu_path, points_belonging_to_cluster_hulls, + {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}, "imu"); + + const auto mpc_path_objects = + (!use_predicted_trajectory_ || !predicted_traj_ptr_ || !ego_mpc_path.has_value()) + ? std::vector{} + : get_objects_on_path( + ego_mpc_path.value(), points_belonging_to_cluster_hulls, + {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}, "mpc"); + + // merge object data which comes from the ego (imu) path and predicted path + auto merge_objects = + [&](const std::vector & imu_objects, const std::vector & mpc_objects) { + std::vector merged_objects = imu_objects; + merged_objects.insert(merged_objects.end(), mpc_objects.begin(), mpc_objects.end()); + return merged_objects; + }; + + auto merged_imu_mpc_objects = merge_objects(imu_path_objects, mpc_path_objects); + if (merged_imu_mpc_objects.empty()) return false; + + // merge path points for the collision checking + auto merge_paths = [&](const std::optional & mpc_path, const Path & imu_path) { + if (!mpc_path.has_value()) { + return imu_path; + } + Path merged_path = imu_path; // Start with imu_path + merged_path.insert( + merged_path.end(), mpc_path.value().begin(), mpc_path.value().end()); // Append mpc_path + return merged_path; }; - // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); - // evaluate if there is a collision for both paths - const bool has_collision = - has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); + auto merge_imu_mpc_path = merge_paths(ego_mpc_path, ego_imu_path); + if (merge_imu_mpc_path.empty()) return false; + + // evaluate if there is a collision for merged (imu and mpc) paths + const bool has_collision = check_collision(merge_imu_mpc_path, merged_imu_mpc_objects); // Debug print if (!filtered_objects->empty() && publish_debug_pointcloud_) { - const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); - pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); - pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); + const auto filtered_objects_ros_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr); + pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr); } return has_collision; } bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) { - const double & obj_v = closest_object.velocity; - const double & t = t_response_; - const double rss_dist = std::abs(current_v) * t + - (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - if (closest_object.distance_to_object < rss_dist) { - // collision happens - ObjectData collision_data = closest_object; - collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; - collision_data_keeper_.setCollisionData(collision_data); - return true; - } - return false; + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const double rss_dist = std::invoke([&]() { + const double & obj_v = closest_object.velocity; + const double & t = t_response_; + const double pre_braking_covered_distance = std::abs(current_v) * t; + const double braking_distance = (current_v * current_v) / (2 * std::fabs(a_ego_min_)); + const double ego_stopping_distance = pre_braking_covered_distance + braking_distance; + const double obj_braking_distance = (obj_v > 0.0) + ? -(obj_v * obj_v) / (2 * std::fabs(a_obj_min_)) + : (obj_v * obj_v) / (2 * std::fabs(a_obj_min_)); + return ego_stopping_distance + obj_braking_distance + longitudinal_offset_; + }); + + tier4_debug_msgs::msg::Float32Stamped rss_distance_msg; + rss_distance_msg.stamp = get_clock()->now(); + rss_distance_msg.data = rss_dist; + debug_rss_distance_publisher_->publish(rss_distance_msg); + + if (closest_object.distance_to_object > rss_dist) return false; + + // collision happens + ObjectData collision_data = closest_object; + collision_data.rss = rss_dist; + collision_data_keeper_.setCollisionData(collision_data); + return true; } Path AEB::generateEgoPath(const double curr_v, const double curr_w) { + autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(IMU)", *time_keeper_); Path path; double curr_x = 0.0; double curr_y = 0.0; @@ -497,39 +642,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = std::abs(curr_v) * dt; + constexpr double minimum_distance_between_points{1e-2}; + // if current velocity is too small, assume it stops at the same point + // if distance between points is too small, arc length calculation is unreliable, so we skip + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = 1e-6; - const double & dt = imu_prediction_time_interval_; const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -537,38 +678,60 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) { + autoware::universe_utils::ScopedTimeTrack st(std::string(__func__) + "(MPC)", *time_keeper_); if (predicted_traj.points.empty()) { return std::nullopt; } - - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); - return std::nullopt; - } - + const auto logger = get_logger(); + const auto transform_stamped = + utils::getTransform("base_link", predicted_traj.header.frame_id, tf_buffer_, logger); + if (!transform_stamped.has_value()) return std::nullopt; // create path + time_keeper_->start_track("createPath"); Path path; - path.resize(predicted_traj.points.size()); + path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; - tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); - path.at(i) = map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); + + // skip points that are too close to the last point in the path + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { + continue; + } + + path.push_back(map_pose); if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { break; } } - return path; + time_keeper_->end_track("createPath"); + return (!path.empty()) ? std::make_optional(path) : std::nullopt; +} + +void AEB::generatePathFootprint( + const Path & path, const double extra_width_margin, std::vector & polygons) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return; + } + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.push_back( + createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); + } } std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( @@ -577,17 +740,97 @@ std::vector AEB::generatePathFootprint( return polygons; } -void AEB::createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) +void AEB::createObjectDataUsingPredictedObjects( + const Path & ego_path, const std::vector & ego_polys, + std::vector & object_data_vector) { - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { - return; - } + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; + + const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; + const auto & objects = predicted_objects_ptr_->objects; + const auto & stamp = predicted_objects_ptr_->header.stamp; + + // Ego position + const auto current_p = [&]() { + const auto & first_point_of_path = ego_path.front(); + const auto & p = first_point_of_path.position; + return autoware::universe_utils::createPoint(p.x, p.y, p.z); + }(); + + auto get_object_tangent_velocity = + [&](const PredictedObject & predicted_object, const auto & obj_pose) { + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); + + const auto obj_yaw = tf2::getYaw(obj_pose.orientation); + const auto obj_idx = autoware::motion_utils::findNearestIndex(ego_path, obj_pose.position); + const auto path_yaw = (current_ego_speed > 0.0) + ? tf2::getYaw(ego_path.at(obj_idx).orientation) + : tf2::getYaw(ego_path.at(obj_idx).orientation) + M_PI; + return obj_vel_norm * std::cos(obj_yaw - path_yaw); + }; + + const auto logger = get_logger(); + const auto transform_stamped_opt = + utils::getTransform("base_link", predicted_objects_ptr_->header.frame_id, tf_buffer_, logger); + if (!transform_stamped_opt.has_value()) return; + // Check which objects collide with the ego footprints + std::for_each(objects.begin(), objects.end(), [&](const auto & predicted_object) { + // get objects in base_link frame + const auto & transform_stamped = transform_stamped_opt.value(); + const auto t_predicted_object = + utils::transformObjectFrame(predicted_object, transform_stamped); + const auto & obj_pose = t_predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto obj_poly = convertObjToPolygon(t_predicted_object); + const double obj_tangent_velocity = get_object_tangent_velocity(t_predicted_object, obj_pose); + + for (const auto & ego_poly : ego_polys) { + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(ego_poly, obj_poly, collision_points_bg); + if (collision_points_bg.empty()) continue; + + // Create an object for each intersection point + bool collision_points_added{false}; + for (const auto & collision_point : collision_points_bg) { + const auto obj_position = + autoware::universe_utils::createPoint(collision_point.x(), collision_point.y(), 0.0); + const double obj_arc_length = + autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); + if (std::isnan(obj_arc_length)) continue; + + // If the object is behind the ego, we need to use the backward long offset. The + // distance should be a positive number in any case + const bool is_object_in_front_of_ego = obj_arc_length > 0.0; + const double dist_ego_to_object = + (is_object_in_front_of_ego) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m + : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; + + ObjectData obj; + obj.stamp = stamp; + obj.position = obj_position; + obj.velocity = obj_tangent_velocity; + obj.distance_to_object = std::abs(dist_ego_to_object); + object_data_vector.push_back(obj); + collision_points_added = true; + } + // The ego polygons are in order, so the first intersection points found are the closest + // points. It is not necessary to continue iterating the ego polys for the same object. + if (collision_points_added) break; + } + }); +} +void AEB::getPointsBelongingToClusterHulls( + const PointCloud::Ptr obstacle_points_ptr, + const PointCloud::Ptr points_belonging_to_cluster_hulls, MarkerArray & debug_markers) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // eliminate noisy points by only considering points belonging to clusters of at least a certain // size + if (obstacle_points_ptr->empty()) return; const std::vector cluster_indices = std::invoke([&]() { std::vector cluster_idx; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); @@ -601,25 +844,55 @@ void AEB::createObjectDataUsingPointCloudClusters( ec.extract(cluster_idx); return cluster_idx; }); - - PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); + std::vector hull_polygons; for (const auto & indices : cluster_indices) { PointCloud::Ptr cluster(new PointCloud); + bool cluster_surpasses_threshold_height{false}; for (const auto & index : indices.indices) { - cluster->push_back((*obstacle_points_ptr)[index]); + const auto & p = (*obstacle_points_ptr)[index]; + cluster_surpasses_threshold_height = (cluster_surpasses_threshold_height) + ? cluster_surpasses_threshold_height + : (p.z > cluster_minimum_height_); + cluster->push_back(p); } + if (!cluster_surpasses_threshold_height) continue; // Make a 2d convex hull for the objects pcl::ConvexHull hull; hull.setDimension(2); hull.setInputCloud(cluster); std::vector polygons; - PointCloud::Ptr surface_hull(new pcl::PointCloud); + PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); + Polygon3d hull_polygon; for (const auto & p : *surface_hull) { points_belonging_to_cluster_hulls->push_back(p); + if (publish_debug_markers_) { + const auto geom_point = autoware::universe_utils::createPoint(p.x, p.y, p.z); + appendPointToPolygon(hull_polygon, geom_point); + } } + hull_polygons.push_back(hull_polygon); + } + if (publish_debug_markers_ && !hull_polygons.empty()) { + constexpr colorTuple debug_color = {255.0 / 256.0, 51.0 / 256.0, 255.0 / 256.0, 0.999}; + addClusterHullMarkers(now(), hull_polygons, debug_color, "hulls", debug_markers); } +} +void AEB::getClosestObjectsOnPath( + const Path & ego_path, const rclcpp::Time & stamp, + const PointCloud::Ptr points_belonging_to_cluster_hulls, std::vector & objects) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // check if the predicted path has a valid number of points + if (ego_path.size() < 2 || points_belonging_to_cluster_hulls->empty()) { + return; + } + const auto ego_is_driving_forward_opt = autoware::motion_utils::isDrivingForward(ego_path); + if (!ego_is_driving_forward_opt.has_value()) { + return; + } + const bool ego_is_driving_forward = ego_is_driving_forward_opt.value(); // select points inside the ego footprint path const auto current_p = [&]() { const auto & first_point_of_path = ego_path.front(); @@ -627,38 +900,51 @@ void AEB::createObjectDataUsingPointCloudClusters( return autoware::universe_utils::createPoint(p.x, p.y, p.z); }(); + const auto path_length = autoware::motion_utils::calcArcLength(ego_path); for (const auto & p : *points_belonging_to_cluster_hulls) { const auto obj_position = autoware::universe_utils::createPoint(p.x, p.y, p.z); const double obj_arc_length = autoware::motion_utils::calcSignedArcLength(ego_path, current_p, obj_position); - if (std::isnan(obj_arc_length)) continue; + if ( + std::isnan(obj_arc_length) || + obj_arc_length > path_length + vehicle_info_.max_longitudinal_offset_m) + continue; + + // calculate the lateral offset between the ego vehicle and the object + const double lateral_offset = + std::abs(autoware::motion_utils::calcLateralOffset(ego_path, obj_position)); + + if (std::isnan(lateral_offset)) continue; + + // object is outside region of interest + if ( + lateral_offset > + vehicle_info_.vehicle_width_m / 2.0 + expand_width_ + speed_calculation_expansion_margin_) { + continue; + } // If the object is behind the ego, we need to use the backward long offset. The distance should // be a positive number in any case - const bool is_object_in_front_of_ego = obj_arc_length > 0.0; - const double dist_ego_to_object = (is_object_in_front_of_ego) + const double dist_ego_to_object = (ego_is_driving_forward) ? obj_arc_length - vehicle_info_.max_longitudinal_offset_m : obj_arc_length + vehicle_info_.min_longitudinal_offset_m; - ObjectData obj; obj.stamp = stamp; obj.position = obj_position; obj.velocity = 0.0; obj.distance_to_object = std::abs(dist_ego_to_object); - - const Point2d obj_point(p.x, p.y); - for (const auto & ego_poly : ego_polys) { - if (bg::within(obj_point, ego_poly)) { - objects.push_back(obj); - break; - } - } + obj.is_target = (lateral_offset < vehicle_info_.vehicle_width_m / 2.0 + expand_width_); + objects.push_back(obj); } } void AEB::cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) + const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (ego_polys.empty()) { + return; + } PointCloud::Ptr full_points_ptr(new PointCloud); pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); // Create a Point cloud with the points of the ego footprint @@ -674,7 +960,7 @@ void AEB::cropPointCloudWithEgoFootprintPath( hull.setDimension(2); hull.setInputCloud(path_polygon_points); std::vector polygons; - pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); + PointCloud::Ptr surface_hull(new PointCloud); hull.reconstruct(*surface_hull, polygons); // Filter out points outside of the path's convex hull pcl::CropHull path_polygon_hull_filter; @@ -683,22 +969,39 @@ void AEB::cropPointCloudWithEgoFootprintPath( path_polygon_hull_filter.setHullIndices(polygons); path_polygon_hull_filter.setHullCloud(surface_hull); path_polygon_hull_filter.filter(*filtered_objects); - filtered_objects->header.stamp = full_points_ptr->header.stamp; + filtered_objects->header = full_points_ptr->header; +} + +void AEB::addClusterHullMarkers( + const rclcpp::Time & current_time, const std::vector & hulls, + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto [color_r, color_g, color_b, color_a] = debug_colors; + + auto hull_marker = autoware::universe_utils::createDefaultMarker( + "base_link", current_time, ns, 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + utils::fillMarkerFromPolygon(hulls, hull_marker); + debug_markers.markers.push_back(hull_marker); } void AEB::addMarker( const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, const std::vector & objects, const std::optional & closest_object, - const double color_r, const double color_g, const double color_b, const double color_a, - const std::string & ns, MarkerArray & debug_markers) + const colorTuple & debug_colors, const std::string & ns, MarkerArray & debug_markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto [color_r, color_g, color_b, color_a] = debug_colors; + auto path_marker = autoware::universe_utils::createDefaultMarker( "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, autoware::universe_utils::createMarkerScale(0.2, 0.2, 0.2), autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - path_marker.points.resize(path.size()); - for (size_t i = 0; i < path.size(); ++i) { - path_marker.points.at(i) = path.at(i).position; + path_marker.points.reserve(path.size()); + for (const auto & p : path) { + path_marker.points.push_back(p.position); } debug_markers.markers.push_back(path_marker); @@ -706,18 +1009,7 @@ void AEB::addMarker( "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, autoware::universe_utils::createMarkerScale(0.03, 0.0, 0.0), autoware::universe_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - for (const auto & poly : polygons) { - for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { - const auto & boost_cp = poly.outer().at(dp_idx); - const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = - autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = - autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); - polygon_marker.points.push_back(curr_point); - polygon_marker.points.push_back(next_point); - } - } + utils::fillMarkerFromPolygon(polygons, polygon_marker); debug_markers.markers.push_back(polygon_marker); auto object_data_marker = autoware::universe_utils::createDefaultMarker( @@ -743,13 +1035,44 @@ void AEB::addMarker( "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; closest_object_velocity_marker_array.text += "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) + - " [m/s]"; + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object distance to ego: " + std::to_string(obj.distance_to_object) + " [m]\n"; + closest_object_velocity_marker_array.text += + "RSS distance: " + std::to_string(obj.rss) + " [m]"; debug_markers.markers.push_back(closest_object_velocity_marker_array); } } +void AEB::addVirtualStopWallMarker(MarkerArray & markers) +{ + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto ego_map_pose = std::invoke([this]() -> std::optional { + const auto logger = get_logger(); + const auto tf_current_pose = utils::getTransform("map", "base_link", tf_buffer_, logger); + if (!tf_current_pose.has_value()) return std::nullopt; + const auto transform = tf_current_pose.value().transform; + geometry_msgs::msg::Pose p; + p.orientation = transform.rotation; + p.position.x = transform.translation.x; + p.position.y = transform.translation.y; + p.position.z = transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0); + const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker( + ego_front_pose, "autonomous_emergency_braking", this->now(), 0); + autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers); + } +} + void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); auto point_marker = autoware::universe_utils::createDefaultMarker( "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3), diff --git a/control/autoware_autonomous_emergency_braking/src/utils.cpp b/control/autoware_autonomous_emergency_braking/src/utils.cpp new file mode 100644 index 0000000000000..5d9782ada5fbd --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/utils.cpp @@ -0,0 +1,206 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +namespace autoware::motion::control::autonomous_emergency_braking::utils +{ +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; + +PredictedObject transformObjectFrame( + const PredictedObject & input, const geometry_msgs::msg::TransformStamped & transform_stamped) +{ + PredictedObject output = input; + const auto & linear_twist = input.kinematics.initial_twist_with_covariance.twist.linear; + const auto & angular_twist = input.kinematics.initial_twist_with_covariance.twist.angular; + const auto & pose = input.kinematics.initial_pose_with_covariance.pose; + + geometry_msgs::msg::Pose t_pose; + Vector3 t_linear_twist; + Vector3 t_angular_twist; + + tf2::doTransform(pose, t_pose, transform_stamped); + tf2::doTransform(linear_twist, t_linear_twist, transform_stamped); + tf2::doTransform(angular_twist, t_angular_twist, transform_stamped); + + output.kinematics.initial_pose_with_covariance.pose = t_pose; + output.kinematics.initial_twist_with_covariance.twist.linear = t_linear_twist; + output.kinematics.initial_twist_with_covariance.twist.angular = t_angular_twist; + return output; +} + +Polygon2d convertPolygonObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + if (obj_shape.footprint.points.empty()) { + return {}; + } + Polygon2d object_polygon; + tf2::Transform tf_map2obj; + fromMsg(current_pose, tf_map2obj); + const auto obj_points = obj_shape.footprint.points; + object_polygon.outer().reserve(obj_points.size() + 1); + for (const auto & obj_point : obj_points) { + tf2::Vector3 obj(obj_point.x, obj_point.y, obj_point.z); + tf2::Vector3 tf_obj = tf_map2obj * obj; + object_polygon.outer().emplace_back(tf_obj.x(), tf_obj.y()); + } + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertCylindricalObjectToGeometryPolygon( + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) +{ + Polygon2d object_polygon; + + const double obj_x = current_pose.position.x; + const double obj_y = current_pose.position.y; + + constexpr int n = 20; + const double r = obj_shape.dimensions.x / 2; + object_polygon.outer().reserve(n + 1); + for (int i = 0; i < n; ++i) { + object_polygon.outer().emplace_back( + obj_x + r * std::cos(2.0 * M_PI / n * i), obj_y + r * std::sin(2.0 * M_PI / n * i)); + } + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertBoundingBoxObjectToGeometryPolygon( + const Pose & current_pose, const double & base_to_front, const double & base_to_rear, + const double & base_to_width) +{ + const auto mapped_point = [](const double & length_scalar, const double & width_scalar) { + tf2::Vector3 map; + map.setX(length_scalar); + map.setY(width_scalar); + map.setZ(0.0); + map.setW(1.0); + return map; + }; + + // set vertices at map coordinate + const tf2::Vector3 p1_map = std::invoke(mapped_point, base_to_front, -base_to_width); + const tf2::Vector3 p2_map = std::invoke(mapped_point, base_to_front, base_to_width); + const tf2::Vector3 p3_map = std::invoke(mapped_point, -base_to_rear, base_to_width); + const tf2::Vector3 p4_map = std::invoke(mapped_point, -base_to_rear, -base_to_width); + + // transform vertices from map coordinate to object coordinate + tf2::Transform tf_map2obj; + tf2::fromMsg(current_pose, tf_map2obj); + const tf2::Vector3 p1_obj = tf_map2obj * p1_map; + const tf2::Vector3 p2_obj = tf_map2obj * p2_map; + const tf2::Vector3 p3_obj = tf_map2obj * p3_map; + const tf2::Vector3 p4_obj = tf_map2obj * p4_map; + + Polygon2d object_polygon; + object_polygon.outer().reserve(5); + object_polygon.outer().emplace_back(p1_obj.x(), p1_obj.y()); + object_polygon.outer().emplace_back(p2_obj.x(), p2_obj.y()); + object_polygon.outer().emplace_back(p3_obj.x(), p3_obj.y()); + object_polygon.outer().emplace_back(p4_obj.x(), p4_obj.y()); + + object_polygon.outer().push_back(object_polygon.outer().front()); + boost::geometry::correct(object_polygon); + + return object_polygon; +} + +Polygon2d convertObjToPolygon(const PredictedObject & obj) +{ + Polygon2d object_polygon{}; + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + const double & length_m = obj.shape.dimensions.x / 2; + const double & width_m = obj.shape.dimensions.y / 2; + object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + object_polygon = utils::convertPolygonObjectToGeometryPolygon( + obj.kinematics.initial_pose_with_covariance.pose, obj.shape); + } else { + throw std::runtime_error("Unsupported shape type"); + } + return object_polygon; +} + +std::optional getTransform( + const std::string & target_frame, const std::string & source_frame, + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger) +{ + geometry_msgs::msg::TransformStamped tf_current_pose; + try { + tf_current_pose = tf_buffer.lookupTransform( + target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + logger, "[AEB] Failed to look up transform from " + source_frame + " to " + target_frame); + return std::nullopt; + } + return std::make_optional(tf_current_pose); +} + +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + +void fillMarkerFromPolygon( + const std::vector & polygons, visualization_msgs::msg::Marker & polygon_marker) +{ + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = + autoware::universe_utils::createPoint(boost_cp.x(), boost_cp.y(), boost_cp.z()); + const auto next_point = + autoware::universe_utils::createPoint(boost_np.x(), boost_np.y(), boost_np.z()); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } +} + +} // namespace autoware::motion::control::autonomous_emergency_braking::utils diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp new file mode 100644 index 0000000000000..c2a58941cc144 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -0,0 +1,351 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test.hpp" + +#include "autoware/autonomous_emergency_braking/node.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::Header; + +Header get_header(const char * const frame_id, rclcpp::Time t) +{ + std_msgs::msg::Header header; + header.stamp = t; + header.frame_id = frame_id; + return header; +}; + +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z) +{ + Imu imu_msg; + imu_msg.header = header; + imu_msg.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); + imu_msg.angular_velocity.z = angular_velocity_z; + imu_msg.linear_acceleration.x = ax; + imu_msg.linear_acceleration.y = ay; + return imu_msg; +}; + +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate) +{ + VelocityReport velocity_msg; + velocity_msg.header = header; + velocity_msg.lateral_velocity = lat_velocity; + velocity_msg.longitudinal_velocity = long_velocity; + velocity_msg.heading_rate = heading_rate; + return velocity_msg; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto aeb_dir = + ament_index_cpp::get_package_share_directory("autoware_autonomous_emergency_braking"); + const auto vehicle_info_util_dir = + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); + + node_options.arguments( + {"--ros-args", "--params-file", aeb_dir + "/config/autonomous_emergency_braking.param.yaml", + "--ros-args", "--params-file", vehicle_info_util_dir + "/config/vehicle_info.param.yaml"}); + return std::make_shared(node_options); +}; + +std::shared_ptr generatePubSubNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments({"--ros-args"}); + return std::make_shared(node_options); +}; + +PubSubNode::PubSubNode(const rclcpp::NodeOptions & node_options) +: Node("test_aeb_pubsub", node_options) +{ + rclcpp::QoS qos{1}; + qos.transient_local(); + + pub_imu_ = create_publisher("~/input/imu", qos); + pub_point_cloud_ = create_publisher("~/input/pointcloud", qos); + pub_velocity_ = create_publisher("~/input/velocity", qos); + pub_predicted_traj_ = create_publisher("~/input/predicted_trajectory", qos); + pub_predicted_objects_ = create_publisher("~/input/objects", qos); + pub_autoware_state_ = create_publisher("autoware/state", qos); +} + +TEST_F(TestAEB, checkCollision) +{ + constexpr double longitudinal_velocity = 3.0; + ObjectData object_collision; + object_collision.distance_to_object = 0.5; + object_collision.velocity = 0.1; + object_collision.position.x = 1.0; + object_collision.position.y = 1.0; + ASSERT_TRUE(aeb_node_->hasCollision(longitudinal_velocity, object_collision)); + + ObjectData object_no_collision; + object_no_collision.distance_to_object = 10.0; + object_no_collision.velocity = 0.1; + ASSERT_FALSE(aeb_node_->hasCollision(longitudinal_velocity, object_no_collision)); +} + +TEST_F(TestAEB, checkImuPathGeneration) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); + + const auto stamp = rclcpp::Time(); + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + const double x_start{0.0}; + const double y_start{0.0}; + + for (size_t i = 0; i < 15; ++i) { + pcl::PointXYZ p1( + x_start + static_cast(i / 100.0), y_start - static_cast(i / 100.0), 0.5); + pcl::PointXYZ p2( + x_start + static_cast((i + 10) / 100.0), y_start - static_cast(i / 100.0), + 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + } + PointCloud::Ptr points_belonging_to_cluster_hulls = pcl::make_shared(); + MarkerArray debug_markers; + aeb_node_->getPointsBelongingToClusterHulls( + obstacle_points_ptr, points_belonging_to_cluster_hulls, debug_markers); + std::vector objects; + aeb_node_->getClosestObjectsOnPath(imu_path, stamp, points_belonging_to_cluster_hulls, objects); + ASSERT_FALSE(objects.empty()); +} + +TEST_F(TestAEB, checkIncompleteImuPathGeneration) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; + const double slow_velocity = min_generated_path_length / (2.0 * horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + ASSERT_TRUE(imu_path.size() >= static_cast(horizon / dt)); + ASSERT_TRUE(autoware::motion_utils::calcArcLength(imu_path) >= min_generated_path_length); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + +TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) +{ + const double velocity = 0.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(velocity, yaw_rate); + ASSERT_EQ(imu_path.size(), 1); +} + +TEST_F(TestAEB, checkParamUpdate) +{ + std::vector parameters{rclcpp::Parameter("param")}; + const auto result = aeb_node_->onParameter(parameters); + ASSERT_TRUE(result.successful); +} + +TEST_F(TestAEB, checkEmptyFetchData) +{ + ASSERT_FALSE(aeb_node_->fetchLatestData()); +} + +TEST_F(TestAEB, checkConvertObjectToPolygon) +{ + using autoware_perception_msgs::msg::Shape; + PredictedObject obj_cylinder; + obj_cylinder.shape.type = Shape::CYLINDER; + obj_cylinder.shape.dimensions.x = 1.0; + Pose obj_cylinder_pose; + obj_cylinder_pose.position.x = 1.0; + obj_cylinder_pose.position.y = 1.0; + obj_cylinder.kinematics.initial_pose_with_covariance.pose = obj_cylinder_pose; + const auto cylinder_polygon = utils::convertObjToPolygon(obj_cylinder); + ASSERT_FALSE(cylinder_polygon.outer().empty()); + + PredictedObject obj_box; + obj_box.shape.type = Shape::BOUNDING_BOX; + obj_box.shape.dimensions.x = 1.0; + obj_box.shape.dimensions.y = 2.0; + Pose obj_box_pose; + obj_box_pose.position.x = 1.0; + obj_box_pose.position.y = 1.0; + obj_box.kinematics.initial_pose_with_covariance.pose = obj_box_pose; + const auto box_polygon = utils::convertObjToPolygon(obj_box); + ASSERT_FALSE(box_polygon.outer().empty()); + + geometry_msgs::msg::TransformStamped tf_stamped; + geometry_msgs::msg::Transform transform; + + constexpr double yaw{0.0}; + transform.rotation = autoware::universe_utils::createQuaternionFromYaw(yaw); + geometry_msgs::msg::Vector3 translation; + translation.x = 1.0; + translation.y = 0.0; + translation.z = 0.0; + transform.translation = translation; + tf_stamped.set__transform(transform); + const auto t_obj_box = utils::transformObjectFrame(obj_box, tf_stamped); + const auto t_pose = t_obj_box.kinematics.initial_pose_with_covariance.pose; + Pose expected_pose; + expected_pose.position.x = obj_box_pose.position.x + translation.x; + expected_pose.position.y = obj_box_pose.position.y + translation.y; + expected_pose.position.z = obj_box_pose.position.z + translation.z; + + ASSERT_DOUBLE_EQ(expected_pose.position.x, t_pose.position.x); + ASSERT_DOUBLE_EQ(expected_pose.position.y, t_pose.position.y); + ASSERT_DOUBLE_EQ(expected_pose.position.z, t_pose.position.z); +} + +TEST_F(TestAEB, CollisionDataKeeper) +{ + using namespace std::literals::chrono_literals; + constexpr double collision_keeping_sec{1.0}, previous_obstacle_keep_time{1.0}; + CollisionDataKeeper collision_data_keeper_(aeb_node_->get_clock()); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); + ASSERT_TRUE(collision_data_keeper_.checkPreviousObjectDataExpired()); + + ObjectData obj; + obj.stamp = aeb_node_->now(); + obj.velocity = 0.0; + obj.position.x = 0.0; + rclcpp::sleep_for(100ms); + + ObjectData obj2; + obj2.stamp = aeb_node_->now(); + obj2.velocity = 0.0; + obj2.position.x = 0.1; + rclcpp::sleep_for(100ms); + + constexpr double ego_longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.0; + const auto imu_path = aeb_node_->generateEgoPath(ego_longitudinal_velocity, yaw_rate); + + const auto speed_null = + collision_data_keeper_.calcObjectSpeedFromHistory(obj, imu_path, ego_longitudinal_velocity); + ASSERT_FALSE(speed_null.has_value()); + + const auto median_velocity = + collision_data_keeper_.calcObjectSpeedFromHistory(obj2, imu_path, ego_longitudinal_velocity); + ASSERT_TRUE(median_velocity.has_value()); + + // object speed is 1.0 m/s greater than ego's = 0.1 [m] / 0.1 [s] + longitudinal_velocity + ASSERT_TRUE(std::abs(median_velocity.value() - 4.0) < 1e-2); + rclcpp::sleep_for(1100ms); + ASSERT_TRUE(collision_data_keeper_.checkCollisionExpired()); +} + +TEST_F(TestAEB, TestCropPointCloud) +{ + constexpr double longitudinal_velocity = 3.0; + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(longitudinal_velocity, yaw_rate); + ASSERT_FALSE(imu_path.empty()); + + constexpr size_t n_points{15}; + // Create n_points inside the path and 1 point outside. + pcl::PointCloud::Ptr obstacle_points_ptr = + pcl::make_shared>(); + { + constexpr double x_start{0.0}; + constexpr double y_start{0.0}; + + for (size_t i = 0; i < n_points; ++i) { + const double offset_1 = static_cast(i / 100.0); + const double offset_2 = static_cast((i + 10) / 100.0); + pcl::PointXYZ p1(x_start + offset_1, y_start - offset_1, 0.5); + pcl::PointXYZ p2(x_start + offset_2, y_start - offset_1, 0.5); + obstacle_points_ptr->push_back(p1); + obstacle_points_ptr->push_back(p2); + } + pcl::PointXYZ p_out(x_start + 100.0, y_start + 100, 0.5); + obstacle_points_ptr->push_back(p_out); + } + aeb_node_->obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*obstacle_points_ptr, *aeb_node_->obstacle_ros_pointcloud_ptr_); + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + + pcl::PointCloud::Ptr filtered_objects = + pcl::make_shared>(); + aeb_node_->cropPointCloudWithEgoFootprintPath(footprint, filtered_objects); + // Check if the point outside the path was excluded + ASSERT_TRUE(filtered_objects->points.size() == 2 * n_points); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking::test diff --git a/control/autoware_autonomous_emergency_braking/test/test.hpp b/control/autoware_autonomous_emergency_braking/test/test.hpp new file mode 100644 index 0000000000000..86edbf73a2a12 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/test/test.hpp @@ -0,0 +1,116 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_HPP_ +#define TEST_HPP_ + +#include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware_test_utils/mock_data_parser.hpp" +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking::test +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using autoware::universe_utils::Polygon2d; +using autoware::vehicle_info_utils::VehicleInfo; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using std_msgs::msg::Header; + +std::shared_ptr generateNode(); +Header get_header(const char * const frame_id, rclcpp::Time t); +Imu make_imu_message( + const Header & header, const double ax, const double ay, const double yaw, + const double angular_velocity_z); +VelocityReport make_velocity_report_msg( + const Header & header, const double lat_velocity, const double long_velocity, + const double heading_rate); +class PubSubNode : public rclcpp::Node +{ +public: + explicit PubSubNode(const rclcpp::NodeOptions & node_options); + // publisher + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_point_cloud_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_predicted_traj_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + rclcpp::Publisher::SharedPtr pub_autoware_state_; + // timer + // rclcpp::TimerBase::SharedPtr timer_; + void publishDefaultTopicsNoSpin() + { + const auto header = get_header("base_link", now()); + const auto imu_msg = make_imu_message(header, 0.0, 0.0, 0.0, 0.05); + const auto velocity_msg = make_velocity_report_msg(header, 0.0, 3.0, 0.0); + + pub_imu_->publish(imu_msg); + pub_velocity_->publish(velocity_msg); + }; +}; + +std::shared_ptr generatePubSubNode(); + +class TestAEB : public ::testing::Test +{ +public: + TestAEB() {} + TestAEB(const TestAEB &) = delete; + TestAEB(TestAEB &&) = delete; + TestAEB & operator=(const TestAEB &) = delete; + TestAEB & operator=(TestAEB &&) = delete; + ~TestAEB() override = default; + + void SetUp() override + { + rclcpp::init(0, nullptr); + pub_sub_node_ = generatePubSubNode(); + aeb_node_ = generateNode(); + } + + void TearDown() override + { + aeb_node_.reset(); + pub_sub_node_.reset(); + rclcpp::shutdown(); + } + + std::shared_ptr pub_sub_node_; + std::shared_ptr aeb_node_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking::test + +#endif // TEST_HPP_ diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c561a8c1b9018..6aa70b4b464d4 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -148,6 +148,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/velocity", "/vehicle/status/velocity_status"), ("~/input/imu", "/sensing/imu/imu_data"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/objects", "/perception/object_recognition/objects"), ( "~/input/predicted_trajectory", "/control/trajectory_follower/lateral/predicted_trajectory",