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,23 @@
+# predicted_path_checker
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+### Algorithms
+
+### Check data
+
+### Diagnostic update
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits

control/predicted_path_checker/config/predicted_path_checker.param.yaml

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

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_