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

feat(obstacle_avoidance_planner): use polling subscriber #7213

Merged
merged 1 commit into from
Jun 2, 2024
Merged

feat(obstacle_avoidance_planner): use polling subscriber #7213

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
12 changes: 6 additions & 6 deletions planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include "obstacle_avoidance_planner/type_alias.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/ros/logger_level_configure.hpp"
#include "tier4_autoware_utils/ros/polling_subscriber.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"

#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
Expand Down Expand Up @@ -83,16 +84,14 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
TrajectoryParam traj_param_{};
EgoNearestParam ego_nearest_param_{};

// variables for subscribers
Odometry::ConstSharedPtr ego_state_ptr_;

// interface publisher
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;

// interface subscriber
rclcpp::Subscription<Path>::SharedPtr path_sub_;
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
tier4_autoware_utils::InterProcessPollingSubscriber<Odometry> ego_odom_sub_{
this, "~/input/odometry"};

// debug publisher
rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
Expand All @@ -113,8 +112,9 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
void resetPreviousData();

// main functions
bool isDataReady(const Path & path, rclcpp::Clock clock) const;
PlannerData createPlannerData(const Path & path) const;
bool checkInputPath(const Path & path, rclcpp::Clock clock) const;
PlannerData createPlannerData(
const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const;
std::vector<TrajectoryPoint> generateOptimizedTrajectory(const PlannerData & planner_data);
std::vector<TrajectoryPoint> extendTrajectory(
const std::vector<TrajectoryPoint> & traj_points,
Expand Down
30 changes: 16 additions & 14 deletions planning/obstacle_avoidance_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,6 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
// interface subscriber
path_sub_ = create_subscription<Path>(
"~/input/path", 1, std::bind(&ObstacleAvoidancePlanner::onPath, this, std::placeholders::_1));
odom_sub_ = create_subscription<Odometry>(
"~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; });

// debug publisher
debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
Expand Down Expand Up @@ -224,8 +222,16 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
time_keeper_ptr_->init();
time_keeper_ptr_->tic(__func__);

// check if data is ready and valid
if (!isDataReady(*path_ptr, *get_clock())) {
// check if input path is valid
if (!checkInputPath(*path_ptr, *get_clock())) {
return;
}

// check if ego's odometry is valid
const auto ego_odom_ptr = ego_odom_sub_.takeData();
if (!ego_odom_ptr) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(
get_logger(), *get_clock(), 5000, "Waiting for ego pose and twist.");
return;
}

Expand All @@ -245,7 +251,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
}

// 1. create planner data
const auto planner_data = createPlannerData(*path_ptr);
const auto planner_data = createPlannerData(*path_ptr, ego_odom_ptr);

// 2. generate optimized trajectory
const auto optimized_traj_points = generateOptimizedTrajectory(planner_data);
Expand Down Expand Up @@ -276,13 +282,8 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp);
}

bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const
bool ObstacleAvoidancePlanner::checkInputPath(const Path & path, rclcpp::Clock clock) const
{
if (!ego_state_ptr_) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist.");
return false;
}

if (path.points.size() < 2) {
RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Path points size is less than 1.");
return false;
Expand All @@ -297,16 +298,17 @@ bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock cloc
return true;
}

PlannerData ObstacleAvoidancePlanner::createPlannerData(const Path & path) const
PlannerData ObstacleAvoidancePlanner::createPlannerData(
const Path & path, const Odometry::ConstSharedPtr ego_odom_ptr) const
{
// create planner data
PlannerData planner_data;
planner_data.header = path.header;
planner_data.traj_points = trajectory_utils::convertToTrajectoryPoints(path.points);
planner_data.left_bound = path.left_bound;
planner_data.right_bound = path.right_bound;
planner_data.ego_pose = ego_state_ptr_->pose.pose;
planner_data.ego_vel = ego_state_ptr_->twist.twist.linear.x;
planner_data.ego_pose = ego_odom_ptr->pose.pose;
planner_data.ego_vel = ego_odom_ptr->twist.twist.linear.x;

debug_data_ptr_->ego_pose = planner_data.ego_pose;
return planner_data;
Expand Down
Loading