diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 41da20f0522ad..aff97be9e2fbc 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -763,6 +763,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; // ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg); diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index 9fed29828ed9f..78127590685fb 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -13,7 +13,7 @@ - + diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index af8b29f55f340..77eee2c551846 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -38,7 +38,7 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * use_intensity_feature = node_->declare_parameter("use_intensity_feature", true); use_constant_feature = node_->declare_parameter("use_constant_feature", true); target_frame_ = node_->declare_parameter("target_frame", "base_link"); - z_offset_ = node_->declare_parameter("z_offset", 2); + z_offset_ = node_->declare_parameter("z_offset", -2.0); // load weight file std::ifstream fs(engine_file); diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 3bda53beb588a..e800ae9284139 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -160,7 +160,7 @@ class MapBasedPredictionNode : public rclcpp::Node const lanelet::BasicPoint2d & search_point); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; - static double getObjectYaw(const TrackedObject & object); + void updateObjectData(TrackedObject & object); void updateObjectsHistory( const std_msgs::msg::Header & header, const TrackedObject & object, 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 87a4d276512c0..65c448eb56218 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -17,6 +17,8 @@ #include #include +#include + #include #include @@ -385,6 +387,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt label == ObjectClassification::CAR || label == ObjectClassification::BUS || label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::TRUCK) { + // Update object yaw and velocity + updateObjectData(transformed_object); + // Get Closest Lanelet const auto current_lanelets = getCurrentLanelets(transformed_object); @@ -522,7 +527,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const auto entry_point = getCrosswalkEntryPoint(crossing_crosswalk.get()); if (hasPotentialToReach( - object, entry_point.first, prediction_time_horizon_, + object, entry_point.first, std::numeric_limits::max(), min_velocity_for_map_based_prediction_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, entry_point.first); @@ -531,7 +536,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } if (hasPotentialToReach( - object, entry_point.second, prediction_time_horizon_, + object, entry_point.second, std::numeric_limits::max(), min_velocity_for_map_based_prediction_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, entry_point.second); @@ -540,11 +545,16 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { - for (const auto & crosswalk : crosswalks_) { - const auto entry_point = getCrosswalkEntryPoint(crosswalk); + lanelet::ConstLanelet closest_crosswalk{}; + const auto & obj_pose = object.kinematics.pose_with_covariance.pose; + const auto found_closest_crosswalk = + lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk); + + if (found_closest_crosswalk) { + const auto entry_point = getCrosswalkEntryPoint(closest_crosswalk); if (hasPotentialToReach( - object, entry_point.first, prediction_time_horizon_, + object, entry_point.first, prediction_time_horizon_ * 2.0, min_velocity_for_map_based_prediction_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, entry_point.first); @@ -553,7 +563,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } if (hasPotentialToReach( - object, entry_point.second, prediction_time_horizon_, + object, entry_point.second, prediction_time_horizon_ * 2.0, min_velocity_for_map_based_prediction_)) { PredictedPath predicted_path = path_generator_->generatePathToTargetPoint(object, entry_point.second); @@ -605,17 +615,41 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( return predicted_object; } -double MapBasedPredictionNode::getObjectYaw(const TrackedObject & object) +void MapBasedPredictionNode::updateObjectData(TrackedObject & object) { - if (object.kinematics.orientation_availability) { - return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + return; } + // Compute yaw angle from the velocity and position of the object const auto & object_pose = object.kinematics.pose_with_covariance.pose; const auto & object_twist = object.kinematics.twist_with_covariance.twist; const auto future_object_pose = tier4_autoware_utils::calcOffsetPose( object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); - return tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); + + if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + const auto original_yaw = + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + // flip the angle + object.kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); + } else { + const auto updated_object_yaw = + tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); + + object.kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + } + + object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + } + + return; } void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time) @@ -726,7 +760,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( } // Step3. Calculate the angle difference between the lane angle and obstacle angle - const double object_yaw = getObjectYaw(object); + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle( lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_yaw - lane_yaw; @@ -753,7 +787,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood( const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; // compute yaw difference between the object and lane - const double obj_yaw = getObjectYaw(object); + const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); const double delta_yaw = obj_yaw - lane_yaw; const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw)); @@ -793,7 +827,7 @@ void MapBasedPredictionNode::updateObjectsHistory( single_object_data.current_lanelets = current_lanelets; single_object_data.future_possible_lanelets = current_lanelets; single_object_data.pose = object.kinematics.pose_with_covariance.pose; - const double object_yaw = getObjectYaw(object); + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); single_object_data.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(object_yaw); single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds()); single_object_data.twist = object.kinematics.twist_with_covariance.twist; 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 454bdb6753e36..4a4456ccac553 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 @@ -475,8 +475,6 @@ 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 - road_shoulder_safety_margin; const auto avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width; @@ -493,13 +491,16 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects( avoidance_debug_msg_array.push_back(avoidance_debug_msg); }; + const auto max_allowable_lateral_distance = + o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width; + avoidance_debug_msg.object_id = getUuidStr(o); avoidance_debug_msg.longitudinal_distance = o.longitudinal; avoidance_debug_msg.lateral_distance_from_centerline = o.lateral; avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance; avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance; - if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) { + if (max_allowable_lateral_distance <= avoid_margin) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN); continue; } diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg new file mode 100644 index 0000000000000..a5cc48a88bd42 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg @@ -0,0 +1,4 @@ + + + +
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a s...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection_fig.png b/planning/behavior_velocity_planner/docs/intersection/intersection_fig.png index 2b1b2bc979ece..552f5a362f0c3 100644 Binary files a/planning/behavior_velocity_planner/docs/intersection/intersection_fig.png and b/planning/behavior_velocity_planner/docs/intersection/intersection_fig.png differ diff --git a/planning/behavior_velocity_planner/intersection-design.md b/planning/behavior_velocity_planner/intersection-design.md index 02d54ef39798f..df8bd479f0c1c 100644 --- a/planning/behavior_velocity_planner/intersection-design.md +++ b/planning/behavior_velocity_planner/intersection-design.md @@ -7,7 +7,7 @@ This module is designed for rule-based intersection velocity decision that is ea In addition, the external users / modules (e.g. remote operation) to can intervene the STOP/GO decision for the vehicle behavior. The override interface is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development. -![brief](./docs/intersection/intersection.svg) +![brief](./docs/intersection/intersection.drawio.svg) ### Activation Timing diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 7a31b4358bb2a..01f479e0d9bec 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -229,11 +229,13 @@ PathPointWithLaneId getBackwardPointFromBasePoint( PathPointWithLaneId output; const double dx = p_to.point.pose.position.x - p_from.point.pose.position.x; const double dy = p_to.point.pose.position.y - p_from.point.pose.position.y; - const double norm = std::hypot(dx, dy); + const double dz = p_to.point.pose.position.z - p_from.point.pose.position.z; + const double norm = std::hypot(dx, dy, dz); output = p_base; output.point.pose.position.x += backward_length * dx / norm; output.point.pose.position.y += backward_length * dy / norm; + output.point.pose.position.z += backward_length * dz / norm; return output; } @@ -638,7 +640,12 @@ void CrosswalkModule::insertDecelPoint( setDistance(stop_point_distance); debug_data_.first_stop_pose = stop_point.second.point.pose; - debug_data_.stop_poses.push_back(stop_point.second.point.pose); + + if (std::abs(target_velocity) < 1e-3) { + debug_data_.stop_poses.push_back(stop_point.second.point.pose); + } else { + debug_data_.slow_poses.push_back(stop_point.second.point.pose); + } } float CrosswalkModule::calcTargetVelocity( 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 cfdd9c1d64624..d0ae2ce6e530a 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 @@ -272,7 +272,6 @@ bool IntersectionModule::checkCollision( 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/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index 0254b53fc7eb0..fab01c175cdbc 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -30,9 +30,11 @@ This function is used to approach near the obstacle or improve the accuracy of s #### Apply lateral acceleration limit It applies the velocity limit to decelerate at the curve. -It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. +It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`. The velocity limit is set as not to fall under `min_curve_velocity`. +Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed. + #### Resample trajectory It resamples the points on the reference trajectory with designated time interval. diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 27bcef0855e37..4b29bb821c4ac 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -157,7 +157,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const; - bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const; + bool smoothVelocity( + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const; std::tuple calcInitialMotion( const TrajectoryPoints & input_traj, const size_t input_closest, diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 0cbc7f3ed17d6..5a02e48afb91d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -75,7 +75,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase const TrajectoryPoints & input, const double v_current, const int closest_id) const override; boost::optional applyLateralAccelerationFilter( - const TrajectoryPoints & input) const override; + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, + [[maybe_unused]] const bool enable_smooth_limit) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 6650c418267c1..e34487b767b45 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -60,7 +60,9 @@ class SmootherBase const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0; virtual boost::optional applyLateralAccelerationFilter( - const TrajectoryPoints & input) const; + const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, + [[maybe_unused]] const double a0 = 0.0, + [[maybe_unused]] const bool enable_smooth_limit = false) const; double getMaxAccel() const; double getMinDecel() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index f7d77c5ceb4c0..b6b90491b14a9 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -85,6 +85,10 @@ boost::optional applyDecelFilterWithJerkConstraint( boost::optional> updateStateWithJerkConstraint( const double v0, const double a0, const std::map & jerk_profile, const double t); +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min); + } // namespace trajectory_utils } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index a9395e82668a1..4e8f16bbd8deb 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -478,7 +478,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } // Smoothing velocity - if (!smoothVelocity(*traj_extracted, output)) { + if (!smoothVelocity(*traj_extracted, *traj_extracted_closest, output)) { return prev_output_; } @@ -493,10 +493,18 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity( } bool MotionVelocitySmootherNode::smoothVelocity( - const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const + const TrajectoryPoints & input, const size_t input_closest, + TrajectoryPoints & traj_smoothed) const { + // Calculate initial motion for smoothing + double initial_vel{}; + double initial_acc{}; + InitializeType type{}; + std::tie(initial_vel, initial_acc, type) = calcInitialMotion(input, input_closest, prev_output_); + // Lateral acceleration limit - const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input); + const auto traj_lateral_acc_filtered = + smoother_->applyLateralAccelerationFilter(input, initial_vel, initial_acc, true); if (!traj_lateral_acc_filtered) { return false; } @@ -505,6 +513,11 @@ bool MotionVelocitySmootherNode::smoothVelocity( const auto traj_pre_resampled_closest = motion_utils::findNearestIndex( *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits::max(), node_param_.delta_yaw_threshold); + if (!traj_pre_resampled_closest) { + RCLCPP_WARN( + get_logger(), "Cannot find closest waypoint for traj_lateral_acc_filtered trajectory"); + return false; + } auto traj_resampled = smoother_->resampleTrajectory( *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x, *traj_pre_resampled_closest); @@ -512,6 +525,13 @@ bool MotionVelocitySmootherNode::smoothVelocity( RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization"); return false; } + const auto traj_resampled_closest = motion_utils::findNearestIndex( + *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), + node_param_.delta_yaw_threshold); + if (!traj_resampled_closest) { + RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); + return false; + } // Set 0[m/s] in the terminal point if (!traj_resampled->empty()) { @@ -521,20 +541,6 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Publish Closest Resample Trajectory Velocity publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_); - // Calculate initial motion for smoothing - double initial_vel{}; - double initial_acc{}; - InitializeType type{}; - const auto traj_resampled_closest = motion_utils::findNearestIndex( - *traj_resampled, current_pose_ptr_->pose, std::numeric_limits::max(), - node_param_.delta_yaw_threshold); - if (!traj_resampled_closest) { - RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory"); - return false; - } - std::tie(initial_vel, initial_acc, type) = - calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_); - // Clip trajectory from closest point TrajectoryPoints clipped; clipped.insert( diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index e862f699c47ef..56019abab8941 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -273,7 +273,8 @@ boost::optional AnalyticalJerkConstrainedSmoother::resampleTra } boost::optional AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( - const TrajectoryPoints & input) const + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { if (input.empty()) { return boost::none; diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 31c6420a67870..52aedeb9f7be3 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -59,7 +59,8 @@ double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } boost::optional SmootherBase::applyLateralAccelerationFilter( - const TrajectoryPoints & input) const + const TrajectoryPoints & input, [[maybe_unused]] const double v0, + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const { if (input.empty()) { return boost::none; @@ -102,6 +103,12 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( static_cast(std::round(base_param_.decel_distance_after_curve / points_interval)); const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel); + const auto latacc_min_vel_arr = + enable_smooth_limit + ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit( + *output, v0, a0, base_param_.min_jerk, base_param_.max_accel, base_param_.min_decel) + : std::vector{}; + for (size_t i = 0; i < output->size(); ++i) { double curvature = 0.0; const size_t start = i > after_decel_index ? i - after_decel_index : 0; @@ -111,6 +118,9 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( } double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5)); v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity); + if (enable_smooth_limit) { + v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i)); + } if (output->at(i).longitudinal_velocity_mps > v_curvature_max) { output->at(i).longitudinal_velocity_mps = v_curvature_max; } diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index c2b907eec29cb..0dfe4a0c8e6c7 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -639,5 +639,32 @@ boost::optional> updateStateWithJerkC return {}; } +std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( + const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk, + const double acc_max, const double acc_min) +{ + if (trajectory.empty()) return {}; + + std::vector velocities(trajectory.size()); + velocities.at(0) = v0; + auto curr_v = v0; + auto curr_a = a0; + + const auto intervals = calcTrajectoryIntervalDistance(trajectory); + + if (intervals.size() + 1 != trajectory.size()) { + throw std::logic_error("interval calculation result has unexpected array size."); + } + + for (size_t i = 0; i < intervals.size(); ++i) { + const auto t = intervals.at(i) / std::max(velocities.at(i), 1.0e-5); + curr_v = integ_v(curr_v, curr_a, jerk, t); + velocities.at(i + 1) = curr_v; + curr_a = std::clamp(integ_a(curr_a, jerk, t), acc_min, acc_max); + } + + return velocities; +} + } // namespace trajectory_utils } // namespace motion_velocity_smoother diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index e56f2a8604ed5..19a63964942de 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -236,7 +236,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node void insertVelocity( TrajectoryPoints & trajectory, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const StopParam & stop_param); + const double current_acc, const double current_vel, const StopParam & stop_param); TrajectoryPoints decimateTrajectory( const TrajectoryPoints & input, const double step_length, std::map & index_map); @@ -286,7 +286,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node SlowDownSection createSlowDownSection( const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, const double dist_remain, const double dist_vehicle_to_obstacle, - const VehicleInfo & vehicle_info, const double current_acc); + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel); SlowDownSection createSlowDownSectionFromMargin( const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, @@ -301,9 +301,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node void setExternalVelocityLimit(); - void resetExternalVelocityLimit(const double current_acc); + void resetExternalVelocityLimit(const double current_acc, const double current_vel); - void publishDebugData(const PlannerData & planner_data, const double current_acc); + void publishDebugData( + const PlannerData & planner_data, const double current_acc, const double current_vel); }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 5091df4bce7d4..d8f1de636a5fb 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -577,13 +577,21 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu // NOTE: these variables must not be referenced for multithreading const auto vehicle_info = vehicle_info_; const auto stop_param = stop_param_; - const double current_acc = current_acc_; const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; + const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; + const auto current_acc = current_acc_; mutex_.unlock(); { std::lock_guard lock(mutex_); + if (!object_ptr_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for dynamic objects..."); + return; + } + if (!obstacle_ros_pointcloud_ptr) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), @@ -636,17 +644,17 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu // insert slow-down-section/stop-point insertVelocity( output_trajectory_points, planner_data, input_msg->header, vehicle_info, current_acc, - stop_param); + current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > node_param_.hunting_threshold; if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { - resetExternalVelocityLimit(current_acc); + resetExternalVelocityLimit(current_acc, current_vel); } auto trajectory = motion_utils::convertToTrajectory(output_trajectory_points); - publishDebugData(planner_data, current_acc); + publishDebugData(planner_data, current_acc, current_vel); trajectory.header = input_msg->header; path_pub_->publish(trajectory); @@ -752,10 +760,16 @@ void ObstacleStopPlannerNode::searchObstacle( one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision); planner_data.stop_require = planner_data.found_collision_points; + + mutex_.lock(); + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; + mutex_.unlock(); + acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory, planner_data.decimate_trajectory_collision_index, planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr_, current_velocity_ptr_, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, &planner_data.stop_require, &output, trajectory_header); break; @@ -767,7 +781,7 @@ void ObstacleStopPlannerNode::searchObstacle( void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const StopParam & stop_param) + const double current_acc, const double current_vel, const StopParam & stop_param) { if (planner_data.stop_require) { // insert stop point @@ -808,7 +822,8 @@ void ObstacleStopPlannerNode::insertVelocity( dist_baselink_to_obstacle + index_with_dist_remain.get().second); const auto slow_down_section = createSlowDownSection( index_with_dist_remain.get().first, output, planner_data.lateral_deviation, - index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc); + index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, + current_vel); if ( !latest_slow_down_section_ && @@ -988,15 +1003,9 @@ StopPoint ObstacleStopPlannerNode::createTargetPoint( SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, const double dist_remain, const double dist_baselink_to_obstacle, - const VehicleInfo & vehicle_info, const double current_acc) + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel) { - if (!current_velocity_ptr_) { - // TODO(Satoshi Ota) - return SlowDownSection{}; - } - if (slow_down_param_.consider_constraints) { - const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; const auto margin_with_vel = calcFeasibleMarginAndVelocity( slow_down_param_, dist_baselink_to_obstacle + dist_remain, current_vel, current_acc); @@ -1008,7 +1017,7 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const auto no_need_velocity_limit = dist_baselink_to_obstacle + dist_remain > slow_down_param_.forward_margin; if (set_velocity_limit_ && no_need_velocity_limit) { - resetExternalVelocityLimit(current_acc); + resetExternalVelocityLimit(current_acc, current_vel); } const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; @@ -1137,6 +1146,8 @@ void ObstacleStopPlannerNode::insertSlowDownSection( void ObstacleStopPlannerNode::dynamicObjectCallback( const PredictedObjects::ConstSharedPtr input_msg) { + std::lock_guard lock(mutex_); + object_ptr_ = input_msg; } @@ -1490,9 +1501,9 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() slow_down_limit_vel->constraints.min_jerk, slow_down_limit_vel->constraints.min_acceleration); } -void ObstacleStopPlannerNode::resetExternalVelocityLimit(const double current_acc) +void ObstacleStopPlannerNode::resetExternalVelocityLimit( + const double current_acc, const double current_vel) { - const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; const auto reach_target_vel = current_vel < slow_down_param_.slow_down_vel + slow_down_param_.vel_threshold_reset_velocity_limit_; @@ -1516,9 +1527,8 @@ void ObstacleStopPlannerNode::resetExternalVelocityLimit(const double current_ac } void ObstacleStopPlannerNode::publishDebugData( - const PlannerData & planner_data, const double current_acc) + const PlannerData & planner_data, const double current_acc, const double current_vel) { - const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_VEL, current_vel); debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, current_acc); debug_ptr_->setDebugValues(