diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 5b634823302c0..bd9c5750af78f 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -87,7 +87,7 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index a069b4b6f9395..43400acb86bd9 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -552,9 +552,10 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const { const auto & p = behavior_determination_param_; - const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose; - const double step_length = p.decimate_trajectory_step_length; - const double time_to_convergence = p.time_to_convergence; + + const double front_length = vehicle_info.max_longitudinal_offset_m; + const double rear_length = vehicle_info.rear_overhang_m; + const double vehicle_width = vehicle_info.vehicle_width_m; const size_t nearest_idx = motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); @@ -563,41 +564,68 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( tier4_autoware_utils::inverseTransformPose(current_ego_pose, nearest_pose); const double current_ego_lat_error = current_ego_pose_error.position.y; const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); - double time_elapsed = 0.0; - - std::vector polygons; - std::vector last_poses = {traj_points.at(0).pose}; - if (is_enable_current_pose_consideration) { - last_poses.push_back(current_ego_pose); - } + double time_elapsed{0.0}; + std::vector output_polygons; + Polygon2d tmp_polys{}; for (size_t i = 0; i < traj_points.size(); ++i) { std::vector current_poses = {traj_points.at(i).pose}; // estimate the future ego pose with assuming that the pose error against the reference path // will decrease to zero by the time_to_convergence - if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) { - const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + if (p.enable_to_consider_current_pose && time_elapsed < p.time_to_convergence) { + const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; geometry_msgs::msg::Pose indexed_pose_err; indexed_pose_err.set__orientation( tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); indexed_pose_err.set__position( tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); - current_poses.push_back( tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); - if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { - time_elapsed += step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); + time_elapsed += + p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); } else { time_elapsed = std::numeric_limits::max(); } } - polygons.push_back( - polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin)); - last_poses = current_poses; + + Polygon2d idx_poly{}; + for (const auto & pose : current_poses) { + if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { + boost::geometry::append( + idx_poly, + tier4_autoware_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + .outer()); + boost::geometry::append( + idx_poly, + tier4_autoware_utils::fromMsg(tier4_autoware_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); + boost::geometry::append( + idx_poly, tier4_autoware_utils::fromMsg( + tier4_autoware_utils::calcOffsetPose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); + } else { + boost::geometry::append( + idx_poly, tier4_autoware_utils::toFootprint( + pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) + .outer()); + } + } + + boost::geometry::append(tmp_polys, idx_poly.outer()); + Polygon2d hull_polygon; + boost::geometry::convex_hull(tmp_polys, hull_polygon); + boost::geometry::correct(hull_polygon); + + output_polygons.push_back(hull_polygon); + tmp_polys = std::move(idx_poly); } - return polygons; + return output_polygons; } std::vector ObstacleCruisePlannerNode::convertToObstacles( diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 6b422fbda5118..fa5a96b934f7a 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -20,18 +20,6 @@ namespace { -void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) -{ - Point2d point(geom_point.x, geom_point.y); - bg::append(polygon.outer(), point); -} - -geometry_msgs::msg::Point calcOffsetPosition( - const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) -{ - return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position; -} - PointWithStamp calcNearestCollisionPoint( const size_t first_within_idx, const std::vector & collision_points, const std::vector & decimated_traj_points, const bool is_driving_forward) @@ -104,38 +92,6 @@ std::optional>> getCollisionIndex( namespace polygon_utils { -Polygon2d createOneStepPolygon( - const std::vector & last_poses, - const std::vector & current_poses, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - Polygon2d polygon; - - const double base_to_front = vehicle_info.max_longitudinal_offset_m; - const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; - const double base_to_rear = vehicle_info.rear_overhang_m; - - for (auto & pose : last_poses) { - appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); - } - for (auto & pose : current_poses) { - appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); - } - - bg::correct(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - std::optional> getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward,