diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 963c3600e6ca5..aaaff2055361c 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/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 diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml index a78fe5ea537d7..f97b2b57794e3 100644 --- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml +++ b/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 diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 0acfa2b8f067a..c836b380e5cf2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/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); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 0af3a1eb96279..9fda908964722 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/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; }; 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 index b28739cd9d073..e372ff7ceb0b2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/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 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 diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 84571085dc61c..964d321eecd0c 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -77,6 +77,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod p.voxel_grid_z = declare_parameter("voxel_grid_z"); p.use_predicted_objects = declare_parameter("use_predicted_objects"); p.publish_obstacle_polygon = declare_parameter("publish_obstacle_polygon"); + p.predicted_object_filtering_threshold = + declare_parameter("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 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( 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) { diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 646fbba0022c0..7ff05e61968c0 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -700,4 +700,34 @@ boost::optional 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