Merge pull request autowarefoundation#813 from tier4/feat/tunable-sur…

…round-obstacle-checker

feat(surround_obstacle_checker): make parameters more tunable (autowarefoundation#4925)

planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml

@@ -1,12 +1,61 @@
 /**:
   ros__parameters:
-
     # obstacle check
-    use_pointcloud: true # use pointcloud as obstacle check
-    use_dynamic_object: true # use dynamic object as obstacle check
-    surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
-    surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m]
+    # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
+    # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m]
+    pointcloud:
+      enable_check: false
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    unknown:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    car:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    truck:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    bus:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    trailer:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    motorcycle:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    bicycle:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    pedestrian:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+
+    surround_check_hysteresis_distance: 0.3
+
     state_clear_time: 2.0
 
+    # ego stop state
+    stop_state_ego_speed: 0.1 #[m/s]
+
     # debug
     publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets
+    debug_footprint_label: "car"

planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp

@@ -16,6 +16,7 @@
 #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
@@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 using tier4_planning_msgs::msg::StopReasonArray;
+using vehicle_info_util::VehicleInfo;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode
 {
 public:
   explicit SurroundObstacleCheckerDebugNode(
-    const Polygon2d & ego_polygon, const double base_link2front,
-    const double & surround_check_distance, const double & surround_check_recover_distance,
-    const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
-    rclcpp::Node & node);
+    const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
+    const std::string & object_label, const double & surround_check_front_distance,
+    const double & surround_check_side_distance, const double & surround_check_back_distance,
+    const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
+    const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node);
 
   bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
   bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
   void publish();
   void publishFootprints();
+  void updateFootprintMargin(
+    const std::string & object_label, const double front_distance, const double side_distance,
+    const double back_distance);
 
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_virtual_wall_pub_;
@@ -72,18 +78,20 @@ class SurroundObstacleCheckerDebugNode
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_offset_pub_;
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_recover_offset_pub_;
 
-  Polygon2d ego_polygon_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
   double base_link2front_;
-  double surround_check_distance_;
-  double surround_check_recover_distance_;
+  std::string object_label_;
+  double surround_check_front_distance_;
+  double surround_check_side_distance_;
+  double surround_check_back_distance_;
+  double surround_check_hysteresis_distance_;
   geometry_msgs::msg::Pose self_pose_;
 
   MarkerArray makeVirtualWallMarker();
   MarkerArray makeVisualizationMarker();
   StopReasonArray makeStopReasonArray();
   VelocityFactorArray makeVelocityFactorArray();
 
-  Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset);
   PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);
 
   std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;

planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp

@@ -38,6 +38,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
 
   struct NodeParam
   {
-    bool use_pointcloud;
-    bool use_dynamic_object;
     bool is_surround_obstacle;
-    // For preventing chattering,
-    // surround_check_recover_distance_ must be  bigger than surround_check_distance_
-    double surround_check_recover_distance;
-    double surround_check_distance;
+    std::unordered_map<int, bool> enable_check_map;
+    std::unordered_map<int, double> surround_check_front_distance_map;
+    std::unordered_map<int, double> surround_check_side_distance_map;
+    std::unordered_map<int, double> surround_check_back_distance_map;
+    bool pointcloud_enable_check;
+    double pointcloud_surround_check_front_distance;
+    double pointcloud_surround_check_side_distance;
+    double pointcloud_surround_check_back_distance;
+    double surround_check_hysteresis_distance;
     double state_clear_time;
     bool publish_debug_footprints;
+    std::string debug_footprint_label;
   };
 
 private:
+  rcl_interfaces::msg::SetParametersResult onParam(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  std::array<double, 3> getCheckDistances(const std::string & str_label) const;
+
   void onTimer();
 
   void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
@@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
 
+  // parameter callback result
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
   // stop checker
   std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
 

planning/surround_obstacle_checker/src/debug_marker.cpp

@@ -26,6 +26,29 @@
 
 namespace surround_obstacle_checker
 {
+namespace
+{
+Polygon2d createSelfPolygon(
+  const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0,
+  const double rear_margin = 0.0)
+{
+  const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin;
+  const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin;
+  const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin;
+  const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin;
+
+  Polygon2d ego_polygon;
+
+  ego_polygon.outer().push_back(Point2d(front_m, width_left_m));
+  ego_polygon.outer().push_back(Point2d(front_m, width_right_m));
+  ego_polygon.outer().push_back(Point2d(rear_m, width_right_m));
+  ego_polygon.outer().push_back(Point2d(rear_m, width_left_m));
+
+  bg::correct(ego_polygon);
+
+  return ego_polygon;
+}
+}  // namespace
 
 using motion_utils::createStopVirtualWallMarker;
 using tier4_autoware_utils::appendMarkerArray;
@@ -36,14 +59,18 @@ using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createPoint;
 
 SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
-  const Polygon2d & ego_polygon, const double base_link2front,
-  const double & surround_check_distance, const double & surround_check_recover_distance,
-  const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
-  rclcpp::Node & node)
-: ego_polygon_(ego_polygon),
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
+  const std::string & object_label, const double & surround_check_front_distance,
+  const double & surround_check_side_distance, const double & surround_check_back_distance,
+  const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
+  const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node)
+: vehicle_info_(vehicle_info),
   base_link2front_(base_link2front),
-  surround_check_distance_(surround_check_distance),
-  surround_check_recover_distance_(surround_check_recover_distance),
+  object_label_(object_label),
+  surround_check_front_distance_(surround_check_front_distance),
+  surround_check_side_distance_(surround_check_side_distance),
+  surround_check_back_distance_(surround_check_back_distance),
+  surround_check_hysteresis_distance_(surround_check_hysteresis_distance),
   self_pose_(self_pose),
   clock_(clock)
 {
@@ -86,20 +113,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint(
 
 void SurroundObstacleCheckerDebugNode::publishFootprints()
 {
+  const auto ego_polygon = createSelfPolygon(vehicle_info_);
+
   /* publish vehicle footprint polygon */
-  const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z);
+  const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z);
   vehicle_footprint_pub_->publish(footprint);
 
   /* publish vehicle footprint polygon with offset */
