diff --git a/common/autoware_ad_api_msgs/CMakeLists.txt b/common/autoware_ad_api_msgs/CMakeLists.txt index ff8a6382a247..031134af5a5d 100644 --- a/common/autoware_ad_api_msgs/CMakeLists.txt +++ b/common/autoware_ad_api_msgs/CMakeLists.txt @@ -5,7 +5,8 @@ find_package(autoware_cmake REQUIRED) autoware_package() rosidl_generate_interfaces(${PROJECT_NAME} - srv/InterfaceVersion.srv + common/msg/ResponseStatus.msg + interface/srv/InterfaceVersion.srv DEPENDENCIES builtin_interfaces std_msgs diff --git a/common/autoware_ad_api_msgs/README.md b/common/autoware_ad_api_msgs/README.md index d6c01f285425..9a17d7c90393 100644 --- a/common/autoware_ad_api_msgs/README.md +++ b/common/autoware_ad_api_msgs/README.md @@ -1,17 +1,25 @@ # autoware_ad_api_msgs -## InterfaceVersion +## ResponseStatus -This API provides the interface version of the set of AD APIs. -It follows [Semantic Versioning][semver] in order to provide an intuitive understanding of the changes between versions. +This message is a response status commonly used in the service type API. For details, see [the response status][docs-response-status]. +Each API can define its own status codes. +The status codes are primarily used to indicate the error cause, such as invalid parameter and timeout. +If the API succeeds, set success to true, code to zero, and message to the empty string. +Alternatively, codes and messages can be used for warnings or additional information. +If the API fails, set success to false, code to the related status code, and message to the information. +The status code zero is reserved for success. The status code 50000 or over are also reserved for typical cases. -### Use cases +| Name | Code | Description | +| ---------- | ----: | ------------------------------------ | +| SUCCESS | 0 | This API has completed successfully. | +| DEPRECATED | 50000 | This API is deprecated. | -Considering the product life cycle, there will be multiple vehicles that use different versions of the AD API due to changes in requirements or some improvements. -For example, a vehicle uses `v1` for stability and another vehicle uses `v2` for more functionality. +## InterfaceVersion -In that situation, the AD API users such as developers of a Web service have to switch the application behavior based on the version that each vehicle uses. +This message is for the interface version of the set of AD APIs. For details, see [the interface feature][docs-interface]. -[semver]: https://semver.org/ +[docs-response-status]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/#response-status +[docs-interface]: https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/interface/ diff --git a/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg new file mode 100644 index 000000000000..efeaa68535e7 --- /dev/null +++ b/common/autoware_ad_api_msgs/common/msg/ResponseStatus.msg @@ -0,0 +1,7 @@ +# constants for code +uint16 DEPRECATED = 50000 + +# variables +bool success +uint16 code +string message diff --git a/common/autoware_ad_api_msgs/srv/InterfaceVersion.srv b/common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv similarity index 100% rename from common/autoware_ad_api_msgs/srv/InterfaceVersion.srv rename to common/autoware_ad_api_msgs/interface/srv/InterfaceVersion.srv diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp index 010fba8272ce..6cd729841fe7 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp @@ -22,6 +22,7 @@ #include "motion_common/motion_common.hpp" #include "motion_common/trajectory_common.hpp" #include "tf2/utils.h" +#include "tier4_autoware_utils/trajectory/trajectory.hpp" #include "trajectory_follower/visibility_control.hpp" #include // NOLINT @@ -60,8 +61,9 @@ TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj); /** * @brief calculate distance to stopline from current vehicle position where velocity is 0 */ -TRAJECTORY_FOLLOWER_PUBLIC float64_t -calcStopDistance(const Point & current_pos, const Trajectory & traj); +TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, + const float64_t max_yaw); /** * @brief calculate pitch angle from estimated current pose @@ -106,21 +108,23 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6 * @param [in] point Interpolated point is nearest to this point. */ template -TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint -lerpTrajectoryPoint(const T & points, const Point & point) +TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint( + const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw) { TrajectoryPoint interpolated_point; - - const size_t nearest_seg_idx = trajectory_common::findNearestSegmentIndex(points, point); + auto seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose, max_dist, max_yaw); + if (!seg_idx) { // if not fund idx + seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, pose); + } const float64_t len_to_interpolated = - trajectory_common::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); + tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, *seg_idx, pose.position); const float64_t len_segment = - trajectory_common::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); + trajectory_common::calcSignedArcLength(points, *seg_idx, *seg_idx + 1); const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); { - const size_t i = nearest_seg_idx; + const size_t i = *seg_idx; interpolated_point.pose.position.x = motion_common::interpolate( points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio); diff --git a/control/trajectory_follower/package.xml b/control/trajectory_follower/package.xml index 840cad74aeb5..f43ffdb07ed2 100644 --- a/control/trajectory_follower/package.xml +++ b/control/trajectory_follower/package.xml @@ -27,6 +27,7 @@ std_msgs tf2 tf2_ros + tier4_autoware_utils ament_cmake_ros ament_lint_auto diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 1f5018f84540..db9370ba8dab 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -62,17 +62,31 @@ bool isValidTrajectory(const Trajectory & traj) return true; } -float64_t calcStopDistance(const Point & current_pos, const Trajectory & traj) +float64_t calcStopDistance( + const Pose & current_pose, const Trajectory & traj, const float64_t max_dist, + const float64_t max_yaw) { const std::experimental::optional stop_idx_opt = trajectory_common::searchZeroVelocityIndex(traj.points); + auto seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose, max_dist, max_yaw); + if (!seg_idx) { // if not fund idx + seg_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose); + } + const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( + traj.points, *seg_idx, current_pose.position); + // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { - return trajectory_common::calcSignedArcLength(traj.points, current_pos, traj.points.size() - 1); + float64_t signed_length_on_traj = + tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, traj.points.size() - 1); + return signed_length_on_traj - signed_length_src_offset; } - return trajectory_common::calcSignedArcLength(traj.points, current_pos, *stop_idx_opt); + float64_t signed_length_on_traj = + tier4_autoware_utils::calcSignedArcLength(traj.points, *seg_idx, *stop_idx_opt); + return signed_length_on_traj - signed_length_src_offset; } float64_t getPitchByPose(const Quaternion & quaternion_msg) diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index 437a7882f96d..354c483982a8 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -50,20 +50,26 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using geometry_msgs::msg::Point; - Point current_pos; - current_pos.x = 0.0; - current_pos.y = 0.0; + using geometry_msgs::msg::Pose; + Pose current_pose; + current_pose.position.x = 0.0; + current_pose.position.y = 0.0; + current_pose.position.z = 0.0; Trajectory traj; + double max_dist = 3.0; + double max_yaw = 0.7; // empty trajectory : exception - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::invalid_argument); + EXPECT_THROW( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), + std::invalid_argument); // one point trajectory : exception TrajectoryPoint point; point.pose.position.x = 0.0; point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW(longitudinal_utils::calcStopDistance(current_pos, traj), std::out_of_range); + EXPECT_THROW( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0; @@ -78,7 +84,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 1.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 2.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 2.0); // stopping trajectory: stop distance = length until stopping point point.pose.position.x = 3.0; point.pose.position.y = 0.0; @@ -92,7 +98,7 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pos, traj), 3.0); + EXPECT_EQ(longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 3.0); } TEST(TestLongitudinalControllerUtils, getPitchByPose) @@ -319,7 +325,7 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using geometry_msgs::msg::Point; + using geometry_msgs::msg::Pose; const double abs_err = 1e-15; decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; @@ -344,72 +350,73 @@ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) p.acceleration_mps2 = 40.0; points.push_back(p); TrajectoryPoint result; - Point point; - + Pose pose; + double max_dist = 3.0; + double max_yaw = 0.7; // Points on the trajectory gives back the original trajectory points values - point.x = 0.0; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 0.0; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err); - point.x = 1.0; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 1.0; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err); - point.x = 1.0; - point.y = 1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 1.0; + pose.position.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err); - point.x = 2.0; - point.y = 1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 2.0; + pose.position.y = 1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err); // Interpolate between trajectory points - point.x = 0.5; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + pose.position.x = 0.5; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); - point.x = 0.75; - point.y = 0.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); + pose.position.x = 0.75; + pose.position.y = 0.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); - EXPECT_NEAR(result.pose.position.y, point.y, abs_err); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); + EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err); EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err); // Interpolate away from the trajectory (interpolated point is projected) - point.x = 0.5; - point.y = -1.0; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + pose.position.x = 0.5; + pose.position.y = -1.0; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); // Ambiguous projections: possibility with the lowest index is used - point.x = 0.5; - point.y = 0.5; - result = longitudinal_utils::lerpTrajectoryPoint(points, point); - EXPECT_NEAR(result.pose.position.x, point.x, abs_err); + pose.position.x = 0.5; + pose.position.y = 0.5; + result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw); + EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err); EXPECT_NEAR(result.pose.position.y, 0.0, abs_err); EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err); EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err); diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp index 9832f0dfd6e0..f9825902954f 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/longitudinal_controller_node.hpp @@ -326,7 +326,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC LongitudinalController : public rclcpp::Node */ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedTargetValue( const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Point & point, const size_t nearest_idx) const; + const geometry_msgs::msg::Pose & pose, const size_t nearest_idx) const; /** * @brief calculate predicted velocity after time delay based on past control commands diff --git a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp index 70930416f841..fb3cda35d831 100644 --- a/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp +++ b/control/trajectory_follower_nodes/src/longitudinal_controller_node.cpp @@ -425,7 +425,7 @@ LongitudinalController::ControlData LongitudinalController::getControlData( // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose.position, *m_trajectory_ptr); + current_pose, *m_trajectory_ptr, max_dist, max_yaw); // pitch const float64_t raw_pitch = @@ -553,7 +553,7 @@ LongitudinalController::Motion LongitudinalController::calcCtrlCmd( const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( current_pose, m_delay_compensation_time, current_vel); const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose.position, nearest_idx); + calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose, nearest_idx); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, target_interpolated_point.acceleration_mps2}; @@ -800,8 +800,8 @@ LongitudinalController::Motion LongitudinalController::keepBrakeBeforeStop( autoware_auto_planning_msgs::msg::TrajectoryPoint LongitudinalController::calcInterpolatedTargetValue( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Point & point, const size_t nearest_idx) const + const autoware_auto_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose, + const size_t nearest_idx) const { if (traj.points.size() == 1) { return traj.points.at(0); @@ -810,18 +810,21 @@ LongitudinalController::calcInterpolatedTargetValue( // If the current position is not within the reference trajectory, enable the edge value. // Else, apply linear interpolation if (nearest_idx == 0) { - if (motion_common::calcSignedArcLength(traj.points, point, 0) > 0) { + if (motion_common::calcSignedArcLength(traj.points, pose.position, 0) > 0) { return traj.points.at(0); } } if (nearest_idx == traj.points.size() - 1) { - if (motion_common::calcSignedArcLength(traj.points, point, traj.points.size() - 1) < 0) { + if ( + motion_common::calcSignedArcLength(traj.points, pose.position, traj.points.size() - 1) < 0) { return traj.points.at(traj.points.size() - 1); } } // apply linear interpolation - return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint(traj.points, point); + return trajectory_follower::longitudinal_utils::lerpTrajectoryPoint( + traj.points, pose, m_state_transition_params.emergency_state_traj_trans_dev, + m_state_transition_params.emergency_state_traj_rot_dev); } float64_t LongitudinalController::predictedVelocityInTargetPoint( @@ -920,7 +923,7 @@ void LongitudinalController::updateDebugVelAcc( const size_t nearest_idx = control_data.nearest_idx; const auto interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose.position, nearest_idx); + calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose, nearest_idx); m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 1725a4d0193d..bfcaeb820183 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -10,6 +10,8 @@ intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8fbdeb25d398..62f55ebd475a 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -25,6 +25,16 @@ + + + + + + + + + + @@ -54,18 +64,6 @@ - - - - - - - - - - - - diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index 2a5d48deee54..6737da3e4ec8 100644 --- a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -35,6 +35,10 @@ lanelet::LineString3d generateFineCenterline( const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0); lanelet::ConstLineString3d getCenterlineWithOffset( const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0); lanelet::ConstLanelet getExpandedLanelet( const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset); diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp index 450da120e137..6a274488bb6b 100644 --- a/map/lanelet2_extension/lib/utilities.cpp +++ b/map/lanelet2_extension/lib/utilities.cpp @@ -282,7 +282,7 @@ lanelet::ConstLineString3d getCenterlineWithOffset( const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); // Resample points - const auto left_points = lanelet::utils::resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); // Create centerline @@ -303,6 +303,65 @@ lanelet::ConstLineString3d getCenterlineWithOffset( return static_cast(centerline); } +lanelet::ConstLineString3d getRightBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d rightBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + const auto vec_left_2_right = (right_points.at(i) - left_points.at(i)).normalized(); + + const auto offset_right_basic_point = right_points.at(i) + vec_left_2_right * offset; + + const lanelet::Point3d rightBound_point( + lanelet::utils::getId(), offset_right_basic_point.x(), offset_right_basic_point.y(), + offset_right_basic_point.z()); + rightBound.push_back(rightBound_point); + } + return static_cast(rightBound); +} + +lanelet::ConstLineString3d getLeftBoundWithOffset( + const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution) +{ + // Get length of longer border + const double left_length = lanelet::geometry::length(lanelet_obj.leftBound()); + const double right_length = lanelet::geometry::length(lanelet_obj.rightBound()); + const double longer_distance = (left_length > right_length) ? left_length : right_length; + const int num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + // Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + // Create centerline + lanelet::LineString3d leftBound(lanelet::utils::getId()); + for (int i = 0; i < num_segments + 1; i++) { + // Add ID for the average point of left and right + + const auto vec_right_2_left = (left_points.at(i) - right_points.at(i)).normalized(); + + const auto offset_left_basic_point = left_points.at(i) + vec_right_2_left * offset; + + const lanelet::Point3d leftBound_point( + lanelet::utils::getId(), offset_left_basic_point.x(), offset_left_basic_point.y(), + offset_left_basic_point.z()); + leftBound.push_back(leftBound_point); + } + return static_cast(leftBound); +} + lanelet::ConstLanelet getExpandedLanelet( const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset) { diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 244880dadeb6..49d294ff1bb7 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -39,6 +39,17 @@ rclcpp_components_register_node(lanelet2_map_visualization_node EXECUTABLE lanelet2_map_visualization ) +if(BUILD_TESTING) + add_ros_test( + test/lanelet2_map_loader_launch.test.py + TIMEOUT "30" + ) + install(DIRECTORY + test/data/ + DESTINATION share/${PROJECT_NAME}/test/data/ + ) +endif() + install(PROGRAMS script/map_hash_generator DESTINATION lib/${PROJECT_NAME} diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index aa345d6a153f..a2afc96c24e5 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -27,6 +27,7 @@ ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/map/map_loader/test/data/test_map.osm b/map/map_loader/test/data/test_map.osm new file mode 100644 index 000000000000..406cd85c151e --- /dev/null +++ b/map/map_loader/test/data/test_map.osm @@ -0,0 +1,146 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py new file mode 100644 index 000000000000..75abe2164ca0 --- /dev/null +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +# Copyright 2022 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_testing +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + + lanelet2_map_path = os.path.join( + get_package_share_directory("map_loader"), "test/data/test_map.osm" + ) + + lanelet2_map_loader = Node( + package="map_loader", + executable="lanelet2_map_loader", + parameters=[{"lanelet2_map_path": lanelet2_map_path}], + ) + + context = {} + + return ( + LaunchDescription( + [ + lanelet2_map_loader, + # Start test after 1s - gives time for the map_loader to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c4df020077b2..d3b61e13eba5 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -426,10 +426,12 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // Step4. Check if the closest lanelet is valid, and add all // of the lanelets that are below max_dist and max_delta_yaw + const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const bool is_yaw_reversed = + M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; if ( lanelet.first < dist_threshold_for_searching_lanelet_ && - (M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta || - abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_)) { + (is_yaw_reversed || abs_norm_delta < delta_yaw_threshold_for_searching_lanelet_)) { return true; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 0cffa379fd89..ea891ce9bec8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -174,8 +174,8 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double overhang) - : object(obj), lateral(lat), longitudinal(lon), overhang_dist(overhang) + ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) + : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang) { } @@ -187,6 +187,9 @@ struct ObjectData // avoidance target // longitudinal position of the CoM, in Frenet coordinate from ego-pose double longitudinal; + // longitudinal length of vehicle, in Frenet coordinate + double length; + // lateral distance to the closest footprint, in Frenet coordinate double overhang_dist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 14bbf6a37c4a..8fb8d132a541 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner @@ -43,8 +44,9 @@ double lerpShiftLengthOnArc(double arc, const AvoidPoint & ap); void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & path); -double calcDistanceToClosestFootprintPoint( - const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos); +void fillLongitudinalAndLengthByClosestFootprint( + const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos, + ObjectData & obj); double calcOverhangDistance( const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2923996ca869..15c6039d42d4 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -216,7 +216,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( object_data.object = object; avoidance_debug_msg.object_id = getUuidStr(object_data); // calc longitudinal distance from ego to closest target object footprint point. - object_data.longitudinal = calcDistanceToClosestFootprintPoint(reference_path, object, ego_pos); + fillLongitudinalAndLengthByClosestFootprint(reference_path, object, ego_pos, object_data); avoidance_debug_msg.longitudinal_distance = object_data.longitudinal; // object is behind ego or too far. @@ -465,8 +465,8 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( const auto & lat_collision_margin = parameters_.lateral_collision_margin; const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin; - const auto max_allowable_lateral_distance = - lat_collision_safety_buffer + lat_collision_margin + vehicle_width; + const auto max_allowable_lateral_distance = lat_collision_safety_buffer + lat_collision_margin + + vehicle_width - road_shoulder_safety_margin; const auto avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width; @@ -494,20 +494,9 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( continue; } - const auto max_shift_length = - o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width; - - const auto max_left_shift_limit = [&max_shift_length, this]() noexcept { - return std::min(getLeftShiftBound(), max_shift_length); - }; - - const auto max_right_shift_limit = [&max_shift_length, this]() noexcept { - return std::max(getRightShiftBound(), -max_shift_length); - }; - const auto shift_length = isOnRight(o) - ? std::min(o.overhang_dist + avoid_margin, max_left_shift_limit()) - : std::max(o.overhang_dist - avoid_margin, max_right_shift_limit()); + ? std::min(o.overhang_dist + avoid_margin, getLeftShiftBound()) + : std::max(o.overhang_dist - avoid_margin, getRightShiftBound()); const auto avoiding_shift = shift_length - current_ego_shift; const auto return_shift = shift_length; @@ -554,7 +543,6 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( parameters_.nominal_lateral_jerk, getNominalAvoidanceEgoSpeed(), prepare_distance, has_enough_distance); - // TODO(Horibe): add margin with object length. __/\__ -> __/¯¯\__ AvoidPoint ap_avoid; ap_avoid.length = shift_length; ap_avoid.start_length = current_ego_shift; @@ -573,9 +561,9 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( AvoidPoint ap_return; ap_return.length = 0.0; ap_return.start_length = shift_length; - ap_return.start_longitudinal = o.longitudinal; + ap_return.start_longitudinal = o.longitudinal + o.length; ap_return.end_longitudinal = - o.longitudinal + std::min(nominal_return_distance, return_remaining_distance); + o.longitudinal + o.length + std::min(nominal_return_distance, return_remaining_distance); ap_return.id = getOriginalShiftPointUniqueId(); ap_return.object = o; avoid_points.push_back(ap_return); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index e4b8afbe8777..59abc9bb0753 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -125,20 +125,27 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa std::vector{path.points.begin() + min_start_idx, path.points.end()}; } -double calcDistanceToClosestFootprintPoint( - const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos) +void fillLongitudinalAndLengthByClosestFootprint( + const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos, + ObjectData & obj) { tier4_autoware_utils::Polygon2d object_poly{}; util::calcObjectPolygon(object, &object_poly); - double distance = tier4_autoware_utils::calcSignedArcLength( + const double distance = tier4_autoware_utils::calcSignedArcLength( path.points, ego_pos, object.kinematics.initial_pose_with_covariance.pose.position); + double min_distance = distance; + double max_distance = distance; for (const auto & p : object_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - distance = - std::min(distance, tier4_autoware_utils::calcSignedArcLength(path.points, ego_pos, point)); + const double arc_length = + tier4_autoware_utils::calcSignedArcLength(path.points, ego_pos, point); + min_distance = std::min(min_distance, arc_length); + max_distance = std::max(max_distance, arc_length); } - return distance; + obj.longitudinal = min_distance; + obj.length = max_distance - min_distance; + return; } double calcOverhangDistance( diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 1725a4d0193d..bfcaeb820183 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -10,6 +10,8 @@ intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss detection_area_margin: 0.5 # [m] + detection_area_right_margin: 0.5 # [m] + detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml index 6d723c510cce..936bfc76cc98 100644 --- a/planning/behavior_velocity_planner/config/stop_line.param.yaml +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -4,5 +4,6 @@ stop_margin: 0.0 stop_check_dist: 2.0 stop_duration_sec: 1.0 + use_initialization_stop_line_state: true debug: show_stopline_collision_check: false # [-] whether to show stopline collision diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index d55b301d17ee..04352513f927 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -89,6 +89,7 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon ego_lane_polygon; + std::vector detection_area_with_margin; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; @@ -111,8 +112,12 @@ class IntersectionModule : public SceneModuleInterface double intersection_velocity; //! used for intersection passing time double intersection_max_acc; //! used for calculating intersection velocity double detection_area_margin; //! used for detecting objects in detection area - double detection_area_length; //! used to create detection area polygon - double detection_area_angle_thr; //! threshold in checking the angle of detecting objects + double detection_area_right_margin; //! used for detecting objects in detection area only right + //! direction + double detection_area_left_margin; //! used for detecting objects in detection area only left + //! direction + double detection_area_length; //! used to create detection area polygon + double detection_area_angle_thr; //! threshold in checking the angle of detecting objects double min_predicted_path_confidence; //! minimum confidence value of predicted path to use for collision detection double external_input_timeout; //! used to disable external input @@ -149,7 +154,6 @@ class IntersectionModule : public SceneModuleInterface * actual collision check algorithm inside this function) * @param lanelet_map_ptr lanelet map * @param path ego-car lane - * @param detection_areas collision check is performed for vehicles that exist in this area * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet * ids * @param objects_ptr target objects @@ -159,7 +163,6 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::vector & detection_areas, const std::vector & detection_area_lanelet_ids, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 09ac15e48c0c..208fee06504d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -101,6 +101,8 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface { double decel_velocity; double detection_area_length; + double detection_area_right_margin; + double detection_area_left_margin; double stop_line_margin; double stop_duration_sec; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 6a1a8b0c4f3f..fa5aee9044a4 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -56,9 +56,11 @@ bool hasDuplicatedPoint( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, - std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, const rclcpp::Logger logger); + const int lane_id, const double detection_area_length, const double right_margin, + const double left_margin, std::vector * conflicting_lanelets_result, + std::vector * objective_lanelets_result, + std::vector * objective_lanelets_with_margin_result, + const rclcpp::Logger logger); /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, @@ -107,6 +109,8 @@ double calcArcLengthFromPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const size_t src_idx, const size_t dst_idx); +lanelet::ConstLanelet generateOffsetLanelet( + const lanelet::ConstLanelet lanelet, double right_margin, double left_margin); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp index 371873eab83c..d109398e33d5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -72,6 +72,7 @@ class StopLineModule : public SceneModuleInterface double stop_margin; double stop_check_dist; double stop_duration_sec; + bool use_initialization_stop_line_state; bool show_stopline_collision_check; }; diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index 729af3099f81..609b8115d675 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -146,28 +146,9 @@ inline bool smoothPath( const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); auto nearest_idx = tier4_autoware_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position); - const auto dist_to_nearest = tier4_autoware_utils::calcSignedArcLength( - *traj_lateral_acc_filtered, current_pose.position, nearest_idx); - - // if trajectory has the almost same point as ego, don't insert the ego point - constexpr double epsilon = 1e-2; - TrajectoryPoints traj_with_ego_point_on_path = *traj_lateral_acc_filtered; - if (std::fabs(dist_to_nearest) > epsilon) { - // calc ego internal division point on path - const auto traj_with_ego_point_with_idx = - getLerpTrajectoryPointWithIdx(*traj_lateral_acc_filtered, current_pose.position); - if (traj_with_ego_point_with_idx == boost::none) return false; - TrajectoryPoint ego_point_on_path = traj_with_ego_point_with_idx->first; - const size_t nearest_seg_idx = traj_with_ego_point_with_idx->second; - //! insert ego projected pose on path so new nearest segment will be nearest_seg_idx + 1 - traj_with_ego_point_on_path.insert( - traj_with_ego_point_on_path.begin() + nearest_seg_idx, ego_point_on_path); - - // ego point inserted is new nearest point - nearest_idx = nearest_seg_idx + 1; - } + // Resample trajectory with ego-velocity based interval distances - auto traj_resampled = smoother->resampleTrajectory(traj_with_ego_point_on_path, v0, nearest_idx); + auto traj_resampled = smoother->resampleTrajectory(*traj_lateral_acc_filtered, v0, nearest_idx); const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4); if (!traj_resampled_closest) { return false; diff --git a/planning/behavior_velocity_planner/run-out-design.md b/planning/behavior_velocity_planner/run-out-design.md index cea0a18c6321..bfb3a8b25078 100644 --- a/planning/behavior_velocity_planner/run-out-design.md +++ b/planning/behavior_velocity_planner/run-out-design.md @@ -80,7 +80,7 @@ Abstracted obstacle data has following information. | min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | | max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | -Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](#Collision-detection). +Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](./run-out-design.md#Collision-detection). Future work: Determine the maximum/minimum velocity from the estimated velocity with covariance of the object @@ -114,7 +114,7 @@ You can choose whether to use this feature by parameter of `use_partition_lanele Along the ego vehicle path, determine the points where collision detection is to be performed for each `detection_span`. -The travel times to the each points are calculated from [the expected target velocity](#Calculate-the-expected-target-velocity-for-ego-vehicle). +The travel times to the each points are calculated from [the expected target velocity](./run-out-design.md#Calculate-the-expected-target-velocity-for-ego-vehicle). ![brief](./docs/run_out/create_polygon_on_path_point.svg) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 878b19765252..2e729a032933 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -203,6 +203,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", lane_id_), current_time, &debug_marker_array); + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.detection_area_with_margin, "detection_area_with_margin", lane_id_), + current_time, &debug_marker_array); + appendMarkerArray( createPolygonMarkerArray(debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.0, 0.3, 0.7), current_time, &debug_marker_array); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index b46f18a139a7..5fd3a95e5c2f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -75,6 +75,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.intersection_velocity = node.declare_parameter(ns + ".intersection_velocity", 10.0 / 3.6); ip.intersection_max_acc = node.declare_parameter(ns + ".intersection_max_accel", 0.5); ip.detection_area_margin = node.declare_parameter(ns + ".detection_area_margin", 0.5); + ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5); + ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5); ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); ip.detection_area_angle_thr = node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 4395302d9092..afb6bcf27740 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -92,14 +92,20 @@ bool IntersectionModule::modifyPathVelocity( /* get detection area and conflicting area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; + std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); + planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, + &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, + logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLaneletsVec( detection_area_lanelets, planner_param_.detection_area_length); + std::vector detection_areas_with_margin = + util::getPolygon3dFromLaneletsVec( + detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = @@ -110,6 +116,7 @@ bool IntersectionModule::modifyPathVelocity( return true; } debug_data_.detection_area = detection_areas; + debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ int stop_line_idx = -1; @@ -153,8 +160,8 @@ bool IntersectionModule::modifyPathVelocity( const auto objects_ptr = planner_data_->predicted_objects; /* calculate dynamic collision around detection area */ - bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_areas, detection_area_lanelet_ids, objects_ptr, closest_idx); + bool has_collision = + checkCollision(lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx); bool is_stuck = checkStuckVehicleInIntersection( lanelet_map_ptr, *path, closest_idx, stop_line_idx, objects_ptr); bool is_entry_prohibited = (has_collision || is_stuck); @@ -226,7 +233,6 @@ void IntersectionModule::cutPredictPathWithDuration( bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::vector & detection_areas, const std::vector & detection_area_lanelet_ids, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx) @@ -256,24 +262,11 @@ bool IntersectionModule::checkCollision( continue; // TODO(Kenji Miyake): check direction? } - // keep vehicle in detection_area - const Point2d obj_point( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - for (const auto & detection_area : detection_areas) { - const auto detection_poly = lanelet::utils::to2D(detection_area).basicPolygon(); - const double dist_to_detection_area = - boost::geometry::distance(obj_point, toBoostPoly(detection_poly)); - if (dist_to_detection_area > planner_param_.detection_area_margin) { - // ignore the object far from detection area - continue; - } - // check direction of objects - const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); - if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { - target_objects.objects.push_back(object); - break; - } + // check direction of objects + const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); + if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + target_objects.objects.push_back(object); + break; } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 92f42ed7a290..9d8374d25c14 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -67,10 +67,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ std::vector detection_area_lanelets; std::vector conflicting_area_lanelets; + std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - &conflicting_area_lanelets, &detection_area_lanelets, logger_); + planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, + &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, + logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index b0a91cba575f..0a81d6b5d336 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -322,9 +322,11 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, + const int lane_id, const double detection_area_length, double right_margin, double left_margin, std::vector * conflicting_lanelets_result, - std::vector * objective_lanelets_result, const rclcpp::Logger logger) + std::vector * objective_lanelets_result, + std::vector * objective_lanelets_with_margin_result, + const rclcpp::Logger logger) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); @@ -365,6 +367,7 @@ bool getObjectiveLanelets( std::vector // conflicting lanes with "lane_id" conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes std::vector objective_lanelets; // final objective lanelets + std::vector objective_lanelets_with_margin; // final objective lanelets // exclude yield lanelets and ego lanelets from objective_lanelets for (const auto & conflicting_lanelet : conflicting_lanelets) { @@ -374,8 +377,11 @@ bool getObjectiveLanelets( if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { continue; } + const auto objective_lanelet_with_margin = + generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin); conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); objective_lanelets.push_back({conflicting_lanelet}); + objective_lanelets_with_margin.push_back({objective_lanelet_with_margin}); } // get possible lanelet path that reaches conflicting_lane longer than given length @@ -395,6 +401,7 @@ bool getObjectiveLanelets( *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; *objective_lanelets_result = objective_lanelets_sequences; + *objective_lanelets_with_margin_result = objective_lanelets_with_margin; std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os; for (const auto & l : conflicting_lanelets) { @@ -462,5 +469,32 @@ double calcArcLengthFromPath( return length; } +lanelet::ConstLanelet generateOffsetLanelet( + const lanelet::ConstLanelet lanelet, double right_margin, double left_margin) +{ + lanelet::Points3d lefts, rights; + + const double right_offset = right_margin; + const double left_offset = left_margin; + const auto offset_rightBound = lanelet::utils::getRightBoundWithOffset(lanelet, right_offset); + const auto offset_leftBound = lanelet::utils::getLeftBoundWithOffset(lanelet, left_offset); + + const auto original_left_bound = offset_leftBound; + const auto original_right_bound = offset_rightBound; + + for (const auto & pt : original_left_bound) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : original_right_bound) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto lanelet_with_margin = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto centerline = lanelet::utils::generateFineCenterline(lanelet_with_margin, 5.0); + lanelet_with_margin.setCenterline(centerline); + return std::move(lanelet_with_margin); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 71bb6ee12a29..1e11645d01bb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -55,7 +55,7 @@ bool RunOutModule::modifyPathVelocity( const auto current_acc = planner_data_->current_accel.get(); const auto & current_pose = planner_data_->current_pose.pose; - // smooth velocity of the path to calcute time to collision accurately + // smooth velocity of the path to calculate time to collision accurately PathWithLaneId smoothed_path; if (!smoothPath(*path, smoothed_path, planner_data_)) { return true; @@ -505,10 +505,15 @@ std::vector RunOutModule::createBoundingBoxForRangedP tier4_autoware_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); geometry_msgs::msg::Pose p_min_to_p_max; - const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( - pose_with_range.pose_min.position, pose_with_range.pose_max.position); - p_min_to_p_max.position = pose_with_range.pose_min.position; - p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + if (dist_p1_p2 < std::numeric_limits::epsilon()) { + // can't calculate the angle if two points are the same + p_min_to_p_max = pose_with_range.pose_min; + } else { + const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( + pose_with_range.pose_min.position, pose_with_range.pose_max.position); + p_min_to_p_max.position = pose_with_range.pose_min.position; + p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + } std::vector poly; poly.emplace_back( diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index 6be80eb381c9..627a814f89de 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -93,6 +93,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0); p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0); + p.use_initialization_stop_line_state = + node.declare_parameter(ns + ".use_initialization_stop_line_state", false); // debug p.show_stopline_collision_check = node.declare_parameter(ns + ".debug.show_stopline_collision_check", false); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index cc3ed5a3a030..a2bc53a617bc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -278,8 +278,7 @@ bool StopLineModule::modifyPathVelocity( } } else if (state_ == State::START) { // Initialize if vehicle is far from stop_line - constexpr bool use_initialization_after_start = false; - if (use_initialization_after_start) { + if (planner_param_.use_initialization_stop_line_state) { if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) { RCLCPP_INFO(logger_, "START -> APPROACH"); state_ = State::APPROACH; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 8b7c803a605a..9cb92e2815d7 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -117,6 +117,7 @@ class AstarSearch : public AbstractPlanningAlgorithm private: bool search(); + void clearNodes(); void setPath(const AstarNode & goal); bool setStartNode(); bool setGoalNode(); diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index d1b01dd3bbb5..f9dd17951520 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -135,15 +135,16 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) { AbstractPlanningAlgorithm::setMap(costmap); + clearNodes(); + const auto height = costmap_.info.height; const auto width = costmap_.info.width; // Initialize nodes - nodes_.clear(); nodes_.resize(height); - for (uint32_t i = 0; i < height; i++) { + for (size_t i = 0; i < height; i++) { nodes_[i].resize(width); - for (uint32_t j = 0; j < width; j++) { + for (size_t j = 0; j < width; j++) { nodes_[i][j].resize(planner_common_param_.theta_size); } } @@ -166,6 +167,14 @@ bool AstarSearch::makePlan( return search(); } +void AstarSearch::clearNodes() +{ + // clearing openlist is necessary because otherwise remaining elements of openlist + // point to deleted node. + nodes_.clear(); + openlist_ = std::priority_queue, NodeComparison>(); +} + bool AstarSearch::setStartNode() { const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size); diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index ffd8ab5d6d47..86abb73d9ae7 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -53,21 +53,21 @@ nav_msgs::msg::OccupancyGrid construct_cost_map( for (int i = 0; i < n_padding; i++) { // fill left - for (int j = width * i; j <= width * (i + 1); j++) { + for (int j = width * i; j < width * (i + 1); j++) { costmap_msg.data[j] = 100.0; } // fill right - for (int j = width * (height - n_padding + i); j <= width * (height - n_padding + i + 1); j++) { + for (int j = width * (height - n_padding + i); j < width * (height - n_padding + i + 1); j++) { costmap_msg.data[j] = 100.0; } } for (int i = 0; i < height; i++) { // fill bottom - for (int j = i * width; j <= i * width + n_padding; j++) { + for (int j = i * width; j < i * width + n_padding; j++) { costmap_msg.data[j] = 100.0; } - for (int j = (i + 1) * width - n_padding; j <= (i + 1) * width; j++) { + for (int j = (i + 1) * width - n_padding; j < (i + 1) * width; j++) { costmap_msg.data[j] = 100.0; } } diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index f176343c0d7f..fe4082027d6f 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -377,6 +377,9 @@ std::vector MPTOptimizer::getReferencePoints( traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points + tmp_ref_points_margin; ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); + if (ref_points.empty()) { + return std::vector{}; + } // set bounds information calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data_ptr); diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 11dee5effe28..0102265d534d 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1105,6 +1105,10 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( std::vector & traj_points, const CVMaps & cv_maps) { + if (traj_points.empty()) { + return; + } + stop_watch_.tic(__func__); const auto & map_info = cv_maps.map_info;