diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 7a49d3e5f82d3..731c73feeba4d 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -5,6 +5,7 @@ use_emergency_handling: false check_external_emergency_heartbeat: false use_start_request: false + enable_cmd_limit_filter: true external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index dc1d9ec1fedb8..3f6a07b6d5555 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -150,6 +150,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) moderate_stop_service_acceleration_ = declare_parameter("moderate_stop_service_acceleration"); stop_check_duration_ = declare_parameter("stop_check_duration"); + enable_cmd_limit_filter_ = declare_parameter("enable_cmd_limit_filter"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -411,9 +412,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_; } - // Apply limit filtering - filtered_commands.control = filterControlCommand(filtered_commands.control); - + // Check if command filtering option is enable + if (enable_cmd_limit_filter_) { + // Apply limit filtering + filtered_commands.control = filterControlCommand(filtered_commands.control); + } // tmp: Publish vehicle emergency status VehicleEmergencyStamped vehicle_cmd_emergency; vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 17bfe99d45251..b94240ce495af 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -164,6 +164,7 @@ class VehicleCmdGate : public rclcpp::Node double stop_hold_acceleration_; double emergency_acceleration_; double moderate_stop_service_acceleration_; + bool enable_cmd_limit_filter_; // Service rclcpp::Service::SharedPtr srv_engage_; diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 0a9b719939971..79e04e1d6fc61 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -294,6 +294,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 73d256a802340..86ede0d4be47d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -141,6 +141,7 @@ + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index bdcc7ac486cd0..748b5ee5becc0 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -18,6 +18,7 @@ This package includes the following features: - **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy. - **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored. - **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure). +- **Calculation of vertical correction amount from pitch** mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the "Calculate delta from pitch" figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the "Calculate delta from pitch" figure).

@@ -27,6 +28,10 @@ This package includes the following features:

+

+ +

