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(