fix(obstacle_stop_planner): add checking of point height (autowarefou…

…ndation#2296)

Signed-off-by: Łukasz Chojnacki <lukasz.chojnacki@robotec.ai>

planning/obstacle_stop_planner/README.md

@@ -31,11 +31,13 @@
 
 ### Common Parameter
 
-| Parameter              | Type   | Description                                                                               |
-| ---------------------- | ------ | ----------------------------------------------------------------------------------------- |
-| `enable_slow_down`     | bool   | enable slow down planner [-]                                                              |
-| `max_velocity`         | double | max velocity [m/s]                                                                        |
-| `chattering_threshold` | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] |
+| Parameter                          | Type   | Description                                                                               |
+| ---------------------------------- | ------ | ----------------------------------------------------------------------------------------- |
+| `enable_slow_down`                 | bool   | enable slow down planner [-]                                                              |
+| `max_velocity`                     | double | max velocity [m/s]                                                                        |
+| `chattering_threshold`             | double | even if the obstacle disappears, the stop judgment continues for chattering_threshold [s] |
+| `enable_z_axis_obstacle_filtering` | bool   | filter obstacles in z axis (height) [-]                                                   |
+| `z_axis_filtering_buffer`          | double | additional buffer for z axis filtering [m]]                                               |
 
 ## Obstacle Stop Planner
 

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -3,6 +3,8 @@
     chattering_threshold: 0.5                # even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]
     max_velocity: 20.0                       # max velocity [m/s]
     enable_slow_down: False                  # whether to use slow down planner [-]
+    enable_z_axis_obstacle_filtering: True   # filter obstacles in z axis (height) [-]
+    z_axis_filtering_buffer: 0.0             # additional buffer for z axis filtering [m]
     voxel_grid_x: 0.05                       # voxel grid x parameter for filtering pointcloud [m]
     voxel_grid_y: 0.05                       # voxel grid y parameter for filtering pointcloud [m]
     voxel_grid_z: 100000.0                   # voxel grid z parameter for filtering pointcloud [m]

planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp

@@ -110,6 +110,10 @@ class ObstacleStopPlannerDebugNode
   bool pushPolygon(
     const std::vector<cv::Point2d> & polygon, const double z, const PolygonType & type);
   bool pushPolygon(const std::vector<Eigen::Vector3d> & polygon, const PolygonType & type);
+  bool pushPolyhedron(
+    const std::vector<cv::Point2d> & 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 Pose & pose, const PoseType & type);
   bool pushObstaclePoint(const Point & obstacle_point, const PointType & type);
   bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type);
@@ -143,6 +147,8 @@ class ObstacleStopPlannerDebugNode
   std::vector<std::vector<Eigen::Vector3d>> slow_down_range_polygons_;
   std::vector<std::vector<Eigen::Vector3d>> collision_polygons_;
   std::vector<std::vector<Eigen::Vector3d>> slow_down_polygons_;
+  std::vector<std::vector<Eigen::Vector3d>> vehicle_polyhedrons_;
+  std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_;
   std::vector<std::vector<Eigen::Vector3d>> obstacle_polygons_;
 
   DebugValues debug_values_;

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -142,6 +142,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
 
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_collision_pointcloud_debug_;
+
   std::unique_ptr<AdaptiveCruiseController> acc_controller_;
   std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
   boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp

@@ -59,6 +59,12 @@ struct NodeParam
   // set True, slow down for obstacle beside the path
   bool enable_slow_down;
 
+  // set True, filter obstacles in z axis
+  bool enable_z_axis_obstacle_filtering;
+
+  // buffer for z axis filtering [m]
+  double z_axis_filtering_buffer;
+
   // max velocity [m/s]
   double max_velocity;
 

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp

@@ -133,6 +133,11 @@ bool withinPolygon(
   const Point2d & next_point, PointCloud::Ptr candidate_points_ptr,
   PointCloud::Ptr within_points_ptr);
 
+bool withinPolyhedron(
+  const std::vector<cv::Point2d> & cv_polygon, const double radius, const Point2d & prev_point,
+  const Point2d & next_point, PointCloud::Ptr candidate_points_ptr,
+  PointCloud::Ptr within_points_ptr, double z_min, double z_max);
+
 bool convexHull(
   const std::vector<cv::Point2d> & pointcloud, std::vector<cv::Point2d> & polygon_points);
 

planning/obstacle_stop_planner/src/debug_marker.cpp

@@ -98,6 +98,38 @@ bool ObstacleStopPlannerDebugNode::pushPolygon(
   }
 }
 
