Skip to content

Commit e337576

Browse files
brkay54brkay54
authored
feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle (#2528)
* feat(predicted_path_checker): check predicted trajectory to avoid collisions planning can not handle (#2528) Signed-off-by: Berkay Karaman <brkay54@gmail.com> * Added pkg to control.launch.py Signed-off-by: Berkay Karaman <brkay54@gmail.com> --------- Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent ad69c28 commit e337576

17 files changed

+2338
-0
lines changed

‎control/predicted_path_checker/CMakeLists.txt

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(predicted_path_checker)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(Eigen3 REQUIRED)
8+
9+
include_directories(
10+
include
11+
SYSTEM
12+
${Eigen3_INCLUDE_DIRS}
13+
)
14+
15+
ament_auto_add_library(predicted_path_checker SHARED
16+
src/predicted_path_checker_node/predicted_path_checker_node.cpp
17+
src/predicted_path_checker_node/collision_checker.cpp
18+
src/predicted_path_checker_node/utils.cpp
19+
src/predicted_path_checker_node/debug_marker.cpp
20+
21+
)
22+
23+
rclcpp_components_register_node(predicted_path_checker
24+
PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode"
25+
EXECUTABLE predicted_path_checker_node
26+
)
27+
28+
if(BUILD_TESTING)
29+
find_package(ament_lint_auto REQUIRED)
30+
ament_lint_auto_find_test_dependencies()
31+
endif()
32+
33+
ament_auto_package(
34+
INSTALL_TO_SHARE
35+
launch
36+
config
37+
)

‎control/predicted_path_checker/README.md

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
# Predicted Path Checker
2+
3+
## Purpose
4+
5+
The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control
6+
modules. It handles potential collisions that the planning module might not be able to handle and that in the brake
7+
distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert
8+
the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to
9+
pause interface to make the vehicle stop.
10+
11+
![general-structure.png](images%2Fgeneral-structure.png)
12+
13+
## Algorithm
14+
15+
The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in
16+
the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them (
17+
emergency or pause request).
18+
19+
### Inner Algorithm
20+
21+
![FlowChart.png](images%2FFlowChart.png)
22+
23+
**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the
24+
velocity
25+
of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length".
26+
27+
**filterObstacles() ->** It filters the predicted objects in the environment. It filters the objects which are not in
28+
front of the vehicle and far away from predicted trajectory.
29+
30+
**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It
31+
calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is
32+
an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the
33+
predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid
34+
unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold"
35+
seconds ago.
36+
37+
If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by
38+
using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out.
39+
40+
![Z_axis_filtering.png](images%2FZ_axis_filtering.png)
41+
42+
**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on
43+
predicted trajectory's collision point's axes.
44+
45+
**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and
46+
acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in
47+
brake distance, it returns true.
48+
49+
**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not
50+
discrete point, planning should handle the stop.
51+
52+
**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop
53+
point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to
54+
make the vehicle stop.
55+
56+
## Inputs
57+
58+
| Name | Type | Description |
59+
| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- |
60+
| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory |
61+
| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory |
62+
| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment |
63+
| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity |
64+
| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration |
65+
| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle |
66+
67+
## Outputs
68+
69+
| Name | Type | Description |
70+
| ------------------------------------- | ---------------------------------------- | -------------------------------------- |
71+
| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization |
72+
| `~/debug/virtual_wall` | `visualization_msgs::msg::MarkerArray` | Virtual wall marker for visualization |
73+
| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause` | Pause service to make the vehicle stop |
74+
| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle |
75+
76+
## Parameters
77+
78+
### Node Parameters
79+
80+
| Name | Type | Description | Default value |
81+
| :---------------------------------- | :------- | :-------------------------------------------------------------------- | :------------ |
82+
| `update_rate` | `double` | The update rate [Hz] | 10.0 |
83+
| `delay_time` | `double` | he time delay considered for the emergency response [s] | 0.17 |
84+
| `max_deceleration` | `double` | Max deceleration for ego vehicle to stop [m/s^2] | 1.5 |
85+
| `resample_interval` | `double` | Interval for resampling trajectory [m] | 0.5 |
86+
| `stop_margin` | `double` | The stopping margin [m] | 0.5 |
87+
| `ego_nearest_dist_threshold` | `double` | The nearest distance threshold for ego vehicle [m] | 3.0 |
88+
| `ego_nearest_yaw_threshold` | `double` | The nearest yaw threshold for ego vehicle [rad] | 1.046 |
89+
| `min_trajectory_check_length` | `double` | The minimum trajectory check length in meters [m] | 1.5 |
90+
| `trajectory_check_time` | `double` | The trajectory check time in seconds. [s] | 3.0 |
91+
| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m] | 0.3 |
92+
| `distinct_point_yaw_threshold` | `double` | The distinct point yaw threshold [deg] | 5.0 |
93+
| `filtering_distance_threshold` | `double` | It ignores the objects if distance is higher than this [m] | 1.5 |
94+
| `use_object_prediction` | `bool` | If true, node predicts current pose of the objects wrt delta time [-] | true |
95+
96+
### Collision Checker Parameters
97+
98+
| Name | Type | Description | Default value |
99+
| :--------------------------------- | :------- | :---------------------------------------------------------------- | :------------ |
100+
| `width_margin` | `double` | The width margin for collision checking [Hz] | 0.2 |
101+
| `chattering_threshold` | `double` | The chattering threshold for collision detection [s] | 0.2 |
102+
| `z_axis_filtering_buffer` | `double` | The Z-axis filtering buffer [m] | 0.3 |
103+
| `enable_z_axis_obstacle_filtering` | `bool` | A boolean flag indicating if Z-axis obstacle filtering is enabled | false |

‎control/predicted_path_checker/config/predicted_path_checker.param.yaml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
/**:
2+
ros__parameters:
3+
# Node
4+
update_rate: 10.0
5+
delay_time: 0.17
6+
max_deceleration: 1.5
7+
resample_interval: 0.5
8+
stop_margin: 0.5 # [m]
9+
ego_nearest_dist_threshold: 3.0 # [m]
10+
ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]
11+
min_trajectory_check_length: 1.5 # [m]
12+
trajectory_check_time: 3.0
13+
distinct_point_distance_threshold: 0.3
14+
distinct_point_yaw_threshold: 5.0 # [deg]
15+
filtering_distance_threshold: 1.5 # [m]
16+
use_object_prediction: true
17+
18+
collision_checker_params:
19+
width_margin: 0.2
20+
chattering_threshold: 0.2
21+
z_axis_filtering_buffer: 0.3
22+
enable_z_axis_obstacle_filtering: false

‎control/predicted_path_checker/images/FlowChart.png

63.2 KB
Loading

‎control/predicted_path_checker/images/Z_axis_filtering.png

22.1 KB
Loading

‎control/predicted_path_checker/images/general-structure.png

82.7 KB
Loading

‎control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp

+128
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
// Copyright 2023 LeoDrive A.Ş. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
16+
#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
17+
18+
#include <motion_utils/trajectory/interpolation.hpp>
19+
#include <motion_utils/trajectory/tmp_conversion.hpp>
20+
#include <predicted_path_checker/debug_marker.hpp>
21+
#include <predicted_path_checker/utils.hpp>
22+
#include <rclcpp/rclcpp.hpp>
23+
#include <tier4_autoware_utils/geometry/geometry.hpp>
24+
#include <tier4_autoware_utils/ros/debug_publisher.hpp>
25+
#include <tier4_autoware_utils/ros/transform_listener.hpp>
26+
#include <vehicle_info_util/vehicle_info.hpp>
27+
28+
#include <geometry_msgs/msg/pose_stamped.hpp>
29+
#include <nav_msgs/msg/odometry.hpp>
30+
#include <visualization_msgs/msg/marker_array.hpp>
31+
32+
#include <boost/assert.hpp>
33+
#include <boost/assign/list_of.hpp>
34+
#include <boost/geometry.hpp>
35+
36+
#include <tf2_ros/buffer.h>
37+
#include <tf2_ros/transform_listener.h>
38+
39+
#include <memory>
40+
#include <utility>
41+
#include <vector>
42+
43+
namespace autoware::motion::control::predicted_path_checker
44+
{
45+
using autoware_auto_planning_msgs::msg::Trajectory;
46+
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
47+
using TrajectoryPoints = std::vector<TrajectoryPoint>;
48+
using autoware_auto_perception_msgs::msg::PredictedObject;
49+
using autoware_auto_perception_msgs::msg::PredictedObjects;
50+
using geometry_msgs::msg::Pose;
51+
using geometry_msgs::msg::TransformStamped;
52+
using tier4_autoware_utils::Point2d;
53+
using tier4_autoware_utils::Polygon2d;
54+
using PointArray = std::vector<geometry_msgs::msg::Point>;
55+
56+
namespace bg = boost::geometry;
57+
58+
struct CollisionCheckerParam
59+
{
60+
double width_margin;
61+
double z_axis_filtering_buffer;
62+
bool enable_z_axis_obstacle_filtering;
63+
double chattering_threshold;
64+
};
65+
66+
struct PredictedObjectWithDetectionTime
67+
{
68+
explicit PredictedObjectWithDetectionTime(
69+
const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
70+
: detection_time(t), point(p), object(std::move(obj))
71+
{
72+
}
73+
74+
rclcpp::Time detection_time;
75+
geometry_msgs::msg::Point point;
76+
PredictedObject object;
77+
};
78+
79+
class CollisionChecker
80+
{
81+
public:
82+
explicit CollisionChecker(
83+
rclcpp::Node * node, std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr);
84+
85+
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>>
86+
checkTrajectoryForCollision(
87+
TrajectoryPoints & predicted_trajectory_array,
88+
PredictedObjects::ConstSharedPtr dynamic_objects);
89+
90+
void setParam(const CollisionCheckerParam & param);
91+
92+
private:
93+
// Functions
94+
95+
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkObstacleHistory(
96+
const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min,
97+
const double z_max);
98+
99+
boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkDynamicObjects(
100+
const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects,
101+
const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max);
102+
103+
void updatePredictedObjectHistory(const rclcpp::Time & now)
104+
{
105+
for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) {
106+
const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold;
107+
108+
if (expired) {
109+
itr = predicted_object_history_.erase(itr);
110+
continue;
111+
}
112+
113+
itr++;
114+
}
115+
}
116+
117+
// Parameter
118+
CollisionCheckerParam param_;
119+
120+
// Variables
121+
std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr_;
122+
rclcpp::Node * node_;
123+
vehicle_info_util::VehicleInfo vehicle_info_;
124+
std::vector<PredictedObjectWithDetectionTime> predicted_object_history_{};
125+
};
126+
} // namespace autoware::motion::control::predicted_path_checker
127+
128+
#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_

‎control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp

+92
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,92 @@
1+
// Copyright 2023 LeoDrive A.Ş. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
#ifndef PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
15+
#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
16+
17+
#include <rclcpp/rclcpp.hpp>
18+
19+
#include <geometry_msgs/msg/point.hpp>
20+
#include <geometry_msgs/msg/pose.hpp>
21+
#include <visualization_msgs/msg/marker.hpp>
22+
#include <visualization_msgs/msg/marker_array.hpp>
23+
24+
#include <boost/assert.hpp>
25+
#include <boost/assign/list_of.hpp>
26+
#include <boost/geometry.hpp>
27+
28+
#include <memory>
29+
#include <string>
30+
#include <vector>
31+
32+
#define EIGEN_MPL2_ONLY
33+
34+
#include <eigen3/Eigen/Core>
35+
#include <eigen3/Eigen/Geometry>
36+
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
37+
38+
namespace autoware::motion::control::predicted_path_checker
39+
{
40+
41+
enum class PolygonType : int8_t { Vehicle = 0, Collision };
42+
43+
enum class PointType : int8_t { Stop = 0 };
44+
45+
enum class PoseType : int8_t { Stop = 0, Collision };
46+
47+
class PredictedPathCheckerDebugNode
48+
{
49+
public:
50+
explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front);
51+
52+
~PredictedPathCheckerDebugNode() {}
53+
54+
bool pushPolygon(
55+
const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type);
56+
57+
bool pushPolygon(const std::vector<Eigen::Vector3d> & polygon, const PolygonType & type);
58+
59+
bool pushPolyhedron(
60+
const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max,
61+
const PolygonType & type);
62+
63+
bool pushPolyhedron(const std::vector<Eigen::Vector3d> & polyhedron, const PolygonType & type);
64+
65+
bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
66+
67+
bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
68+
69+
visualization_msgs::msg::MarkerArray makeVirtualWallMarker();
70+
71+
visualization_msgs::msg::MarkerArray makeVisualizationMarker();
72+
73+
void publish();
74+
75+
private:
76+
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_;
77+
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
78+
rclcpp::Node * node_;
79+
double base_link2front_;
80+
81+
std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;
82+
std::shared_ptr<geometry_msgs::msg::Pose> collision_pose_ptr_;
83+
std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
84+
std::vector<std::vector<Eigen::Vector3d>> vehicle_polygons_;
85+
std::vector<std::vector<Eigen::Vector3d>> collision_polygons_;
86+
std::vector<std::vector<Eigen::Vector3d>> vehicle_polyhedrons_;
87+
std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_;
88+
};
89+
90+
} // namespace autoware::motion::control::predicted_path_checker
91+
92+
#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_

0 commit comments

Comments
 (0)