Skip to content

Commit

Permalink
fix(obstacle_stop_planner): fix for multithread
Browse files Browse the repository at this point in the history
  • Loading branch information
@h-ohta
h-ohta committed Jan 4, 2023
1 parent a816652 commit 0330459
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 29 deletions.
13 changes: 8 additions & 5 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
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
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

0 comments on commit 0330459

Please sign in to comment.