fix(obstacle_stop_planner): change polygon creating method (autowaref…

…oundation#3505) Signed-off-by: Berkay Karaman <[email protected]>
Interplai · Apr 26, 2023 · 4777680 · 4777680
1 parent 2e7181c
commit 4777680
Show file tree

Hide file tree

Showing 8 changed files with 111 additions and 186 deletions.
diff --git a/planning/obstacle_stop_planner/CMakeLists.txt b/planning/obstacle_stop_planner/CMakeLists.txt
@@ -5,7 +5,6 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Eigen3 REQUIRED)
-find_package(OpenCV REQUIRED)
 find_package(PCL REQUIRED)
 
 ament_auto_add_library(obstacle_stop_planner SHARED
@@ -17,13 +16,11 @@ ament_auto_add_library(obstacle_stop_planner SHARED
 
 target_include_directories(obstacle_stop_planner
   SYSTEM PUBLIC
-    ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIR}
 )
 
 target_link_libraries(obstacle_stop_planner
-    ${OpenCV_LIBRARIES}
     ${PCL_LIBRARIES}
 )
 

diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp
@@ -14,9 +14,6 @@
 #ifndef OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_
 #define OBSTACLE_STOP_PLANNER__DEBUG_MARKER_HPP_
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
@@ -35,6 +32,7 @@
 #define EIGEN_MPL2_ONLY
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Geometry>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 namespace motion_planning
 {
 
@@ -108,10 +106,10 @@ class ObstacleStopPlannerDebugNode
   explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front);
   ~ObstacleStopPlannerDebugNode() {}
   bool pushPolygon(
-    const std::vector<cv::Point2d> & polygon, const double z, const PolygonType & type);
+    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 std::vector<cv::Point2d> & polyhedron, const double z_min, const double z_max,
+    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 Pose & pose, const PoseType & type);

diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
@@ -21,9 +21,6 @@
 
 #include <motion_utils/trajectory/tmp_conversion.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>

diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp
@@ -18,9 +18,6 @@
 
 #include "obstacle_stop_planner/planner_data.hpp"
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -79,20 +76,19 @@ bool isInFrontOfTargetPoint(const Pose & pose, const Point & point);
 bool checkValidIndex(const Pose & p_base, const Pose & p_next, const Pose & p_target);
 
 bool withinPolygon(
-  const std::vector<cv::Point2d> & cv_polygon, const double radius, const Point2d & prev_point,
+  const Polygon2d & boost_polygon, const double radius, const Point2d & prev_point,
   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 Polygon2d & boost_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);
+void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point);
 
 void createOneStepPolygon(
-  const Pose & base_step_pose, const Pose & next_step_pose, std::vector<cv::Point2d> & polygon,
+  const Pose & base_step_pose, const Pose & next_step_pose, Polygon2d & hull_polygon,
   const VehicleInfo & vehicle_info, const double expand_width = 0.0);
 
 bool getSelfPose(const Header & header, const tf2_ros::Buffer & tf_buffer, Pose & self_pose);

diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml
@@ -23,7 +23,6 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>diagnostic_msgs</depend>
-  <depend>libopencv-dev</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>pcl_conversions</depend>

diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp
@@ -53,12 +53,12 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode(
 }
 
 bool ObstacleStopPlannerDebugNode::pushPolygon(
-  const std::vector<cv::Point2d> & polygon, const double z, const PolygonType & type)
+  const tier4_autoware_utils::Polygon2d & polygon, const double z, const PolygonType & type)
 {
   std::vector<Eigen::Vector3d> eigen_polygon;
-  for (const auto & point : polygon) {
+  for (const auto & point : polygon.outer()) {
     Eigen::Vector3d eigen_point;
-    eigen_point << point.x, point.y, z;
+    eigen_point << point.x(), point.y(), z;
     eigen_polygon.push_back(eigen_point);
   }
   return pushPolygon(eigen_polygon, type);
@@ -99,13 +99,13 @@ bool ObstacleStopPlannerDebugNode::pushPolygon(
 }
 
 bool ObstacleStopPlannerDebugNode::pushPolyhedron(
-  const std::vector<cv::Point2d> & polyhedron, const double z_min, const double z_max,
+  const tier4_autoware_utils::Polygon2d & 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);
+  for (const auto & point : polyhedron.outer()) {
+    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);