Skip to content

Commit

Permalink
fix(obstacle_stop_planner): lock object_ptr_, current_velocity_ptr_ (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#1330)

* fix(obstacle_stop_planner): lock object_ptr_, current_velocity_ptr_

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(obstacle_stop_planner): remove &

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(obstacle_stop_planner): current_vel as argument

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(obstacle_stop_planner): guard null object_ptr_

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota authored and yukke42 committed Oct 14, 2022
1 parent 2d260b6 commit bd141cc
Show file tree
Hide file tree
Showing 2 changed files with 34 additions and 23 deletions.
9 changes: 5 additions & 4 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 @@ -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<size_t, size_t> & index_map);
Expand Down Expand Up @@ -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,
Expand All @@ -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

Expand Down
48 changes: 29 additions & 19 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::mutex> 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(),
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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_ &&
Expand Down Expand Up @@ -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);

Expand All @@ -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_;
Expand Down Expand Up @@ -1137,6 +1146,8 @@ 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 @@ -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_;
Expand All @@ -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(
Expand Down

0 comments on commit bd141cc

Please sign in to comment.