+ ## Launch The `ekf_localizer` starts with the default parameters with the following command. diff --git a/localization/ekf_localizer/media/calculation_delta_from_pitch.png b/localization/ekf_localizer/media/calculation_delta_from_pitch.png new file mode 100644 index 0000000000000..0fa459f96dd71 Binary files /dev/null and b/localization/ekf_localizer/media/calculation_delta_from_pitch.png differ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 651eca1e9de12..0e46a26add852 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -446,7 +446,15 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); ekf_.updateWithDelay(y, C, R, delay_step); - updateSimple1DFilters(pose, params_.pose_smoothing_steps); + + // Considering change of z value due to measurement pose delay + const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y); + geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay; + pose_with_z_delay = pose; + pose_with_z_delay.pose.pose.position.z += dz_delay; + + updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); // debug const Eigen::MatrixXd X_result = ekf_.getLatestX(); diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index dcff2b33dcdb5..bdd793896062a 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -75,7 +75,7 @@ find_library(YAML_CPP_LIBRARY PATHS ${YAML_CPP_LIBRARY_DIRS}) link_directories(${YAML_CPP_LIBRARY_DIRS}) - +# cspell:ignore DHAVE, YAMLCPP if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif() diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 3c5846edf3ee9..39d05a7fb761b 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -31,7 +31,8 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 1.0 + object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker + object_recognition_collision_check_max_extra_stopping_margin: 1.0 # pull over pull_over: diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 008087ee32b4b..99dadc959f5a5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | | 1.0 | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance | ## **Goal Search** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 40f0dc0289cf0..2e8c9e57f8842 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -53,7 +53,8 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; Pose pull_out_start_pose{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f4a3a2bc5a88b..f87944641f59b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -68,6 +68,7 @@ struct GoalPlannerParameters // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; + double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params double pull_over_velocity; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index edfec6a7db579..8606f2cd2397d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -101,6 +101,15 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index d27d9ebbdadec..be227f4f7e93e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -164,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject( const PredictedObject & object, const PredictedPath & path, const double start_time, const double end_time, const double resolution); +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); + /** * @brief Check collision between ego path footprints and objects. * @return Has collision or not diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index b69e95875c5fb..8e6ab24929a00 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -101,15 +101,20 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & path_points, const PredictedObject & object) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_yaw = tf2::getYaw(obj_pose.orientation); + const double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); return std::make_pair( - obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); + obj_vel_norm * std::cos(obj_vel_yaw - path_yaw), + obj_vel_norm * std::sin(obj_vel_yaw - path_yaw)); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) @@ -365,7 +370,9 @@ void DynamicAvoidanceModule::updateTargetObjects() for (const auto & predicted_object : predicted_objects) { const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double obj_vel_norm = std::hypot( + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); // 1.a. check label @@ -393,7 +400,7 @@ void DynamicAvoidanceModule::updateTargetObjects() ? parameters_->min_overtaking_crossing_object_vel : parameters_->min_oncoming_crossing_object_vel; const bool is_crossing_object_to_ignore = - min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + min_crossing_object_vel < obj_vel_norm && is_obstacle_crossing_path; if (is_crossing_object_to_ignore) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 40998ae3bcd0d..f37149f706436 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -553,7 +553,9 @@ void GoalPlannerModule::selectSafePullOverPath() } // check if path is valid and safe - if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + if ( + !hasEnoughDistance(pull_over_path) || + checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { continue; } @@ -1178,9 +1180,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_->object_recognition_collision_check_margin)) { + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( + path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, + parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; } } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 07af2c866daa5..7df1749a0fe82 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -88,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); + p.object_recognition_collision_check_max_extra_stopping_margin = + node->declare_parameter( + ns + "object_recognition_collision_check_max_extra_stopping_margin"); } // pull over general params diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index baa9fd0a7e3f5..270e2064228af 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -336,7 +336,8 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra debug_msg.is_front = debug_data.is_front; debug_msg.relative_distance = debug_data.relative_to_ego; debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; + debug_msg.velocity = + std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); } lane_change_debug_msg_array_ = debug_msg_array; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 77c5e78d3db50..dad925c6b2335 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -677,7 +677,9 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { const auto & object = objects.objects.at(i); - const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto & obj_velocity_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); @@ -700,7 +702,7 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( } // ignore static object that are behind the ego vehicle - if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { + if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; } @@ -972,7 +974,16 @@ bool NormalLaneChange::getLaneChangePaths( const lanelet::BasicPoint2d lc_start_point( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.pose.position.y); - if (!boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d)) { + + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + if (!is_valid_start_point) { // lane changing points are not inside of the target preferred lanes or its neighbors continue; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c7541dc8af544..4dd2e7a13a94d 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -209,7 +209,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); @@ -314,7 +314,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); @@ -427,7 +427,7 @@ void StartPlannerModule::planWithPriority( // use current path if back is not needed if (status_.back_finished) { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -448,7 +448,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -503,7 +503,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe = false; + status_.is_safe_static_objects = false; status_.planner_type = PlannerType::NONE; } } @@ -713,7 +713,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe) { + if (!status_.back_finished || !status_.is_safe_static_objects) { return false; } @@ -800,7 +800,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1011,7 +1011,7 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.back_finished = true; return true; } @@ -1063,8 +1063,8 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index e6b946a130682..4cbfebef07a39 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -693,9 +693,9 @@ void fillObjectMovingTime( const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); - const auto & object_vel = - object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto is_faster_than_threshold = object_vel > object_parameter.moving_speed_threshold; + const auto & object_twist = object_data.object.kinematics.initial_twist_with_covariance.twist; + const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto is_faster_than_threshold = object_vel_norm > object_parameter.moving_speed_threshold; const auto id = object_data.object.object_id; const auto same_id_obj = std::find_if( @@ -1475,7 +1475,8 @@ ExtendedPredictedObject transform( extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; extended_object.shape = object.shape; - const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto & obj_velocity_norm = std::hypot( + extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); const auto & time_horizon = parameters->safety_check_time_horizon; const auto & time_resolution = parameters->safety_check_time_resolution; @@ -1490,7 +1491,7 @@ ExtendedPredictedObject transform( if (obj_pose) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity, obj_polygon); + t, *obj_pose, obj_velocity_norm, obj_polygon); } } } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 0e68862f12a5a..f3a1260eecadc 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -855,7 +855,9 @@ bool isParkedObject( using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; - if (object.initial_twist.twist.linear.x > static_object_velocity_threshold) { + const double object_vel_norm = + std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + if (object_vel_norm > static_object_velocity_threshold) { return false; } @@ -1034,7 +1036,9 @@ boost::optional getLeadingStaticObjectIdx( // ignore non-static object // TODO(shimizu): parametrize threshold - if (obj.initial_twist.twist.linear.x > 1.0) { + const double obj_vel_norm = + std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + if (obj_vel_norm > 1.0) { continue; } @@ -1095,9 +1099,9 @@ ExtendedPredictedObject transform( const auto & prepare_duration = common_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; - const auto & obj_vel = extended_object.initial_twist.twist.linear.x; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_velocity = extended_object.initial_twist.twist.linear.x; + const double obj_vel_norm = std::hypot( + extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { @@ -1109,14 +1113,14 @@ ExtendedPredictedObject transform( // create path for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); t += time_resolution) { - if (t < prepare_duration && obj_vel < velocity_threshold) { + if (t < prepare_duration && obj_vel_norm < velocity_threshold) { continue; } const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); if (obj_pose) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity, obj_polygon); + t, *obj_pose, obj_vel_norm, obj_polygon); } } } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index ee23ecc005142..34ba607938385 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -303,4 +303,28 @@ bool checkCollision( return true; } + +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double collision_check_margin, + const double max_extra_stopping_margin) +{ + for (const auto & p : ego_path.points) { + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + max_extra_stopping_margin); + + const auto ego_polygon = tier4_autoware_utils::toFootprint( + p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + + for (const auto & object : dynamic_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, ego_polygon); + if (distance < collision_check_margin) return true; + } + } + + return false; +} } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ce49163006d99..2b3ca11bbe750 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -54,4 +54,5 @@ intersection_to_occlusion: true merge_from_private: + stop_line_margin: 3.0 stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 75c282d117e97..6a2b7200abb2e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -266,7 +266,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = node.get_parameter("intersection.common.stop_line_margin").as_double(); + mp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7f56afde2f4ea..d9ebafbe682a0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -719,9 +719,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & current_pose = planner_data_->current_odometry->pose; + const auto lanelets_on_path = + planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); if (!intersection_lanelets_) { - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); intersection_lanelets_ = util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, planner_param_.common.attention_area_length, @@ -756,8 +756,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, pass_judge_line_idx] = intersection_stop_lines; - const auto ego_lane_with_next_lane = util::getEgoLaneWithNextLane( - *path, associative_ids_, planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = util::generatePathLanelets( + lanelets_on_path, *path, associative_ids_, closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); + if (!path_lanelets_opt.has_value()) { + RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); + return IntersectionModule::Indecisive{}; + } + const auto path_lanelets = path_lanelets_opt.value(); + + const auto ego_lane_with_next_lane = + path_lanelets.next.has_value() + ? std::vector< + lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} + : std::vector{path_lanelets.ego_or_entry2exit}; const bool stuck_detected = checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); @@ -846,8 +858,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getDuration()); const auto target_objects = filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + const bool has_collision = checkCollision( - *path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1026,8 +1039,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on) + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1042,12 +1055,10 @@ bool IntersectionModule::checkCollision( auto target_objects = objects; util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = ego_lane_with_next_lane.front(); - const double distance_until_intersection = - util::calcDistanceUntilIntersectionLanelet(ego_lane, path, closest_idx); - const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area @@ -1059,7 +1070,6 @@ bool IntersectionModule::checkCollision( : planner_param_.collision_detection.normal.collision_end_margin_time; bool collision_detected = false; for (const auto & object : target_objects.objects) { - bool has_collision = false; for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1068,97 +1078,82 @@ bool IntersectionModule::checkCollision( continue; } - std::vector predicted_poses; - for (const auto & pose : predicted_path.path) { - predicted_poses.push_back(pose); - } - has_collision = bg::intersects(ego_poly, to_bg2d(predicted_poses)); - if (has_collision) { - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); - }); - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); - }); - const double ref_object_enter_time = - static_cast(first_itr - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto start_time_distance_itr = time_distance_array.begin(); - if (ref_object_enter_time - collision_start_margin_time > 0) { - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - continue; - } - } - const double ref_object_exit_time = - static_cast(last_itr.base() - predicted_path.path.begin()) * - rclcpp::Duration(predicted_path.time_step).seconds(); - auto end_time_distance_itr = std::lower_bound( + // collision point + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + if (first_itr == predicted_path.path.cend()) continue; + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&ego_poly](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + if (last_itr == predicted_path.path.crend()) continue; + + // possible collision time interval + const double ref_object_enter_time = + static_cast(first_itr - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto start_time_distance_itr = time_distance_array.begin(); + if (ref_object_enter_time - collision_start_margin_time > 0) { + // start of possible ego position in the intersection + start_time_distance_itr = std::lower_bound( time_distance_array.begin(), time_distance_array.end(), - ref_object_exit_time + collision_end_margin_time, + ref_object_enter_time - collision_start_margin_time, [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - end_time_distance_itr = time_distance_array.end() - 1; - } - const double start_arc_length = std::max( - 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - - distance_until_intersection); - const double end_arc_length = std::max( - 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - - distance_until_intersection); - - long double lanes_length = 0.0; - std::vector ego_lane_with_next_lanes; - - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_odometry->pose); - for (const auto & lane : lanelets_on_path) { - lanes_length += bg::length(lane.centerline()); - ego_lane_with_next_lanes.push_back(lane); - if (lanes_length > start_arc_length && lanes_length < end_arc_length) { - break; - } - } - const auto trimmed_ego_polygon = - getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length); - - if (trimmed_ego_polygon.empty()) { + if (start_time_distance_itr == time_distance_array.end()) { + // ego is already at the exit of intersection when npc is at collision point even if npc + // accelerates so ego's position interval is empty continue; } + } + const double ref_object_exit_time = + static_cast(last_itr.base() - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto end_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + // ego is already passing the intersection, when npc is is at collision point + // so ego's position interval is up to the end of intersection lane + end_time_distance_itr = time_distance_array.end() - 1; + } + const double start_arc_length = std::max( + 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - + planner_data_->vehicle_info_.rear_overhang_m); + const double end_arc_length = std::min( + closest_arc_coords.length + (*end_time_distance_itr).second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + + const auto trimmed_ego_polygon = + getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - - polygon.outer().emplace_back(polygon.outer().front()); - - bg::correct(polygon); - - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - debug_data_.candidate_collision_object_polygons.emplace_back( - toGeomPoly(footprint_polygon)); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; break; } } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a427c4930b6d5..9d1fab1339d0e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -258,8 +258,8 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double time_delay, const bool tl_arrow_solid_on); + const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, + const bool tl_arrow_solid_on); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f0343891c6051..7fdae3df2d9d2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -872,29 +872,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width) -{ - // NOTE: findLaneIdsInterval returns (start, end) of associative_ids - const auto ego_lane_interval_opt = findLaneIdsInterval(path, associative_ids); - if (!ego_lane_interval_opt) { - return lanelet::ConstLanelets({}); - } - const auto [ego_start, ego_end] = ego_lane_interval_opt.value(); - if (ego_end < path.points.size() - 1) { - const int next_id = path.points.at(ego_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - return { - planning_utils::generatePathLanelet(path, ego_start, next_start + 1, width), - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width)}; - } - } - return {planning_utils::generatePathLanelet(path, ego_start, ego_end, width)}; -} - static bool isTargetStuckVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -1138,5 +1115,61 @@ void IntersectionLanelets::update( } } +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets prevs; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return prevs; + } + prevs.push_back(ll); + } + return prevs; +} + +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width) +{ + const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + return path_lanelets; +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 081ade30f8f7d..e125f485a65e8 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -126,10 +126,6 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -lanelet::ConstLanelets getEgoLaneWithNextLane( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const double width); - bool checkStuckVehicleInIntersection( const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, @@ -160,6 +156,11 @@ double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); +std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::set & associative_ids, const size_t closest_idx, const double width); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 55e6308d7e67b..4d2cf9695b2db 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -132,6 +132,18 @@ struct IntersectionStopLines size_t pass_judge_line; }; +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::Constlanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; +}; + } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg index 8acecba2f558d..a5601e4e0c503 100644 --- a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg +++ b/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg @@ -304,7 +304,7 @@
- colliision + collision
diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg index 55693e2a2688c..650194484e933 100644 --- a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg +++ b/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg @@ -220,14 +220,14 @@
- ---  safe velocity to stop before collisopn + ---  safe velocity to stop before collision
- ---- safe slow donw velocity + ---- safe slow down velocity
diff --git a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp index cfd8ea0f3e244..936476c800117 100644 --- a/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp +++ b/planning/obstacle_velocity_limiter/benchmarks/collision_checker_benchmark.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp index c33e283ed7c0c..aea169d6ce2e0 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/debug.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -42,8 +41,8 @@ visualization_msgs::msg::Marker makeLinestringMarker(const linestring_t & ls, co /// @param[in] marker_z z-value to use for markers /// @return marker array visualization_msgs::msg::MarkerArray makeDebugMarkers( - const Obstacles & obstacles, const std::vector & original_projections, - const std::vector & adjusted_projections, + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, const std::vector & original_footprints, const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, const Float marker_z); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp index ca80fc468079d..22d70463fc0d9 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/distance.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 51b5111b9d6ac..3f5f8cd814ea3 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -36,7 +35,7 @@ segment_t forwardSimulatedSegment( /// @param [in] origin origin of the projection /// @param [in] params parameters of the forward projection /// @return lines from the origin to its positions after forward projection, ordered left to right -multilinestring_t bicycleProjectionLines( +multi_linestring_t bicycleProjectionLines( const geometry_msgs::msg::Point & origin, const ProjectionParameters & params); /// @brief generate projection line using the bicycle model @@ -64,7 +63,7 @@ polygon_t generateFootprint(const linestring_t & linestring, const double latera /// @param [in] lines linestring from which to create the footprint /// @param [in] lateral_offset offset around the segment used to create the footprint /// @return footprint polygon -polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset); +polygon_t generateFootprint(const multi_linestring_t & lines, const double lateral_offset); /// @brief generate a footprint from a left, right, and middle linestrings and a lateral offset /// @param [in] left left linestring diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp index 32b69b149e9f2..94b7f2425a670 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/map_utils.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__MAP_UTILS_HPP_ #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -30,7 +29,7 @@ namespace obstacle_velocity_limiter /// @param[in] lanelet_map lanelet map /// @param[in] tags tags to identify obstacle linestrings /// @return the extracted obstacles -multilinestring_t extractStaticObstacles( +multi_linestring_t extractStaticObstacles( const lanelet::LaneletMap & lanelet_map, const std::vector & tags); /// @brief Determine if the given linestring is an obstacle diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 91611938119e5..4d89d4b440c21 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -63,7 +62,7 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] buffer buffer used to enlarge the mask /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects -multipolygon_t createPolygonMasks( +multi_polygon_t createPolygonMasks( const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel); @@ -74,7 +73,7 @@ multipolygon_t createPolygonMasks( /// @param[in] lateral_offset offset to create polygons around the lines /// @return polygon footprint of each projection lines std::vector createFootprintPolygons( - const std::vector & projected_linestrings, const Float lateral_offset); + const std::vector & projected_linestrings, const Float lateral_offset); /// @brief create the footprint polygon from a trajectory /// @param[in] trajectory the trajectory for which to create a footprint @@ -104,7 +103,7 @@ polygon_t createEnvelopePolygon(const std::vector & footprints); /// @param[in] trajectory input trajectory /// @param[in] params projection parameters /// @return projection lines for each trajectory point -std::vector createProjectedLines( +std::vector createProjectedLines( const Trajectory & trajectory, ProjectionParameters & params); /// @brief limit the velocity of the given trajectory @@ -116,7 +115,7 @@ std::vector createProjectedLines( /// @param[in] velocity_params velocity parameters void limitVelocity( Trajectory & trajectory, const CollisionChecker & collision_checker, - const std::vector & projections, const std::vector & footprints, + const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params); /// @brief copy the velocity profile of a downsampled trajectory to the original trajectory diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 13fac28291c3d..8916137c84077 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -18,7 +18,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -75,7 +74,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node OccupancyGrid::ConstSharedPtr occupancy_grid_ptr_; PointCloud::ConstSharedPtr pointcloud_ptr_; lanelet::LaneletMapPtr lanelet_map_ptr_{new lanelet::LaneletMap}; - multilinestring_t static_map_obstacles_; + multi_linestring_t static_map_obstacles_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; // parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 0d7edc649857d..8620c254968fe 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -41,7 +40,7 @@ namespace obstacle_velocity_limiter struct Obstacles { - multilinestring_t lines; + multi_linestring_t lines; multipoint_t points; }; @@ -69,11 +68,11 @@ struct ObstacleTree }; template <> -struct ObstacleTree +struct ObstacleTree { bgi::rtree> segments_rtree; - explicit ObstacleTree(const multilinestring_t & obstacles) + explicit ObstacleTree(const multi_linestring_t & obstacles) { auto segment_count = 0lu; for (const auto & line : obstacles) @@ -115,7 +114,7 @@ struct CollisionChecker { const Obstacles obstacles; std::unique_ptr> point_obstacle_tree_ptr; - std::unique_ptr> line_obstacle_tree_ptr; + std::unique_ptr> line_obstacle_tree_ptr; explicit CollisionChecker( Obstacles obs, const size_t rtree_min_points, const size_t rtree_min_segments) @@ -125,7 +124,7 @@ struct CollisionChecker for (const auto & line : obstacles.lines) if (!line.empty()) segment_count += line.size() - 1; if (segment_count > rtree_min_segments) - line_obstacle_tree_ptr = std::make_unique>(obstacles.lines); + line_obstacle_tree_ptr = std::make_unique>(obstacles.lines); if (obstacles.points.size() > rtree_min_points) point_obstacle_tree_ptr = std::make_unique>(obstacles.points); } @@ -163,7 +162,7 @@ polygon_t createObjectPolygon( /// @param [in] buffer buffer to add to the objects dimensions /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects -multipolygon_t createObjectPolygons( +multi_polygon_t createObjectPolygons( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp index 61f4f575f826b..b99407ae97846 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/occupancy_grid_utils.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -42,7 +41,7 @@ grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid); /// @param[in] occupancy_grid input occupancy grid /// @param[in] occupied_threshold threshold to use for identifying obstacles in the occupancy grid /// @return extracted obstacle linestrings -multilinestring_t extractObstacles( +multi_linestring_t extractObstacles( const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid); } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp index 7521e6b1a0bba..def328c8c9ae7 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/parameters.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__PARAMETERS_HPP_ #include -// cspell: ignore multipolygon, multilinestring #include #include @@ -40,8 +39,7 @@ struct ObstacleParameters static constexpr auto RTREE_SEGMENTS_PARAM = "obstacles.rtree_min_segments"; static constexpr auto RTREE_POINTS_PARAM = "obstacles.rtree_min_points"; - // cspell: ignore OCCUPANCYGRID - enum { POINTCLOUD, OCCUPANCYGRID, STATIC_ONLY } dynamic_source = OCCUPANCYGRID; + enum { POINTCLOUD, OCCUPANCY_GRID, STATIC_ONLY } dynamic_source = OCCUPANCY_GRID; int8_t occupancy_grid_threshold{}; Float dynamic_obstacles_buffer{}; Float dynamic_obstacles_min_vel{}; @@ -74,7 +72,7 @@ struct ObstacleParameters if (type == "pointcloud") { dynamic_source = POINTCLOUD; } else if (type == "occupancy_grid") { - dynamic_source = OCCUPANCYGRID; + dynamic_source = OCCUPANCY_GRID; } else if (type == "static_only") { dynamic_source = STATIC_ONLY; } else { @@ -113,7 +111,7 @@ struct ObstacleParameters struct ProjectionParameters { static constexpr auto MODEL_PARAM = "simulation.model"; - static constexpr auto NBPOINTS_PARAM = "simulation.nb_points"; + static constexpr auto NB_POINTS_PARAM = "simulation.nb_points"; static constexpr auto STEER_OFFSET_PARAM = "simulation.steering_offset"; static constexpr auto DISTANCE_METHOD_PARAM = "simulation.distance_method"; static constexpr auto DURATION_PARAM = "min_ttc"; @@ -135,7 +133,7 @@ struct ProjectionParameters { updateModel(node, node.declare_parameter(MODEL_PARAM)); updateDistanceMethod(node, node.declare_parameter(DISTANCE_METHOD_PARAM)); - updateNbPoints(node, node.declare_parameter(NBPOINTS_PARAM)); + updateNbPoints(node, node.declare_parameter(NB_POINTS_PARAM)); steering_angle_offset = node.declare_parameter(STEER_OFFSET_PARAM); duration = node.declare_parameter(DURATION_PARAM); } diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp index 80d79791d0bbc..9edb197ecba72 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/pointcloud_utils.hpp @@ -17,7 +17,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/ros/transform_listener.hpp" #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp index 2ee003206339d..258ef8f01b5f5 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/trajectory_preprocessing.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_VELOCITY_LIMITER__TRAJECTORY_PREPROCESSING_HPP_ #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index 81de7463b9e42..e44011e1f0ee3 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -23,7 +23,6 @@ #include #include -// cspell: ignore multipolygon, multilinestring namespace obstacle_velocity_limiter { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -36,15 +35,15 @@ using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); using point_t = tier4_autoware_utils::Point2d; using multipoint_t = tier4_autoware_utils::MultiPoint2d; using polygon_t = tier4_autoware_utils::Polygon2d; -using multipolygon_t = tier4_autoware_utils::MultiPolygon2d; +using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; using segment_t = tier4_autoware_utils::Segment2d; using linestring_t = tier4_autoware_utils::LineString2d; -using multilinestring_t = tier4_autoware_utils::MultiLineString2d; +using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; struct ObstacleMasks { - polygon_t positive_mask; // discard obstacles outside of this polygon - multipolygon_t negative_masks; // discard obstacles inside of these polygons + polygon_t positive_mask; // discard obstacles outside of this polygon + multi_polygon_t negative_masks; // discard obstacles inside of these polygons }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index d26f6133b0b54..c9255dae679c0 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -22,6 +22,7 @@ from rclpy.node import Node +# cspell: ignore axhline, relim class TrajectoryVisualizer(Node): def __init__(self): super().__init__("trajectory_visualizer") diff --git a/planning/obstacle_velocity_limiter/src/debug.cpp b/planning/obstacle_velocity_limiter/src/debug.cpp index 853dee2d20f49..051846cfeb8ca 100644 --- a/planning/obstacle_velocity_limiter/src/debug.cpp +++ b/planning/obstacle_velocity_limiter/src/debug.cpp @@ -16,7 +16,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -57,8 +56,8 @@ visualization_msgs::msg::Marker makePolygonMarker(const polygon_t & polygon, con } visualization_msgs::msg::MarkerArray makeDebugMarkers( - const Obstacles & obstacles, const std::vector & original_projections, - const std::vector & adjusted_projections, + const Obstacles & obstacles, const std::vector & original_projections, + const std::vector & adjusted_projections, const std::vector & original_footprints, const std::vector & adjusted_footprints, const ObstacleMasks & obstacle_masks, const Float marker_z) diff --git a/planning/obstacle_velocity_limiter/src/forward_projection.cpp b/planning/obstacle_velocity_limiter/src/forward_projection.cpp index d3193eefe0114..e90f17ae140e6 100644 --- a/planning/obstacle_velocity_limiter/src/forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/src/forward_projection.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -48,10 +47,10 @@ segment_t forwardSimulatedSegment( return segment_t{from, to}; } -multilinestring_t bicycleProjectionLines( +multi_linestring_t bicycleProjectionLines( const geometry_msgs::msg::Point & origin, const ProjectionParameters & params) { - multilinestring_t lines; + multi_linestring_t lines; if (params.steering_angle_offset == 0.0) lines.push_back(bicycleProjectionLine(origin, params, params.steering_angle)); else @@ -78,7 +77,7 @@ linestring_t bicycleProjectionLine( return line; } -polygon_t generateFootprint(const multilinestring_t & lines, const double lateral_offset) +polygon_t generateFootprint(const multi_linestring_t & lines, const double lateral_offset) { polygon_t footprint; if (lines.size() == 1) { @@ -98,7 +97,7 @@ polygon_t generateFootprint(const segment_t & segment, const double lateral_offs polygon_t generateFootprint(const linestring_t & linestring, const double lateral_offset) { namespace bg = boost::geometry; - multipolygon_t footprint; + multi_polygon_t footprint; namespace strategy = bg::strategy::buffer; bg::buffer( linestring, footprint, strategy::distance_symmetric(lateral_offset), diff --git a/planning/obstacle_velocity_limiter/src/map_utils.cpp b/planning/obstacle_velocity_limiter/src/map_utils.cpp index e84734355268e..d10244867833b 100644 --- a/planning/obstacle_velocity_limiter/src/map_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/map_utils.cpp @@ -17,7 +17,6 @@ #include "lanelet2_core/primitives/LineString.h" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -27,10 +26,10 @@ namespace obstacle_velocity_limiter { -multilinestring_t extractStaticObstacles( +multi_linestring_t extractStaticObstacles( const lanelet::LaneletMap & lanelet_map, const std::vector & tags) { - multilinestring_t lines; + multi_linestring_t lines; linestring_t line; linestring_t simplified_line; for (const auto & ls : lanelet_map.lineStringLayer) { diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index dbe7c87922564..ac1b4bde65211 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -34,7 +34,7 @@ Float calculateSafeVelocity( std::max(min_adjusted_velocity, static_cast(dist_to_collision / time_buffer))); } -multipolygon_t createPolygonMasks( +multi_polygon_t createPolygonMasks( const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel) { @@ -42,7 +42,7 @@ multipolygon_t createPolygonMasks( } std::vector createFootprintPolygons( - const std::vector & projected_linestrings, const Float lateral_offset) + const std::vector & projected_linestrings, const Float lateral_offset) { std::vector footprints; footprints.reserve(projected_linestrings.size()); @@ -84,8 +84,8 @@ polygon_t createEnvelopePolygon( polygon_t createEnvelopePolygon(const std::vector & footprints) { - multipolygon_t unions; - multipolygon_t result; + multi_polygon_t unions; + multi_polygon_t result; for (const auto & footprint : footprints) { boost::geometry::union_(footprint, unions, result); unions = result; @@ -95,10 +95,10 @@ polygon_t createEnvelopePolygon(const std::vector & footprints) return unions.front(); } -std::vector createProjectedLines( +std::vector createProjectedLines( const Trajectory & trajectory, ProjectionParameters & params) { - std::vector projections; + std::vector projections; projections.reserve(trajectory.points.size()); for (const auto & point : trajectory.points) { params.update(point); @@ -114,7 +114,7 @@ std::vector createProjectedLines( void limitVelocity( Trajectory & trajectory, const CollisionChecker & collision_checker, - const std::vector & projections, const std::vector & footprints, + const std::vector & projections, const std::vector & footprints, ProjectionParameters & projection_params, const VelocityParameters & velocity_params) { Float time = 0.0; diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index a738fff7b0e5f..9ab609378621a 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -21,7 +21,6 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -151,7 +150,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParamete result.successful = false; result.reason = "Unknown forward projection model"; } - } else if (parameter.get_name() == ProjectionParameters::NBPOINTS_PARAM) { + } else if (parameter.get_name() == ProjectionParameters::NB_POINTS_PARAM) { if (!projection_params_.updateNbPoints(*this, static_cast(parameter.as_int()))) { result.successful = false; result.reason = "number of points for projections must be at least 2"; diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 7f6d1cc965319..603e573e68f17 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -58,11 +58,11 @@ polygon_t createObjectPolygon( return translate_obj_poly; } -multipolygon_t createObjectPolygons( +multi_polygon_t createObjectPolygons( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { - multipolygon_t polygons; + multi_polygon_t polygons; for (const auto & object : objects.objects) { const double obj_vel_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, @@ -80,8 +80,7 @@ void addSensorObstacles( const ObstacleMasks & masks, tier4_autoware_utils::TransformListener & transform_listener, const std::string & target_frame, const ObstacleParameters & obstacle_params) { - // cspell: ignore OCCUPANCYGRID - if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCYGRID) { + if (obstacle_params.dynamic_source == ObstacleParameters::OCCUPANCY_GRID) { auto grid_map = convertToGridMap(occupancy_grid); threshold(grid_map, obstacle_params.occupancy_grid_threshold); maskPolygons(grid_map, masks); diff --git a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp index 397e20d05ebe0..7fe3d88885c3a 100644 --- a/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/occupancy_grid_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include #include @@ -70,7 +69,7 @@ grid_map::GridMap convertToGridMap(const OccupancyGrid & occupancy_grid) return grid_map; } -multilinestring_t extractObstacles( +multi_linestring_t extractObstacles( const grid_map::GridMap & grid_map, const OccupancyGrid & occupancy_grid) { cv::Mat cv_image; @@ -79,7 +78,7 @@ multilinestring_t extractObstacles( cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), 2); std::vector> contours; cv::findContours(cv_image, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE); - multilinestring_t obstacles; + multi_linestring_t obstacles; const auto & info = occupancy_grid.info; for (const auto & contour : contours) { linestring_t line; diff --git a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp index 25339a49529d6..4f9ca6c5bc3e7 100644 --- a/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp +++ b/planning/obstacle_velocity_limiter/src/pointcloud_utils.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/pointcloud_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index 64693ef8ef249..e9cf8d9ad4dd6 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -15,7 +15,6 @@ #include "obstacle_velocity_limiter/distance.hpp" #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp index 6e35ec95c62e0..536c3e93e4fa7 100644 --- a/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp +++ b/planning/obstacle_velocity_limiter/test/test_forward_projection.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/forward_projection.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -149,10 +148,10 @@ TEST(TestForwardProjection, generateFootprintMultiLinestrings) { using obstacle_velocity_limiter::generateFootprint; using obstacle_velocity_limiter::linestring_t; - using obstacle_velocity_limiter::multilinestring_t; + using obstacle_velocity_limiter::multi_linestring_t; auto footprint = generateFootprint( - multilinestring_t{ + multi_linestring_t{ linestring_t{{0.0, 0.0}, {0.0, 1.0}}, linestring_t{{0.0, 0.0}, {0.8, 0.8}}, linestring_t{{0.0, 0.0}, {1.0, 0.0}}}, 0.5); diff --git a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp index f75e808828ca6..eaf4a29a3a282 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacles.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacles.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/obstacles.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -49,7 +48,7 @@ TEST(TestObstacles, ObstacleTreeLines) /* using obstacle_velocity_limiter::point_t; using obstacle_velocity_limiter::linestring_t; - const obstacle_velocity_limiter::multilinestring_t lines = { + const obstacle_velocity_limiter::multi_linestring_t lines = { {point_t{-0.5, -0.5}, point_t{0.5,0.5}}, {point_t{0, 0}, point_t{1,1}}, {point_t(2, 2), point_t(-2, 2)}, diff --git a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp index a6ff8543f6ad0..4b49c569ab9f6 100644 --- a/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp +++ b/planning/obstacle_velocity_limiter/test/test_occupancy_grid_utils.cpp @@ -14,7 +14,6 @@ #include "obstacle_velocity_limiter/occupancy_grid_utils.hpp" #include "obstacle_velocity_limiter/types.hpp" -// cspell: ignore multipolygon, multilinestring #include @@ -22,7 +21,7 @@ TEST(TestOccupancyGridUtils, extractObstacleLines) { - using obstacle_velocity_limiter::multipolygon_t; + using obstacle_velocity_limiter::multi_polygon_t; using obstacle_velocity_limiter::polygon_t; constexpr int8_t occupied_thr = 10; nav_msgs::msg::OccupancyGrid occupancy_grid; @@ -37,7 +36,7 @@ TEST(TestOccupancyGridUtils, extractObstacleLines) constexpr auto extractObstacles = []( const nav_msgs::msg::OccupancyGrid & occupancy_grid, - const multipolygon_t & negative_masks, + const multi_polygon_t & negative_masks, const polygon_t & positive_mask, const double thr) { obstacle_velocity_limiter::ObstacleMasks masks; masks.negative_masks = negative_masks; diff --git a/sensing/image_diagnostics/image/block_state_decision.svg b/sensing/image_diagnostics/image/block_state_decision.svg index 373dc7984b3a6..39c49ab325dda 100644 --- a/sensing/image_diagnostics/image/block_state_decision.svg +++ b/sensing/image_diagnostics/image/block_state_decision.svg @@ -1,5 +1,5 @@ - + @@ -30,8 +29,8 @@ - - + + @@ -165,7 +164,7 @@ blockage area ratio - + @@ -187,7 +186,7 @@ freq_average  <... - + @@ -434,7 +433,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/image/image_diagnostics_overview.svg b/sensing/image_diagnostics/image/image_diagnostics_overview.svg index ed72197c7118e..7634974501746 100644 --- a/sensing/image_diagnostics/image/image_diagnostics_overview.svg +++ b/sensing/image_diagnostics/image/image_diagnostics_overview.svg @@ -1,5 +1,5 @@ - + - - + + @@ -111,7 +110,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/image/image_status_decision.svg b/sensing/image_diagnostics/image/image_status_decision.svg index ec5ab743e7900..119b2dfe79e24 100644 --- a/sensing/image_diagnostics/image/image_status_decision.svg +++ b/sensing/image_diagnostics/image/image_status_decision.svg @@ -1,15 +1,14 @@ - + @@ -32,8 +31,8 @@ - - + + @@ -49,10 +48,10 @@ Dark blocks > N_dark_... - - - - + + + + @@ -71,10 +70,10 @@ backlight blocks... - - - - + + + + @@ -86,16 +85,16 @@
bloackage blocks > N_blockage_thresh
+ >blockage blocks > N_blockage_thresh
- bloackage blocks > N_b... + blockage blocks > N_bl... - + - + @@ -365,7 +364,7 @@ - + Text is not SVG - cannot display diff --git a/sensing/image_diagnostics/src/image_diagnostics_node.cpp b/sensing/image_diagnostics/src/image_diagnostics_node.cpp index bd5fbf791261e..3db9ab94f50fa 100644 --- a/sensing/image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/image_diagnostics/src/image_diagnostics_node.cpp @@ -42,10 +42,10 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) void ImageDiagNode::onImageDiagChecker(DiagnosticStatusWrapper & stat) { - stat.add("number dark blocks ", std::to_string(params_.num_of_regions_dark)); - stat.add("number blockaged blocks ", std::to_string(params_.num_of_regions_blockage)); - stat.add("number low visibility blocks ", std::to_string(params_.num_of_regions_low_visibility)); - stat.add("number backlight blocks ", std::to_string(params_.num_of_regions_backlight)); + stat.add("number dark regions ", std::to_string(params_.num_of_regions_dark)); + stat.add("number blockage regions ", std::to_string(params_.num_of_regions_blockage)); + stat.add("number low visibility regions ", std::to_string(params_.num_of_regions_low_visibility)); + stat.add("number backlight regions ", std::to_string(params_.num_of_regions_backlight)); auto level = DiagnosticStatusWrapper::OK; if (params_.diagnostic_status < 0) { diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index 7ff4371f48a9a..a3b5268815f24 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -49,7 +49,7 @@ For the example of actual usage of this node, please refer to the [preprocessor. ### How to tuning timeout_sec and input_offset -The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings. +The values in `timeout_sec` and `input_offset` are used in the timer_callback to control concatenation timings. - Assumptions - when the timer runs out, we concatenate the pointclouds in the buffer diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 09e24bedeb6b9..f30c02a97a6c0 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -88,6 +88,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; +// cspell:ignore Yoshi /** \brief @b PointCloudDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index b2e245c5cbf4f..6b1fc0f5bd976 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -26,14 +26,14 @@ def launch_setup(context, *args, **kwargs): ns = "pointcloud_preprocessor" pkg = "pointcloud_preprocessor" - separate_concatenate_node_and_timesync_node = LaunchConfiguration( - "separate_concatenate_node_and_timesync_node" + separate_concatenate_node_and_time_sync_node = LaunchConfiguration( + "separate_concatenate_node_and_time_sync_node" ).perform(context) - is_separate_concatenate_node_and_timesync_node = ( - separate_concatenate_node_and_timesync_node.lower() == "true" + is_separate_concatenate_node_and_time_sync_node = ( + separate_concatenate_node_and_time_sync_node.lower() == "true" ) - if not is_separate_concatenate_node_and_timesync_node: + if not is_separate_concatenate_node_and_time_sync_node: sync_and_concat_component = ComposableNode( package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", @@ -155,9 +155,9 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("output_points_raw", "/points_raw/cropbox/filtered") add_launch_arg("tf_output_frame", "base_link") add_launch_arg( - "separate_concatenate_node_and_timesync_node", + "separate_concatenate_node_and_time_sync_node", "true", - "Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", + "Set True to separate concatenate node and time_sync node. which will cause to larger memory usage.", ) return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)])