-  const auto polygon_with_offset =
-    createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_);
+  const auto polygon_with_offset = createSelfPolygon(
+    vehicle_info_, surround_check_front_distance_, surround_check_side_distance_,
+    surround_check_back_distance_);
   const auto footprint_with_offset =
     boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z);
   vehicle_footprint_offset_pub_->publish(footprint_with_offset);
 
   /* publish vehicle footprint polygon with recover offset */
-  const auto polygon_with_recover_offset =
-    createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_);
+  const auto polygon_with_recover_offset = createSelfPolygon(
+    vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_,
+    surround_check_side_distance_ + surround_check_hysteresis_distance_,
+    surround_check_back_distance_ + surround_check_hysteresis_distance_);
   const auto footprint_with_recover_offset =
     boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z);
   vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset);
@@ -206,26 +238,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
   return velocity_factor_array;
 }
 
-Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset(
-  const Polygon2d & base_polygon, const double & offset)
-{
-  typedef double coordinate_type;
-  const double buffer_distance = offset;
-  const int points_per_circle = 36;
-  boost::geometry::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy(
-    buffer_distance);
-  boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::side_straight side_strategy;
-  boost::geometry::model::multi_polygon<Polygon2d> result;
-  // Create the buffer of a multi polygon
-  boost::geometry::buffer(
-    base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy,
-    circle_strategy);
-  return result.front();
-}
-
 PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   const Polygon2d & boost_polygon, const double & z)
 {
@@ -244,4 +256,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   return polygon_stamped;
 }
 
+void SurroundObstacleCheckerDebugNode::updateFootprintMargin(
+  const std::string & object_label, const double front_distance, const double side_distance,
+  const double back_distance)
+{
+  object_label_ = object_label;
+  surround_check_front_distance_ = front_distance;
+  surround_check_side_distance_ = side_distance;
+  surround_check_back_distance_ = back_distance;
+}
+
 }  // namespace surround_obstacle_checker