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

Jump to bottom

feat: use multi thread for obstacle stop #269

Merged
merged 4 commits into from
Mar 9, 2022
Merged

feat: use multi thread for obstacle stop #269

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5c13240
feat: use multi thread for obstacle stop
takayuki5168 Jan 19, 2022
465afaa
feat: minor change with comments
takayuki5168 Jan 20, 2022
cc41cba
fix: fix typo
takayuki5168 Jan 20, 2022
9c2844b
Merge branch 'tier4/proposal' into feature/multi-thread-obstacle-stop
takayuki5168 Mar 8, 2022
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
31 changes: 22 additions & 9 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 @@ -54,6 +54,7 @@

#include <map>
#include <memory>
#include <mutex>
#include <vector>

namespace motion_planning
Expand Down Expand Up @@ -197,6 +198,11 @@ class ObstacleStopPlannerNode : public rclcpp::Node
StopParam stop_param_;
SlowDownParam slow_down_param_;

// mutex for vehicle_info_, stop_param_, current_acc_, lpf_acc_, obstacle_ros_pointcloud_ptr_
// NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used
// (current_velocity_ptr_, prev_velocity_ptr_)
std::mutex mutex_;

/*
* Callback
*/
Expand All @@ -217,11 +223,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node

void searchObstacle(
const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output,
PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header);
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);

void insertVelocity(
TrajectoryPoints & trajectory, PlannerData & planner_data,
const std_msgs::msg::Header & trajectory_header);
const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
const double current_acc, const StopParam & stop_param);

TrajectoryPoints decimateTrajectory(
const TrajectoryPoints & input, const double step_length, std::map<size_t, size_t> & index_map);
Expand All @@ -233,12 +242,13 @@ class ObstacleStopPlannerNode : public rclcpp::Node
const TrajectoryPoints & trajectory,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_points_ptr,
pcl::PointCloud<pcl::PointXYZ>::Ptr output_points_ptr,
const std_msgs::msg::Header & trajectory_header);
const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
const StopParam & stop_param);

void createOneStepPolygon(
const geometry_msgs::msg::Pose & base_step_pose,
const geometry_msgs::msg::Pose & next_step_pose, std::vector<cv::Point2d> & polygon,
const double expand_width = 0.0);
const VehicleInfo & vehicle_info, const double expand_width = 0.0);

bool getSelfPose(
const std_msgs::msg::Header & header, const tf2_ros::Buffer & tf_buffer,
Expand All @@ -252,22 +262,25 @@ class ObstacleStopPlannerNode : public rclcpp::Node
const pcl::PointCloud<pcl::PointXYZ> & pointcloud, const geometry_msgs::msg::Pose & base_pose,
pcl::PointXYZ * lateral_nearest_point, double * deviation);

geometry_msgs::msg::Pose getVehicleCenterFromBase(const geometry_msgs::msg::Pose & base_pose);
geometry_msgs::msg::Pose getVehicleCenterFromBase(
const geometry_msgs::msg::Pose & base_pose, const VehicleInfo & vehicle_info);

void insertStopPoint(
const StopPoint & stop_point, TrajectoryPoints & output,
diagnostic_msgs::msg::DiagnosticStatus & stop_reason_diag);

StopPoint searchInsertPoint(
const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain);
const int idx, const TrajectoryPoints & base_trajectory, const double dist_remain,
const StopParam & stop_param);

StopPoint createTargetPoint(
const int idx, const double margin, const TrajectoryPoints & base_trajectory,
const double dist_remain);

SlowDownSection createSlowDownSection(
const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation,
const double dist_remain, const double dist_vehicle_to_obstacle);
const double dist_remain, const double dist_vehicle_to_obstacle,
const VehicleInfo & vehicle_info, const double current_acc);

SlowDownSection createSlowDownSectionFromMargin(
const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin,
Expand All @@ -282,9 +295,9 @@ class ObstacleStopPlannerNode : public rclcpp::Node

void setExternalVelocityLimit();

void resetExternalVelocityLimit();
void resetExternalVelocityLimit(const double current_acc);

void publishDebugData(const PlannerData & planner_data);
void publishDebugData(const PlannerData & planner_data, const double current_acc);
};
} // namespace motion_planning

Expand Down
Loading