Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(obstacle_stop_planner): fix for multithread #231

Merged
merged 1 commit into from
Feb 28, 2023
Merged

fix(obstacle_stop_planner): fix for multithread #231

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 8 additions & 5 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -229,12 +229,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node
const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output,
PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header,
const VehicleInfo & vehicle_info, const StopParam & stop_param,
const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr);
const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr,
const PredictedObjects::ConstSharedPtr object_ptr,
const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr);

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<size_t, size_t> & index_map);
Expand Down Expand Up @@ -284,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,
Expand All @@ -299,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

Expand Down
47 changes: 23 additions & 24 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -570,12 +570,12 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
const auto stop_param = stop_param_;
const double current_acc = current_acc_;
const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_;
const auto object_ptr = object_ptr_;
const auto current_velocity_ptr = current_velocity_ptr_;
mutex_.unlock();

{
std::lock_guard<std::mutex> lock(mutex_);

if (!object_ptr_) {
if (!object_ptr) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"waiting for perception object...");
Expand All @@ -589,7 +589,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
return;
}

if (!current_velocity_ptr_ && node_param_.enable_slow_down) {
if (!current_velocity_ptr && node_param_.enable_slow_down) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(),
"waiting for current velocity...");
Expand All @@ -601,6 +601,8 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
}
}

const auto current_vel = current_velocity_ptr->twist.twist.linear.x;

PlannerData planner_data{};

getSelfPose(input_msg->header, tf_buffer_, planner_data.current_pose);
Expand All @@ -622,30 +624,32 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu
// search obstacles within slow-down/collision area
searchObstacle(
decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info,
stop_param, obstacle_ros_pointcloud_ptr);
stop_param, obstacle_ros_pointcloud_ptr, object_ptr, current_velocity_ptr);
// 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 = tier4_autoware_utils::convertToTrajectory(output_trajectory_points);
trajectory.header = input_msg->header;
path_pub_->publish(trajectory);
publishDebugData(planner_data, current_acc);
publishDebugData(planner_data, current_acc, current_vel);
}

void ObstacleStopPlannerNode::searchObstacle(
const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output,
PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header,
const VehicleInfo & vehicle_info, const StopParam & stop_param,
const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr)
const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr,
const PredictedObjects::ConstSharedPtr object_ptr,
const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr)
{
// search candidate obstacle pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
Expand Down Expand Up @@ -734,7 +738,7 @@ void ObstacleStopPlannerNode::searchObstacle(
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;
Expand All @@ -746,7 +750,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
Expand Down Expand Up @@ -787,7 +791,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_ &&
Expand Down Expand Up @@ -965,15 +970,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);

Expand All @@ -985,7 +984,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_;
Expand Down Expand Up @@ -1114,6 +1113,7 @@ void ObstacleStopPlannerNode::insertSlowDownSection(
void ObstacleStopPlannerNode::dynamicObjectCallback(
const PredictedObjects::ConstSharedPtr input_msg)
{
std::lock_guard<std::mutex> lock(mutex_);
object_ptr_ = input_msg;
}

Expand Down Expand Up @@ -1467,9 +1467,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_;
Expand All @@ -1493,9 +1493,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(
Expand Down