Skip to content

Commit

Permalink
feat(motion_velocity_planner): use polling subscriber to efficiently …
Browse files Browse the repository at this point in the history 
…get messages (#7223)

* feat(motion_velocity_planner): use polling subscriber for odometry topic

Signed-off-by: Maxime CLEMENT <[email protected]>

* use polling subscribers for more topics

Signed-off-by: Maxime CLEMENT <[email protected]>

* remove blocking mutex lock when processing traffic lights

Signed-off-by: Maxime CLEMENT <[email protected]>

* fix assign after return

Signed-off-by: Maxime CLEMENT <[email protected]>

---------

Signed-off-by: Maxime CLEMENT <[email protected]>
  • Loading branch information
@maxime-clem @KhalilSelyan
maxime-clem authored and KhalilSelyan committed Jul 22, 2024
1 parent 02182e1 commit 566a60c
Show file tree
Hide file tree
Showing 5 changed files with 85 additions and 132 deletions.
4 changes: 2 additions & 2 deletions ...city_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,8 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
const PlannerParam & params)
{
autoware_perception_msgs::msg::PredictedObjects filtered_objects;
filtered_objects.header = planner_data->predicted_objects->header;
for (const auto & object : planner_data->predicted_objects->objects) {
filtered_objects.header = planner_data->predicted_objects.header;
for (const auto & object : planner_data->predicted_objects.objects) {
const auto is_pedestrian =
std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) {
return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
Expand Down
4 changes: 2 additions & 2 deletions ...n_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,11 +154,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
tier4_autoware_utils::StopWatch<std::chrono::microseconds> stopwatch;
stopwatch.tic();
out_of_lane::EgoData ego_data;
ego_data.pose = planner_data->current_odometry->pose;
ego_data.pose = planner_data->current_odometry.pose.pose;
ego_data.trajectory_points = ego_trajectory_points;
ego_data.first_trajectory_idx =
motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
ego_data.velocity = planner_data->current_velocity->twist.linear.x;
ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
stopwatch.tic("calculate_trajectory_footprints");
const auto current_ego_footprint =
Expand Down
16 changes: 7 additions & 9 deletions ..._velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,8 @@
#include <autoware_perception_msgs/msg/traffic_light_group.hpp>
#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/header.hpp>
#include <tier4_api_msgs/msg/crosswalk_status.hpp>
Expand Down Expand Up @@ -60,12 +59,11 @@ struct PlannerData
}

// msgs from callbacks that are used for data-ready
geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry;
geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity;
geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration;
autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
pcl::PointCloud<pcl::PointXYZ>::ConstPtr no_ground_pointcloud;
nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
nav_msgs::msg::Odometry current_odometry{};
geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{};
autoware_perception_msgs::msg::PredictedObjects predicted_objects{};
pcl::PointCloud<pcl::PointXYZ> no_ground_pointcloud{};
nav_msgs::msg::OccupancyGrid occupancy_grid{};
std::shared_ptr<route_handler::RouteHandler> route_handler;

// nearest search
Expand All @@ -78,7 +76,7 @@ struct PlannerData
std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
std::optional<tier4_planning_msgs::msg::VelocityLimit> external_velocity_limit;
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states;

// velocity smoother
std::shared_ptr<autoware::velocity_smoother::SmootherBase> velocity_smoother_{};
Expand Down
147 changes: 48 additions & 99 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,38 +63,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions &
sub_trajectory_ = this->create_subscription<autoware_planning_msgs::msg::Trajectory>(
"~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1),
create_subscription_options(this));
sub_predicted_objects_ =
this->create_subscription<autoware_perception_msgs::msg::PredictedObjects>(
"~/input/dynamic_objects", 1,
std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1),
create_subscription_options(this));
sub_no_ground_pointcloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(),
std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1),
create_subscription_options(this));
sub_vehicle_odometry_ = this->create_subscription<nav_msgs::msg::Odometry>(
"~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1),
create_subscription_options(this));
sub_acceleration_ = this->create_subscription<geometry_msgs::msg::AccelWithCovarianceStamped>(
"~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1),
create_subscription_options(this));
sub_lanelet_map_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"~/input/vector_map", rclcpp::QoS(10).transient_local(),
std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1),
create_subscription_options(this));
sub_traffic_signals_ =
this->create_subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>(
"~/input/traffic_signals", 1,
std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1),
create_subscription_options(this));
sub_virtual_traffic_light_states_ =
this->create_subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>(
"~/input/virtual_traffic_light_states", 1,
std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1),
create_subscription_options(this));
sub_occupancy_grid_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
"~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1),
create_subscription_options(this));

