feat(predicted_path_checker): check predicted trajectory to avoid co…

…llisions planning can not handle (autowarefoundation#2528)

Signed-off-by: Berkay Karaman <[email protected]>

control/predicted_path_checker/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.14)
+project(predicted_path_checker)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Eigen3 REQUIRED)
+
+
+
+ament_auto_add_library(predicted_path_checker SHARED
+  src/predicted_path_checker_node/predicted_path_checker_node.cpp
+  src/predicted_path_checker_node/collision_checker.cpp
+  src/predicted_path_checker_node/utils.cpp
+  src/predicted_path_checker_node/debug_marker.cpp
+
+)
+
+target_include_directories(predicted_path_checker
+  SYSTEM PUBLIC
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+rclcpp_components_register_node(predicted_path_checker
+  PLUGIN "predicted_path_checker::PredictedPathCheckerNode"
+  EXECUTABLE predicted_path_checker_node
+)
+
+if(BUILD_TESTING)
+
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

control/predicted_path_checker/README.md

@@ -0,0 +1,98 @@
+# Predicted Path Checker
+
+## Purpose
+
+The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control
+modules. It handles potential collisions that the planning module might not be able to handle and that in the brake
+distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert
+the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to
+pause interface to make the vehicle stop.
+
+![general-structure.png](images%2Fgeneral-structure.png)
+
+## Algorithm
+
+The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in
+the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them (
+emergency or pause request).
+
+### Inner Algorithm
+
+![FlowChart.png](images%2FFlowChart.png)
+
+**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the
+velocity
+of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length".
+
+**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It
+calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is
+an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the
+predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid
+unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold"
+seconds ago.
+
+If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by
+using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out.
+
+![Z_axis_filtering.png](images%2FZ_axis_filtering.png)
+
+**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on
+predicted trajectory's collision point's axes.
+
+**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and
+acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in
+brake distance, it returns true.
+
+**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not
+discrete point, planning should handle the stop.
+
+**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop
+point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to
+make the vehicle stop.
+
+## Inputs
+
+| Name                                  | Type                                                  | Description                                         |
+| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- |
+| `~/input/reference_trajectory`        | `autoware_auto_planning_msgs::msg::Trajectory`        | Reference trajectory                                |
+| `~/input/predicted_trajectory`        | `autoware_auto_planning_msgs::msg::Trajectory`        | Predicted trajectory                                |
+| `~/input/objects`                     | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment                  |
+| `~/input/odometry`                    | `nav_msgs::msg::Odometry`                             | Odometry message of vehicle to get current velocity |
+| `~/input/current_accel`               | `geometry_msgs::msg::AccelWithCovarianceStamped`      | Current acceleration                                |
+| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused`                   | Current pause state of the vehicle                  |
+
+## Outputs
+
+| Name                                  | Type                                     | Description                            |
+| ------------------------------------- | ---------------------------------------- | -------------------------------------- |
+| `~/debug/marker`                      | `visualization_msgs::msg::MarkerArray`   | Marker for visualization               |
+| `~/debug/virtual_wall`                | `visualization_msgs::msg::MarkerArray`   | Virtual wall marker for visualization  |
+| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause`      | Pause service to make the vehicle stop |
+| `/diagnostics`                        | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle           |
+
+## Parameters
+
+**Node Parameters**
+
+| Name                                | Type     | Description                                             | Default value |
+| :---------------------------------- | :------- | :------------------------------------------------------ | :------------ |
+| `update_rate`                       | `double` | The update rate [Hz]                                    | 10.0          |
+| `delay_time`                        | `double` | he time delay considered for the emergency response [s] | 0.17          |
+| `max_deceleration`                  | `double` | Max deceleration for ego vehicle to stop [m/s^2]        | 1.5           |
+| `resample_interval`                 | `double` | Interval for resampling trajectory [m]                  | 0.5           |
+| `stop_margin`                       | `double` | The stopping margin [m]                                 | 0.5           |
+| `ego_nearest_dist_threshold`        | `double` | The nearest distance threshold for ego vehicle [m]      | 3.0           |
+| `ego_nearest_yaw_threshold`         | `double` | The nearest yaw threshold for ego vehicle [rad]         | 1.046         |
+| `min_trajectory_check_length`       | `double` | The minimum trajectory check length in meters [m]       | 1.5           |
+| `trajectory_check_time`             | `double` | The trajectory check time in seconds. [s]               | 3.0           |
+| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m]               | 0.3           |
+| `distinct_point_yaw_threshold`      | `double` | The distinct point yaw threshold [deg]                  | 5.0           |
+
+**Collision Checker Parameters**
+
+| Name                               | Type     | Description                                                       | Default value |
+| :--------------------------------- | :------- | :---------------------------------------------------------------- | :------------ |
+| `width_margin`                     | `double` | The width margin for collision checking [Hz]                      | 0.2           |
+| `chattering_threshold`             | `double` | The chattering threshold for collision detection [s]              | 0.2           |
+| `z_axis_filtering_buffer`          | `double` | The Z-axis filtering buffer [m]                                   | 0.3           |
+| `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

@@ -0,0 +1,20 @@
+/**:
+  ros__parameters:
+    # Node
+    update_rate: 10.0
+    delay_time: 0.17
+    max_deceleration: 1.5
+    resample_interval: 0.5
+    stop_margin: 0.5 # [m]
+    ego_nearest_dist_threshold: 3.0 # [m]
+    ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]
+    min_trajectory_check_length: 1.5
+    trajectory_check_time: 3.0
+    distinct_point_distance_threshold: 0.3
+    distinct_point_yaw_threshold: 5.0 # [deg]
+
+    collision_checker_params:
+      width_margin: 0.2
+      chattering_threshold: 0.2
+      z_axis_filtering_buffer: 0.3
+      enable_z_axis_obstacle_filtering: false

control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp

@@ -0,0 +1,128 @@
+// Copyright 2023 LeoDrive A.Ş. All rights reserved.
+//
+// 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 PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
+#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
+
+#include <motion_utils/trajectory/interpolation.hpp>
+#include <motion_utils/trajectory/tmp_conversion.hpp>
+#include <predicted_path_checker/debug_marker.hpp>
+#include <predicted_path_checker/utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <vehicle_info_util/vehicle_info.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <boost/assert.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/geometry.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace predicted_path_checker
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
+using PointArray = std::vector<geometry_msgs::msg::Point>;
+
+namespace bg = boost::geometry;
+
+struct CollisionCheckerParam
+{
+  double width_margin;
+  double z_axis_filtering_buffer;
+  bool enable_z_axis_obstacle_filtering;
+  double chattering_threshold;
+};
+
+struct PredictedObjectWithDetectionTime
+{
+  explicit PredictedObjectWithDetectionTime(
+    const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
+  : detection_time(t), point(p), object(std::move(obj))
+  {
+  }
+
+  rclcpp::Time detection_time;
+  geometry_msgs::msg::Point point;
+  PredictedObject object;
+};
+
+class CollisionChecker
+{
+public:
+  explicit CollisionChecker(
+    rclcpp::Node * node, std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr);
+
+  boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>>
+  checkTrajectoryForCollision(
+    TrajectoryPoints & predicted_trajectory_array,
+    PredictedObjects::ConstSharedPtr dynamic_objects);
+
+  void setParam(const CollisionCheckerParam & param);
+
+private:
+  // Functions
+
+  boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkObstacleHistory(
+    const Pose & base_pose, const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min,
+    const double z_max);
+
+  boost::optional<std::pair<geometry_msgs::msg::Point, PredictedObject>> checkDynamicObjects(
+    const Pose & base_pose, PredictedObjects::ConstSharedPtr dynamic_objects,
+    const Polygon2d & one_step_move_vehicle_polygon2d, const double z_min, const double z_max);
+
+  void updatePredictedObjectHistory(const rclcpp::Time & now)
+  {
+    for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) {
+      const auto expired = (now - itr->detection_time).seconds() > param_.chattering_threshold;
+
+      if (expired) {
+        itr = predicted_object_history_.erase(itr);
+        continue;
+      }
+
+      itr++;
+    }
+  }
+
+  // Parameter
+  CollisionCheckerParam param_;
+
+  // Variables
+  std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr_;
+  rclcpp::Node * node_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
+  std::vector<PredictedObjectWithDetectionTime> predicted_object_history_{};
+};
+}  // namespace predicted_path_checker
+
+#endif  // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_

control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp

@@ -0,0 +1,92 @@
+// Copyright 2023 LeoDrive A.Ş. All rights reserved.
+//
+// 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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
+#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <boost/assert.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/geometry.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#define EIGEN_MPL2_ONLY
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
+
+namespace predicted_path_checker
+{
+
+enum class PolygonType : int8_t { Vehicle = 0, Collision };
+
+enum class PointType : int8_t { Stop = 0 };
+
+enum class PoseType : int8_t { Stop = 0, Collision };
+
+class PredictedPathCheckerDebugNode
+{
+public:
+  explicit PredictedPathCheckerDebugNode(rclcpp::Node * node, const double base_link2front);
+
+  ~PredictedPathCheckerDebugNode() {}
+
+  bool pushPolygon(
+    const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type);
+
+  bool pushPolygon(const std::vector<Eigen::Vector3d> & polygon, const PolygonType & type);
+
+  bool pushPolyhedron(
+    const tier4_autoware_utils::Polygon2d & polyhedron, const double z_min, const double z_max,
+    const PolygonType & type);
+
+  bool pushPolyhedron(const std::vector<Eigen::Vector3d> & polyhedron, const PolygonType & type);
+
+  bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
+
+  bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
+
+  visualization_msgs::msg::MarkerArray makeVirtualWallMarker();
+
+  visualization_msgs::msg::MarkerArray makeVisualizationMarker();
+
+  void publish();
+
+private:
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
+  rclcpp::Node * node_;
+  double base_link2front_;
+
+  std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;
+  std::shared_ptr<geometry_msgs::msg::Pose> collision_pose_ptr_;
+  std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
+  std::vector<std::vector<Eigen::Vector3d>> vehicle_polygons_;
+  std::vector<std::vector<Eigen::Vector3d>> collision_polygons_;
+  std::vector<std::vector<Eigen::Vector3d>> vehicle_polyhedrons_;
+  std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_;
+};
+
+}  // namespace predicted_path_checker
+
+#endif  // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_