+bool ObstacleStopPlannerDebugNode::pushPolyhedron(
+  const std::vector<cv::Point2d> & polyhedron, const double z_min, const double z_max,
+  const PolygonType & type)
+{
+  std::vector<Eigen::Vector3d> eigen_polyhedron;
+  for (const auto & point : polyhedron) {
+    eigen_polyhedron.emplace_back(point.x, point.y, z_min);
+    eigen_polyhedron.emplace_back(point.x, point.y, z_max);
+  }
+
+  return pushPolyhedron(eigen_polyhedron, type);
+}
+
+bool ObstacleStopPlannerDebugNode::pushPolyhedron(
+  const std::vector<Eigen::Vector3d> & polyhedron, const PolygonType & type)
+{
+  switch (type) {
+    case PolygonType::Vehicle:
+      if (!polyhedron.empty()) {
+        vehicle_polyhedrons_.push_back(polyhedron);
+      }
+      return true;
+    case PolygonType::Collision:
+      if (!polyhedron.empty()) {
+        collision_polyhedrons_.push_back(polyhedron);
+      }
+      return true;
+    default:
+      return false;
+  }
+}
+
 bool ObstacleStopPlannerDebugNode::pushPose(const Pose & pose, const PoseType & type)
 {
   switch (type) {
@@ -173,6 +205,8 @@ void ObstacleStopPlannerDebugNode::publish()
   slow_down_range_polygons_.clear();
   slow_down_polygons_.clear();
   obstacle_polygons_.clear();
+  vehicle_polyhedrons_.clear();
+  collision_polyhedrons_.clear();
   stop_pose_ptr_ = nullptr;
   target_stop_pose_ptr_ = nullptr;
   slow_down_start_pose_ptr_ = nullptr;
@@ -231,6 +265,71 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker()
   MarkerArray msg;
   rclcpp::Time current_time = node_->now();
 
+  // cube
+  if (!vehicle_polyhedrons_.empty()) {
+    auto marker = createDefaultMarker(
+      "map", current_time, "detection_cubes", 0, Marker::LINE_LIST,
+      createMarkerScale(0.01, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999));
+
+    for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) {
+      for (size_t j = 0; j < vehicle_polyhedrons_.at(i).size(); ++j) {
+        const auto & p = vehicle_polyhedrons_.at(i).at(j);
+        marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      }
+    }
+
+    for (size_t i = 0; i < vehicle_polyhedrons_.size(); ++i) {
+      for (size_t j = 0; j + 2 < vehicle_polyhedrons_.at(i).size(); ++j) {
+        const auto & p = vehicle_polyhedrons_.at(i).at(j);
+        marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+        const auto & p1 = vehicle_polyhedrons_.at(i).at(j + 2);
+        marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z()));
+      }
+      const auto & p = vehicle_polyhedrons_.at(i).at(1);
+      marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      const auto & p1 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 1);
+      marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z()));
+      const auto & p2 = vehicle_polyhedrons_.at(i).at(0);
+      marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z()));
+      const auto & p3 = vehicle_polyhedrons_.at(i).at(vehicle_polyhedrons_.at(i).size() - 2);
+      marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z()));
+    }
+
+    msg.markers.push_back(marker);
+  }
+
+  if (!collision_polyhedrons_.empty()) {
+    auto marker = createDefaultMarker(
+      "map", current_time, "collision_cubes", 0, Marker::LINE_LIST,
+      createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.0, 0.999));
+
+    for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) {
+      for (size_t j = 0; j < collision_polyhedrons_.at(i).size(); ++j) {
+        const auto & p = collision_polyhedrons_.at(i).at(j);
+        marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      }
+    }
+
+    for (size_t i = 0; i < collision_polyhedrons_.size(); ++i) {
+      for (size_t j = 0; j + 2 < collision_polyhedrons_.at(i).size(); ++j) {
+        const auto & p = collision_polyhedrons_.at(i).at(j);
+        marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+        const auto & p1 = collision_polyhedrons_.at(i).at(j + 2);
+        marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z()));
+      }
+      const auto & p = collision_polyhedrons_.at(i).at(1);
+      marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
+      const auto & p1 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 1);
+      marker.points.push_back(createPoint(p1.x(), p1.y(), p1.z()));
+      const auto & p2 = collision_polyhedrons_.at(i).at(0);
+      marker.points.push_back(createPoint(p2.x(), p2.y(), p2.z()));
+      const auto & p3 = collision_polyhedrons_.at(i).at(collision_polyhedrons_.at(i).size() - 2);
+      marker.points.push_back(createPoint(p3.x(), p3.y(), p3.z()));
+    }
+
+    msg.markers.push_back(marker);
+  }
+
   // polygon
   if (!vehicle_polygons_.empty()) {
     auto marker = createDefaultMarker(