srv_load_plugin_ = create_service<LoadPlugin>(
"~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2));
Expand Down Expand Up @@ -151,45 +123,59 @@ void MotionVelocityPlannerNode::on_unload_plugin(
}

// NOTE: argument planner_data must not be referenced for multithreading
bool MotionVelocityPlannerNode::is_data_ready() const
bool MotionVelocityPlannerNode::update_planner_data()
{
const auto & d = planner_data_;
auto clock = *get_clock();
const auto check_with_msg = [&](const auto ptr, const auto & msg) {
auto is_ready = true;
const auto check_with_log = [&](const auto ptr, const auto & log) {
constexpr auto throttle_duration = 3000; // [ms]
if (!ptr) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg);
RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log);
is_ready = false;
return false;
}
return true;
};

return check_with_msg(d.current_odometry, "Waiting for current odometry") &&
check_with_msg(d.current_velocity, "Waiting for current velocity") &&
check_with_msg(d.current_acceleration, "Waiting for current acceleration") &&
check_with_msg(d.predicted_objects, "Waiting for predicted objects") &&
check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") &&
check_with_msg(map_ptr_, "Waiting for the map") &&
check_with_msg(
d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") &&
check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid");
}
const auto ego_state_ptr = sub_vehicle_odometry_.takeData();
if (check_with_log(ego_state_ptr, "Waiting for current odometry"))
planner_data_.current_odometry = *ego_state_ptr;

void MotionVelocityPlannerNode::on_occupancy_grid(
const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
planner_data_.occupancy_grid = msg;
}
const auto ego_accel_ptr = sub_acceleration_.takeData();
if (check_with_log(ego_accel_ptr, "Waiting for current acceleration"))
planner_data_.current_acceleration = *ego_accel_ptr;

void MotionVelocityPlannerNode::on_predicted_objects(
const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
planner_data_.predicted_objects = msg;
const auto predicted_objects_ptr = sub_predicted_objects_.takeData();
if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects"))
planner_data_.predicted_objects = *predicted_objects_ptr;

const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData();
if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) {
const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr);
if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud;
}

const auto occupancy_grid_ptr = sub_occupancy_grid_.takeData();
if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid"))
planner_data_.occupancy_grid = *occupancy_grid_ptr;

// here we use bitwise operator to not short-circuit the logging messages
is_ready &= check_with_log(map_ptr_, "Waiting for the map");
is_ready &= check_with_log(
planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother");

// optional data
const auto traffic_signals_ptr = sub_traffic_signals_.takeData();
if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr);
const auto virtual_traffic_light_states_ptr = sub_virtual_traffic_light_states_.takeData();
if (virtual_traffic_light_states_ptr)
planner_data_.virtual_traffic_light_states = *virtual_traffic_light_states_ptr;

return is_ready;
}

void MotionVelocityPlannerNode::on_no_ground_pointcloud(
std::optional<pcl::PointCloud<pcl::PointXYZ>>
MotionVelocityPlannerNode::process_no_ground_pointcloud(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg)
{
geometry_msgs::msg::TransformStamped transform;
Expand All @@ -198,44 +184,16 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud(
"map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
} catch (tf2::TransformException & e) {
RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what());
return;
return {};
}

pcl::PointCloud<pcl::PointXYZ> pc;
pcl::fromROSMsg(*msg, pc);

Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast<float>();
pcl::PointCloud<pcl::PointXYZ>::Ptr pc_transformed(new pcl::PointCloud<pcl::PointXYZ>);
if (!pc.empty()) {
tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine);
}

{
std::lock_guard<std::mutex> lock(mutex_);
planner_data_.no_ground_pointcloud = pc_transformed;
}
}

void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);

auto current_odometry = std::make_shared<geometry_msgs::msg::PoseStamped>();
current_odometry->header = msg->header;
current_odometry->pose = msg->pose.pose;
planner_data_.current_odometry = current_odometry;

