diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 96c33ad813c21..0dcfaf8a36563 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -142,6 +142,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), ("~/output/path", "path_with_lane_id"), ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), @@ -329,6 +330,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/path_with_lane_id", "path_with_lane_id"), ("~/input/vector_map", "/map/vector_map"), ("~/input/vehicle_odometry", "/localization/kinematic_state"), + ("~/input/accel", "/localization/acceleration"), ("~/input/dynamic_objects", "/perception/object_recognition/objects"), ( "~/input/no_ground_pointcloud", diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 3a9c1e43d61d6..c4c7b3bab7597 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -8,6 +8,7 @@ + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 2b062c5742fd5..58df76bc78baa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -86,6 +86,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr route_subscriber_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; + rclcpp::Subscription::SharedPtr acceleration_subscriber_; rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; @@ -123,6 +124,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node // callback void onVelocity(const Odometry::ConstSharedPtr msg); + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 282f8a19f246d..89f329508516f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -34,6 +35,7 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; @@ -61,13 +63,14 @@ struct PlannerData { PoseStamped::ConstSharedPtr self_pose{}; Odometry::ConstSharedPtr self_odometry{}; + AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{}; PredictedObjects::ConstSharedPtr dynamic_object{}; OccupancyGrid::ConstSharedPtr occupancy_grid{}; PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; - BehaviorPathPlannerParameters parameters{}; lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; + BehaviorPathPlannerParameters parameters{}; Approval approval{}; }; diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index 22d5283b1b0e3..a7df8ae9bf428 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -10,6 +10,7 @@ + diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3be04884b6243..5fe8ea5c422ca 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -77,6 +77,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod velocity_subscriber_ = create_subscription( "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1), createSubscriptionOptions(this)); + acceleration_subscriber_ = create_subscription( + "~/input/accel", 1, std::bind(&BehaviorPathPlannerNode::onAcceleration, this, _1), + createSubscriptionOptions(this)); perception_subscriber_ = create_subscription( "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1), createSubscriptionOptions(this)); @@ -528,6 +531,10 @@ bool BehaviorPathPlannerNode::isDataReady() return missing("self_odometry"); } + if (!planner_data_->self_acceleration) { + return missing("self_acceleration"); + } + planner_data_->self_pose = self_pose_listener_.getCurrentPose(); if (!planner_data_->self_pose) { return missing("self_pose"); @@ -698,6 +705,11 @@ void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; } +void BehaviorPathPlannerNode::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_pd_); + planner_data_->self_acceleration = msg; +} void BehaviorPathPlannerNode::onPerception(const PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); 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 68e0e460dc4e3..40155ae90f4fb 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -58,6 +58,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; @@ -78,6 +79,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onVehicleVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); 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 fa736324c053e..9de2d422c1d2c 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 @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -70,7 +71,7 @@ struct PlannerData // msgs from callbacks that are used for data-ready geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; - boost::optional current_accel; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; std::deque velocity_buffer; autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; @@ -156,44 +157,6 @@ struct PlannerData return std::make_shared( external_traffic_light_id_map.at(id)); } - -private: - boost::optional prev_accel_; - geometry_msgs::msg::TwistStamped::ConstSharedPtr prev_velocity_; - double accel_lowpass_gain_; - - void updateCurrentAcc() - { - current_accel = {}; - - if (!current_velocity) { - return; - } - - if (!prev_velocity_) { - prev_velocity_ = current_velocity; - return; - } - - const double dv = current_velocity->twist.linear.x - prev_velocity_->twist.linear.x; - const double dt = std::max( - (rclcpp::Time(current_velocity->header.stamp) - rclcpp::Time(prev_velocity_->header.stamp)) - .seconds(), - 1e-03); - - const double accel = dv / dt; - prev_velocity_ = current_velocity; - - if (!prev_accel_) { - prev_accel_ = accel; - return; - } - // apply lowpass filter - current_accel = accel_lowpass_gain_ * accel + (1.0 - accel_lowpass_gain_) * prev_accel_.get(); - - prev_accel_ = current_accel; - } - friend BehaviorVelocityPlannerNode; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index 3f6474ffbf895..26624913d3edc 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -92,7 +92,7 @@ inline bool smoothPath( { 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 double a0 = planner_data->current_acceleration->accel.accel.linear.x; const auto & external_v_limit = planner_data->external_velocity_limit; const auto & smoother = planner_data->velocity_smoother_; diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index a5c81187575eb..adbf844ce0376 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -41,6 +41,7 @@ + diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 6d44723389831..3b8711543571e 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -113,6 +113,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onVehicleVelocity, this, _1), createSubscriptionOptions(this)); + sub_acceleration_ = this->create_subscription( + "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), + createSubscriptionOptions(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), @@ -159,8 +162,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Parameters forward_path_length_ = this->declare_parameter("forward_path_length", 1000.0); backward_path_length_ = this->declare_parameter("backward_path_length", 5.0); - // TODO(yukkysaito): This will become unnecessary when acc output from localization is available. - planner_data_.accel_lowpass_gain_ = this->declare_parameter("lowpass_gain", 0.5); planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0); // nearest search @@ -225,7 +226,7 @@ bool BehaviorVelocityPlannerNode::isDataReady( RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity"); return false; } - if (!d.current_accel) { + if (!d.current_acceleration) { RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration"); return false; } @@ -305,8 +306,6 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity( current_velocity->twist = msg->twist.twist; planner_data_.current_velocity = current_velocity; - planner_data_.updateCurrentAcc(); - // Add velocity to buffer planner_data_.velocity_buffer.push_front(*current_velocity); const rclcpp::Time now = this->now(); @@ -329,6 +328,13 @@ void BehaviorVelocityPlannerNode::onVehicleVelocity( } } +void BehaviorVelocityPlannerNode::onAcceleration( + const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + planner_data_.current_acceleration = msg; +} + void BehaviorVelocityPlannerNode::onParam() { planner_data_.velocity_smoother_ = 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 3542b82c1bd92..99ef42304ada4 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 @@ -470,7 +470,7 @@ float CrosswalkModule::calcTargetVelocity( return 0.0; } - const auto ego_acc = planner_data_->current_accel.get(); + const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; const auto dist_deceleration = calcSignedArcLength(ego_path.points, ego_pos, stop_point); const auto feasible_velocity = planning_utils::calcDecelerationVelocityFromDistanceToTarget( max_jerk, max_accel, ego_acc, ego_vel, dist_deceleration); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index c821d1044d72c..2acd007f7e759 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -123,7 +123,7 @@ bool generateStopLine( { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; - const double current_acc = planner_data->current_accel.get(); + const double current_acc = planner_data->current_acceleration->accel.accel.linear.x; const double max_acc = planner_data->max_stop_acceleration_threshold; const double max_jerk = planner_data->max_stop_jerk_threshold; const double delay_response_time = planner_data->delay_response_time; @@ -499,7 +499,7 @@ bool generateStopLineBeforeIntersection( { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; - const double current_acc = planner_data->current_accel.get(); + const double current_acc = planner_data->current_acceleration->accel.accel.linear.x; const double max_acc = planner_data->max_stop_acceleration_threshold; const double max_jerk = planner_data->max_stop_jerk_threshold; const double delay_response_time = planner_data->delay_response_time; diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 0472cc4aae720..8bae54a382edb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -368,16 +368,10 @@ bool NoStoppingAreaModule::isStoppable( { // get vehicle info and compute pass_judge_line_distance const auto current_velocity = planner_data_->current_velocity->twist.linear.x; - const auto current_acceleration = planner_data_->current_accel.get(); + const auto current_acceleration = planner_data_->current_acceleration->accel.accel.linear.x; const double max_acc = planner_data_->max_stop_acceleration_threshold; const double max_jerk = planner_data_->max_stop_jerk_threshold; const double delay_response_time = planner_data_->delay_response_time; - if (!planner_data_->current_accel) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 1000, - "[no stopping area] empty current acc! check current vel has been received."); - return false; - } const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time); const double signed_arc_length = 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 c9c062110c1d1..983cdcde488a9 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 @@ -91,7 +91,7 @@ bool OcclusionSpotModule::modifyPathVelocity( param_.v.max_stop_jerk = planner_data_->max_stop_jerk_threshold; param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold; param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; - param_.v.a_ego = planner_data_->current_accel.get(); + param_.v.a_ego = planner_data_->current_acceleration->accel.accel.linear.x; param_.v.delay_time = planner_data_->system_delay; param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 25cae0b46802c..0b57d1f20bad5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -52,7 +52,7 @@ bool RunOutModule::modifyPathVelocity( // set planner data const auto current_vel = planner_data_->current_velocity->twist.linear.x; - const auto current_acc = planner_data_->current_accel.get(); + const auto current_acc = planner_data_->current_acceleration->accel.accel.linear.x; const auto & current_pose = planner_data_->current_pose.pose; // set height of debug data @@ -129,7 +129,7 @@ Polygons2d RunOutModule::createDetectionAreaPolygon(const PathWithLaneId & smoot { // calculate distance needed to stop with jerk and acc constraints const float initial_vel = planner_data_->current_velocity->twist.linear.x; - const float initial_acc = planner_data_->current_accel.get(); + const float initial_acc = planner_data_->current_acceleration->accel.accel.linear.x; const float target_vel = 0.0; const float jerk_dec_max = planner_param_.smoother.start_jerk; const float jerk_dec = planner_param_.run_out.specify_decel_jerk diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index a232d097ec5fe..dfeac305d8402 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -332,17 +332,12 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const double reachable_distance = planner_data_->current_velocity->twist.linear.x * planner_param_.yellow_lamp_period; - if (!planner_data_->current_accel) { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 1000, - "[traffic_light] empty current acc! check current vel has been received."); - return false; - } // Calculate distance until ego vehicle decide not to stop, // taking into account the jerk and acceleration. const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithJerkLimit( - planner_data_->current_velocity->twist.linear.x, planner_data_->current_accel.get(), max_acc, - max_jerk, delay_response_time); + planner_data_->current_velocity->twist.linear.x, + planner_data_->current_acceleration->accel.accel.linear.x, max_acc, max_jerk, + delay_response_time); const bool distance_stoppable = pass_judge_line_distance < signed_arc_length; const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0;