feat(obstacle_stop_planner): add filtering feature for predicted obje…

…cts (autowarefoundation#3558)

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

planning/obstacle_stop_planner/README.md

@@ -31,14 +31,16 @@
 
 ### 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] |
-| `enable_z_axis_obstacle_filtering` | bool   | filter obstacles in z axis (height) [-]                                                   |
-| `z_axis_filtering_buffer`          | double | additional buffer for z axis filtering [m]]                                               |
-| `use_predicted_objects`            | bool   | whether to use predicted objects for collision and slowdown detection [-]]                |
+| 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]]                                               |
+| `use_predicted_objects`                | bool   | whether to use predicted objects for collision and slowdown detection [-]]                |
+| `predicted_object_filtering_threshold` | double | threshold for filtering predicted objects [valid only publish_obstacle_polygon true](m)]  |
+| `publish_obstacle_polygon`             | bool   | if use_predicted_objects is true, node publishes collision polygon [-]]                   |
 
 ## Obstacle Stop Planner
 

planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml

@@ -1,15 +1,16 @@
 /**:
   ros__parameters:
-    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]
-    use_predicted_objects : False            # whether to use predicted objects [-]
-    publish_obstacle_polygon: False          # whether to publish obstacle polygon [-]
+    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]
+    use_predicted_objects : False             # whether to use predicted objects [-]
+    publish_obstacle_polygon: False           # whether to publish obstacle polygon [-]
+    predicted_object_filtering_threshold: 1.5 # threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]
 
     stop_planner:
       # params for stop position

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -159,6 +159,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   bool set_velocity_limit_{false};
 
+  double object_filtering_margin_{};  // only valid if use_predicted_objects is true
+
   VehicleInfo vehicle_info_;
   NodeParam node_param_;
   StopParam stop_param_;
@@ -218,6 +220,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node
   void publishDebugData(
     const PlannerData & planner_data, const double current_acc, const double current_vel);
 
+  void filterObstacles(
+    const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj,
+    const double dist_threshold, PredictedObjects & filtered_objects);
+
   // Callback
   void onTrigger(const Trajectory::ConstSharedPtr input_msg);
 

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp

@@ -87,8 +87,14 @@ struct NodeParam
   // voxel grid z parameter for filtering pointcloud [m]
   double voxel_grid_z;
 
+  // It uses only predicted objects for slowdown and collision checking
   bool use_predicted_objects;
 
+  // If use_predicted_objects is true, objects are ignored if distance to trajectory is larger than
+  // this value [m]
+  double predicted_object_filtering_threshold;
+
+  // If use_predicted_objects is true, node publishes collision polygon
   bool publish_obstacle_polygon;
 };
 

planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp

@@ -149,6 +149,10 @@ pcl::PointXYZ pointToPcl(const double x, const double y, const double z);
 boost::optional<PredictedObject> getObstacleFromUuid(
   const PredictedObjects & obstacles, const unique_identifier_msgs::msg::UUID & target_object_id);
 
+bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos);
+
+double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape);
+
 rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr);
 
 }  // namespace motion_planning

planning/obstacle_stop_planner/src/node.cpp

@@ -77,6 +77,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
     p.voxel_grid_z = declare_parameter<double>("voxel_grid_z");
     p.use_predicted_objects = declare_parameter<bool>("use_predicted_objects");
     p.publish_obstacle_polygon = declare_parameter<bool>("publish_obstacle_polygon");
+    p.predicted_object_filtering_threshold =
+      declare_parameter<double>("predicted_object_filtering_threshold");
   }
 
   {
@@ -170,6 +172,22 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
       std::hypot(i.vehicle_width_m / 2.0 + p.lateral_margin, i.vehicle_length_m / 2.0);
   }
 
+  if (node_param_.use_predicted_objects) {
+    // Search the maximum lateral margin
+    std::vector<double> lateral_margins{
+      stop_param_.pedestrian_lateral_margin, stop_param_.vehicle_lateral_margin,
+      stop_param_.unknown_lateral_margin};
+    if (node_param_.enable_slow_down) {
+      lateral_margins.push_back(slow_down_param_.pedestrian_lateral_margin);
+      lateral_margins.push_back(slow_down_param_.vehicle_lateral_margin);
+      lateral_margins.push_back(slow_down_param_.unknown_lateral_margin);
+    }
+    const double max_lateral_margin =
+      *std::max_element(lateral_margins.begin(), lateral_margins.end());
+    object_filtering_margin_ =
+      max_lateral_margin + node_param_.predicted_object_filtering_threshold;
+  }
+
   // Initializer
   acc_controller_ = std::make_unique<AdaptiveCruiseController>(
     this, i.vehicle_width_m, i.vehicle_length_m, i.max_longitudinal_offset_m);
@@ -580,9 +598,13 @@ void ObstacleStopPlannerNode::searchPredictedObject(
 {
   mutex_.lock();
   const auto object_ptr = object_ptr_;
+  const auto current_odometry_pointer = current_odometry_ptr_;
   mutex_.unlock();
 
-  // TODO(brkay54): Add filtering function here!
+  const auto ego_pose = current_odometry_pointer->pose.pose;
+  PredictedObjects filtered_objects;
+  filterObstacles(
+    *object_ptr.get(), ego_pose, decimate_trajectory, object_filtering_margin_, filtered_objects);
 
   const auto now = this->now();
 
@@ -602,8 +624,8 @@ void ObstacleStopPlannerNode::searchPredictedObject(
       geometry_msgs::msg::Point nearest_slow_down_point;
       geometry_msgs::msg::PoseArray slow_down_points;
 
-      for (size_t j = 0; j < object_ptr->objects.size(); ++j) {
-        const auto & obj = object_ptr->objects.at(j);
+      for (size_t j = 0; j < filtered_objects.objects.size(); ++j) {
+        const auto & obj = filtered_objects.objects.at(j);
         if (node_param_.enable_z_axis_obstacle_filtering) {
           if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) {
             continue;
@@ -692,9 +714,9 @@ void ObstacleStopPlannerNode::searchPredictedObject(
         planner_data.slow_down_require = true;
         planner_data.nearest_slow_down_point =
           pointToPcl(nearest_slow_down_point.x, nearest_slow_down_point.y, p_front.position.z);
-        planner_data.nearest_collision_point_time = object_ptr->header.stamp;
+        planner_data.nearest_collision_point_time = filtered_objects.header.stamp;
         planner_data.slow_down_object_shape =
-          object_ptr->objects.at(nearest_slow_down_object_index).shape;
+          filtered_objects.objects.at(nearest_slow_down_object_index).shape;
 
         // TODO(brkay54): lateral_nearest_slow_down_point_pose and nearest_slow_down_point_pose are
         // not used
@@ -705,7 +727,7 @@ void ObstacleStopPlannerNode::searchPredictedObject(
         // Push slow down debugging points
         Polygon2d one_step_move_slow_down_vehicle_polygon;
 
-        const auto & obj = object_ptr->objects.at(nearest_slow_down_object_index);
+        const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index);
         if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
           createOneStepPolygon(
             p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info,
@@ -741,8 +763,8 @@ void ObstacleStopPlannerNode::searchPredictedObject(
       size_t nearest_collision_object_index = 0;
       geometry_msgs::msg::Point nearest_collision_point;
 
-      for (size_t j = 0; j < object_ptr->objects.size(); ++j) {
-        const auto & obj = object_ptr->objects.at(j);
+      for (size_t j = 0; j < filtered_objects.objects.size(); ++j) {
+        const auto & obj = filtered_objects.objects.at(j);
         if (node_param_.enable_z_axis_obstacle_filtering) {
           if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) {
             continue;
@@ -818,7 +840,8 @@ void ObstacleStopPlannerNode::searchPredictedObject(
       }
       if (is_init) {
         predicted_object_history_.emplace_back(
-          now, nearest_collision_point, object_ptr->objects.at(nearest_collision_object_index));
+          now, nearest_collision_point,
+          filtered_objects.objects.at(nearest_collision_object_index));
         break;
       }
 
@@ -955,7 +978,7 @@ void ObstacleStopPlannerNode::searchPredictedObject(
       const auto current_odometry_ptr = current_odometry_ptr_;
       const auto latest_object_ptr = object_ptr_;
       mutex_.unlock();
-      // find latest state of predicted object
+      // find latest state of predicted object to get latest velocity and acceleration values
       auto obj_latest_state = getObstacleFromUuid(*latest_object_ptr, obj.object_id);
       if (!obj_latest_state) {
         // Can not find the object in the latest object list. Send previous state.
@@ -1530,6 +1553,46 @@ void ObstacleStopPlannerNode::resetExternalVelocityLimit(
   RCLCPP_INFO(get_logger(), "reset velocity limit");
 }
 
+void ObstacleStopPlannerNode::filterObstacles(
+  const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj,
+  const double dist_threshold, PredictedObjects & filtered_objects)
+{
+  filtered_objects.header = input_objects.header;
+
+  for (auto & object : input_objects.objects) {
+    // Check is it in front of ego vehicle
+    if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {
+      continue;
+    }
+
+    // Check is it near to trajectory
+    const double max_length = calcObstacleMaxLength(object.shape);
+    const size_t seg_idx = motion_utils::findNearestSegmentIndex(
+      traj, object.kinematics.initial_pose_with_covariance.pose.position);
+    const auto p_front = tier4_autoware_utils::getPoint(traj.at(seg_idx));
+    const auto p_back = tier4_autoware_utils::getPoint(traj.at(seg_idx + 1));
+    const auto & p_target = object.kinematics.initial_pose_with_covariance.pose.position;
+    const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
+    const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};
+
+    if (seg_idx == traj.size() - 2) {
+      // Calculate longitudinal offset
+      const auto longitudinal_dist = std::abs(segment_vec.dot(target_vec) / segment_vec.norm());
+      if (
+        longitudinal_dist - max_length - vehicle_info_.max_longitudinal_offset_m - dist_threshold >
+        0.0) {
+        continue;
+      }
+    }
+    const auto lateral_dist = std::abs(segment_vec.cross(target_vec)(2) / segment_vec.norm());
+    if (lateral_dist - max_length - vehicle_info_.max_lateral_offset_m - dist_threshold > 0.0) {
+      continue;
+    }
+    PredictedObject filtered_object = object;
+    filtered_objects.objects.push_back(filtered_object);
+  }
+}
+
 void ObstacleStopPlannerNode::publishDebugData(
   const PlannerData & planner_data, const double current_acc, const double current_vel)
 {

planning/obstacle_stop_planner/src/planner_utils.cpp

@@ -700,4 +700,34 @@ boost::optional<PredictedObject> getObstacleFromUuid(
   return boost::make_optional(*itr);
 }
 
+bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos)
+{
+  const auto yaw = tier4_autoware_utils::getRPY(ego_pose).z;
+  const Eigen::Vector2d base_pose_vec(std::cos(yaw), std::sin(yaw));
+  const Eigen::Vector2d obstacle_vec(
+    obstacle_pos.x - ego_pose.position.x, obstacle_pos.y - ego_pose.position.y);
+
+  return base_pose_vec.dot(obstacle_vec) >= 0;
+}
+
+double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape)
+{
+  if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0);
+  } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
+    return shape.dimensions.x / 2.0;
+  } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
+    double max_length_to_point = 0.0;
+    for (const auto rel_point : shape.footprint.points) {
+      const double length_to_point = std::hypot(rel_point.x, rel_point.y);
+      if (max_length_to_point < length_to_point) {
+        max_length_to_point = length_to_point;
+      }
+    }
+    return max_length_to_point;
+  }
+
+  throw std::logic_error("The shape type is not supported in obstacle_cruise_planner.");
+}
+
 }  // namespace motion_planning