auto current_velocity = std::make_shared<geometry_msgs::msg::TwistStamped>();
current_velocity->header = msg->header;
current_velocity->twist = msg->twist.twist;
planner_data_.current_velocity = current_velocity;
}

void MotionVelocityPlannerNode::on_acceleration(
const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
planner_data_.current_acceleration = msg;
if (!pc.empty()) tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine);
return *pc_transformed;
}

void MotionVelocityPlannerNode::set_velocity_smoother_params()
Expand All @@ -253,11 +211,9 @@ void MotionVelocityPlannerNode::on_lanelet_map(
has_received_map_ = true;
}

void MotionVelocityPlannerNode::on_traffic_signals(
void MotionVelocityPlannerNode::process_traffic_signals(
const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);

// clear previous observation
planner_data_.traffic_light_id_map_raw_.clear();
const auto traffic_light_id_map_last_observed_old =
Expand Down Expand Up @@ -290,19 +246,12 @@ void MotionVelocityPlannerNode::on_traffic_signals(
}
}

void MotionVelocityPlannerNode::on_virtual_traffic_light_states(
const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg)
{
std::lock_guard<std::mutex> lock(mutex_);
planner_data_.virtual_traffic_light_states = msg;
}

void MotionVelocityPlannerNode::on_trajectory(
const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg)
{
std::unique_lock<std::mutex> lk(mutex_);

if (!is_data_ready()) {
if (!update_planner_data()) {
return;
}

Expand Down Expand Up @@ -367,9 +316,9 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s
const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points,
const autoware::motion_velocity_planner::PlannerData & planner_data) const
{
const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose;
const double v0 = planner_data.current_velocity->twist.linear.x;
const double a0 = planner_data.current_acceleration->accel.accel.linear.x;
const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose;
const double v0 = planner_data.current_odometry.twist.twist.linear.x;
const double a0 = planner_data.current_acceleration.accel.accel.linear.x;
const auto & external_v_limit = planner_data.external_velocity_limit;
const auto & smoother = planner_data.velocity_smoother_;

Expand Down
46 changes: 26 additions & 20 deletions planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,12 @@
#include <autoware_motion_velocity_planner_node/srv/unload_plugin.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/logger_level_configure.hpp>
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
#include <tier4_autoware_utils/ros/published_time_publisher.hpp>

#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
Expand Down Expand Up @@ -60,31 +62,33 @@ class MotionVelocityPlannerNode : public rclcpp::Node

// subscriber
rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_trajectory_;
rclcpp::Subscription<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr
sub_predicted_objects_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_no_ground_pointcloud_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_vehicle_odometry_;
rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_acceleration_;
tier4_autoware_utils::InterProcessPollingSubscriber<
autoware_perception_msgs::msg::PredictedObjects>
sub_predicted_objects_{this, "~/input/dynamic_objects"};
tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"};
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<
geometry_msgs::msg::AccelWithCovarianceStamped>
sub_acceleration_{this, "~/input/accel"};
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::OccupancyGrid>
sub_occupancy_grid_{this, "~/input/occupancy_grid"};
tier4_autoware_utils::InterProcessPollingSubscriber<
autoware_perception_msgs::msg::TrafficLightGroupArray>
sub_traffic_signals_{this, "~/input/traffic_signals"};
tier4_autoware_utils::InterProcessPollingSubscriber<
tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>
sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"};
rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr sub_lanelet_map_;
rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>::SharedPtr
sub_traffic_signals_;
rclcpp::Subscription<tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr
sub_virtual_traffic_light_states_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_occupancy_grid_;

void on_trajectory(
const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg);
void on_predicted_objects(
const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg);
std::optional<pcl::PointCloud<pcl::PointXYZ>> process_no_ground_pointcloud(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg);
void on_traffic_signals(
void process_traffic_signals(
const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg);
void on_virtual_traffic_light_states(
const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg);
void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg);

// publishers
rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr trajectory_pub_;
Expand Down Expand Up @@ -118,7 +122,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node
std::mutex mutex_;

// function
bool is_data_ready() const;
/// @brief update the PlannerData instance with the latest messages received
/// @return false if some data is not available
bool update_planner_data();
void insert_stop(
autoware_planning_msgs::msg::Trajectory & trajectory,
const geometry_msgs::msg::Point & stop_point) const;
Expand Down

0 comments on commit 566a60c

Please sign in to comment.