From bb3bc197a6798e9d35fb34374319f674e04a1eee Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 8 Apr 2022 18:09:28 +0900 Subject: [PATCH] feat(behavior_velocity): add velocity smoother to behavior velocity (#497) * feat(behavior_velocity): add external velocity limit and velocity smoother to behavior velocity * feat(behavior_velocity): add smooth method Signed-off-by: tanaka3 * chore(behavior_velocity): apply experimental feedback Signed-off-by: taikitanaka3 Co-authored-by: Makoto Kurihara Co-authored-by: taikitanaka3 --- .../config/occlusion_spot.param.yaml | 2 +- .../behavior_velocity_planner/node.hpp | 5 + .../planner_data.hpp | 6 + .../include/utilization/trajectory_utils.hpp | 177 ++++++++++++++++++ .../behavior_velocity_planner/package.xml | 1 + .../behavior_velocity_planner/src/node.cpp | 33 +++- .../src/scene_module/occlusion_spot/debug.cpp | 2 +- .../occlusion_spot/scene_occlusion_spot.cpp | 5 + 8 files changed, 223 insertions(+), 8 deletions(-) create mode 100644 planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index a0b9e9d3243ea..98c72567709bd 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -2,7 +2,7 @@ ros__parameters: occlusion_spot: detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" - pass_judge: "current_velocity" # [-] candidate is "current_velocity" + pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not use_object_info: true # [-] whether to reflect object info to occupancy grid map or not use_partition_lanelet: true # [-] whether to use partition lanelet map data diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp index 103a26a07a245..6626bf0f18920 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -39,6 +40,7 @@ namespace behavior_velocity_planner { +using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node { public: @@ -68,6 +70,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; void onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); @@ -86,6 +89,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); + void onParam(); // publisher rclcpp::Publisher::SharedPtr path_pub_; diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index f682919038b6f..a182711c27604 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -17,6 +17,8 @@ #include "route_handler/route_handler.hpp" +#include +#include #include #include @@ -30,6 +32,7 @@ #include #include #include +#include #include #include @@ -81,8 +84,11 @@ struct PlannerData external_traffic_light_id_map; boost::optional external_crosswalk_status_input; boost::optional external_intersection_status_input; + boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // velocity smoother + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp new file mode 100644 index 0000000000000..89b955c47c346 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -0,0 +1,177 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define UTILIZATION__TRAJECTORY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +inline TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) +{ + TrajectoryPoints tps; + for (const auto & p : path.points) { + TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + tps.emplace_back(tp); + } + return tps; +} +inline PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) +{ + PathWithLaneId path; + for (const auto & p : trajectory) { + PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + path.points.emplace_back(pp); + } + return path; +} + +inline Quaternion lerpOrientation( + const Quaternion & o_from, const Quaternion & o_to, const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} + +/** + * @brief apply linear interpolation to trajectory point that is nearest to a certain point + * @param [in] points trajectory points + * @param [in] point Interpolated point is nearest to this point. + */ +template +TrajectoryPointWithIdx getLerpTrajectoryPointWithIdx( + const T & points, const geometry_msgs::msg::Point & point) +{ + TrajectoryPoint interpolated_point; + const size_t nearest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, point); + const double len_to_interpolated = + tier4_autoware_utils::calcLongitudinalOffsetToSegment(points, nearest_seg_idx, point); + const double len_segment = + tier4_autoware_utils::calcSignedArcLength(points, nearest_seg_idx, nearest_seg_idx + 1); + const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0); + { + const size_t i = nearest_seg_idx; + const auto & pos0 = points.at(i).pose.position; + const auto & pos1 = points.at(i + 1).pose.position; + interpolated_point.pose.position.x = interpolation::lerp(pos0.x, pos1.x, interpolate_ratio); + interpolated_point.pose.position.y = interpolation::lerp(pos0.y, pos1.y, interpolate_ratio); + interpolated_point.pose.position.z = interpolation::lerp(pos0.z, pos1.z, interpolate_ratio); + interpolated_point.pose.orientation = lerpOrientation( + points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio); + interpolated_point.longitudinal_velocity_mps = interpolation::lerp( + points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps, + interpolate_ratio); + interpolated_point.lateral_velocity_mps = interpolation::lerp( + points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio); + interpolated_point.acceleration_mps2 = interpolation::lerp( + points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio); + interpolated_point.heading_rate_rps = interpolation::lerp( + points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio); + } + return std::make_pair(interpolated_point, nearest_seg_idx); +} + +//! smooth path point with lane id starts from ego position on path to the path end +inline bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data) +{ + using tier4_autoware_utils::findNearestIndex; + const geometry_msgs::msg::Pose current_pose = planner_data->current_pose.pose; + const double v0 = planner_data->current_velocity->twist.linear.x; + const double a0 = planner_data->current_accel.get(); + const auto & external_v_limit = planner_data->external_velocity_limit; + const auto & smoother = planner_data->velocity_smoother_; + const double max = std::numeric_limits::max(); + + auto trajectory = convertPathToTrajectoryPoints(in_path); + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + 0, trajectory.size(), external_v_limit->max_velocity, trajectory); + } + const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + // calc ego internal division point on path + const auto traj_with_ego_point_with_idx = + getLerpTrajectoryPointWithIdx(*traj_lateral_acc_filtered, current_pose.position); + TrajectoryPoints traj_with_ego_point_on_path = *traj_lateral_acc_filtered; + TrajectoryPoint ego_point_on_path = traj_with_ego_point_with_idx.first; + const size_t nearest_seg_idx = traj_with_ego_point_with_idx.second; + //! insert ego projected pose on path so new nearest segment will be nearest_seg_idx + 1 + traj_with_ego_point_on_path.insert( + traj_with_ego_point_on_path.begin() + nearest_seg_idx, ego_point_on_path); + // Resample trajectory with ego-velocity based interval distances + auto traj_resampled = + smoother->resampleTrajectory(traj_with_ego_point_on_path, v0, nearest_seg_idx + 1); + const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4); + std::vector debug_trajectories; + // Clip trajectory from closest point + TrajectoryPoints clipped; + TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled->begin() + *traj_resampled_closest, traj_resampled->end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; + return false; + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled->begin(), + traj_resampled->begin() + *traj_resampled_closest); + out_path = convertTrajectoryPointsToPath(traj_smoothed); + return true; +} + +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index a69c2b04733a9..f6773c51f2b0d 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -22,6 +22,7 @@ lanelet2_extension libboost-dev libopencv-dev + motion_velocity_smoother nav_msgs nlohmann-json-dev pcl_conversions diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index d9c1e89800977..5d4d36bffa78a 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -15,6 +15,7 @@ #include "behavior_velocity_planner/node.hpp" #include +#include #include #include @@ -122,8 +123,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio sub_external_intersection_states_ = this->create_subscription( "~/input/external_intersection_states", 10, - std::bind(&BehaviorVelocityPlannerNode::onExternalIntersectionStates, this, _1), - createSubscriptionOptions(this)); + std::bind(&BehaviorVelocityPlannerNode::onExternalIntersectionStates, this, _1)); + sub_external_velocity_limit_ = this->create_subscription( + "~/input/external_velocity_limit_mps", 1, + std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); sub_external_traffic_signals_ = this->create_subscription( "~/input/external_traffic_signals", 10, @@ -138,6 +141,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); + // set velocity smoother param + onParam(); + // Publishers path_pub_ = this->create_publisher("~/output/path", 1); stop_reason_diag_pub_ = @@ -172,9 +178,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (this->declare_parameter("launch_virtual_traffic_light", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); } - if (this->declare_parameter("launch_occlusion_spot", true)) { - planner_manager_.launchSceneModule(std::make_shared(*this)); - } // this module requires all the stop line.Therefore this modules should be placed at the bottom. if (this->declare_parameter("launch_no_stopping_area", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); @@ -183,6 +186,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (this->declare_parameter("launch_stop_line", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); } + // to calculate ttc it's better to be after stop line + if (this->declare_parameter("launch_occlusion_spot", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } } // NOTE: argument planner_data must not be referenced for multithreading @@ -214,7 +221,9 @@ bool BehaviorVelocityPlannerNode::isDataReady(const PlannerData planner_data) co if (!d.route_handler_->isMapMsgReady()) { return false; } - + if (!d.velocity_smoother_) { + return false; + } return true; } @@ -286,6 +295,13 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity( } } +void BehaviorVelocityPlannerNode::onParam() +{ + planner_data_.velocity_smoother_ = + std::make_unique(*this); + return; +} + void BehaviorVelocityPlannerNode::onLaneletMap( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { @@ -322,6 +338,11 @@ void BehaviorVelocityPlannerNode::onExternalIntersectionStates( planner_data_.external_intersection_status_input = *msg; } +void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +{ + planner_data_.external_velocity_limit = *msg; +} + void BehaviorVelocityPlannerNode::onExternalTrafficSignals( const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 83d60cf3d350e..d212f6c1cd60d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -295,7 +295,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), current_time, &debug_marker_array); } - if (!debug_data_.close_partition.empty()) { + if (!debug_data_.close_partition.empty() && param_.is_show_occlusion) { appendMarkerArray( makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), current_time, &debug_marker_array); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 299d0e76da366..9aa70942fd7db 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -99,6 +100,10 @@ bool OcclusionSpotModule::modifyPathVelocity( if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { + if (!smoothPath(interp_path, predicted_path, planner_data_)) { + predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + // use current ego velocity in path if optimization failure + } } DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true)); if (!utils::buildDetectionAreaPolygon(