From a304b8cb2e22e57f4336282e5052714759cc2f71 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Mon, 3 Jun 2024 09:42:47 +0900 Subject: [PATCH 1/4] feat(motion_velocity_planner): use polling subscriber for odometry topic Signed-off-by: Maxime CLEMENT --- .../src/out_of_lane_module.cpp | 4 +-- .../planner_data.hpp | 4 +-- .../src/node.cpp | 35 ++++++------------- .../src/node.hpp | 7 ++-- 4 files changed, 19 insertions(+), 31 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 2cd050d15dd9b..0954d505e6164 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -154,11 +154,11 @@ VelocityPlanningResult OutOfLaneModule::plan( tier4_autoware_utils::StopWatch 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; 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_velocity.twist.linear.x; ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); stopwatch.tic("calculate_trajectory_footprints"); const auto current_ego_footprint = diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index add6b5ef392ea..8c5ec998dfb25 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -60,8 +60,8 @@ 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::PoseStamped current_odometry{}; + geometry_msgs::msg::TwistStamped current_velocity{}; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 4d152afa250b7..4cf7d3843d46e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -72,9 +72,6 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & "~/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( - "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), - create_subscription_options(this)); sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), create_subscription_options(this)); @@ -151,7 +148,8 @@ void MotionVelocityPlannerNode::on_unload_plugin( } // NOTE: argument planner_data must not be referenced for multithreading -bool MotionVelocityPlannerNode::is_data_ready() const +bool MotionVelocityPlannerNode::is_data_ready( + const nav_msgs::msg::Odometry::ConstSharedPtr ego_state_ptr) const { const auto & d = planner_data_; auto clock = *get_clock(); @@ -164,8 +162,7 @@ bool MotionVelocityPlannerNode::is_data_ready() const return true; }; - return check_with_msg(d.current_odometry, "Waiting for current odometry") && - check_with_msg(d.current_velocity, "Waiting for current velocity") && + return check_with_msg(ego_state_ptr, "Waiting for current odometry") && 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") && @@ -216,21 +213,6 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud( } } -void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - - auto current_odometry = std::make_shared(); - current_odometry->header = msg->header; - current_odometry->pose = msg->pose.pose; - planner_data_.current_odometry = current_odometry; - - auto current_velocity = std::make_shared(); - 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) { @@ -302,9 +284,14 @@ void MotionVelocityPlannerNode::on_trajectory( { std::unique_lock lk(mutex_); - if (!is_data_ready()) { + const auto ego_state_ptr = sub_vehicle_odometry_.takeData(); + if (!is_data_ready(ego_state_ptr)) { return; } + planner_data_.current_odometry.header = ego_state_ptr->header; + planner_data_.current_odometry.pose = ego_state_ptr->pose.pose; + planner_data_.current_velocity.header = ego_state_ptr->header; + planner_data_.current_velocity.twist = ego_state_ptr->twist.twist; if (input_trajectory_msg->points.empty()) { RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); @@ -367,8 +354,8 @@ 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 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 auto & external_v_limit = planner_data.external_velocity_limit; const auto & smoother = planner_data.velocity_smoother_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index 4b943dcbb67f2..b2481655580a4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -63,7 +64,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; - rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr @@ -77,7 +79,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node 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); void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void on_traffic_signals( @@ -118,7 +119,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - bool is_data_ready() const; + bool is_data_ready(const nav_msgs::msg::Odometry::ConstSharedPtr ego_state_ptr) const; void insert_stop( autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; From 3b285c343cea9dc8eaf8b5e39b2dedf5bd02d67d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 5 Jun 2024 15:56:03 +0900 Subject: [PATCH 2/4] use polling subscribers for more topics Signed-off-by: Maxime CLEMENT --- .../src/filter_predicted_objects.cpp | 4 +- .../src/out_of_lane_module.cpp | 4 +- .../planner_data.hpp | 16 +-- .../src/node.cpp | 132 +++++++----------- .../src/node.hpp | 41 +++--- 5 files changed, 82 insertions(+), 115 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index ce993dc1cedb0..77292a41b9cf1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -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; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 0954d505e6164..0fe7b783adf31 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -154,11 +154,11 @@ VelocityPlanningResult OutOfLaneModule::plan( tier4_autoware_utils::StopWatch 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 = diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index 8c5ec998dfb25..8a51a1d424f53 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -24,9 +24,8 @@ #include #include #include -#include -#include #include +#include #include #include #include @@ -60,12 +59,11 @@ struct PlannerData } // msgs from callbacks that are used for data-ready - geometry_msgs::msg::PoseStamped current_odometry{}; - geometry_msgs::msg::TwistStamped current_velocity{}; - geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; - autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; - pcl::PointCloud::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 no_ground_pointcloud{}; + nav_msgs::msg::OccupancyGrid occupancy_grid{}; std::shared_ptr route_handler; // nearest search @@ -78,7 +76,7 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional 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 velocity_smoother_{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 4cf7d3843d46e..d99a5489975e7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -63,35 +63,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), create_subscription_options(this)); - sub_predicted_objects_ = - this->create_subscription( - "~/input/dynamic_objects", 1, - std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), - create_subscription_options(this)); - sub_no_ground_pointcloud_ = this->create_subscription( - "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), - create_subscription_options(this)); - sub_acceleration_ = this->create_subscription( - "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), - create_subscription_options(this)); sub_lanelet_map_ = this->create_subscription( "~/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( - "~/input/traffic_signals", 1, - std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), - create_subscription_options(this)); - sub_virtual_traffic_light_states_ = - this->create_subscription( - "~/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( - "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), - create_subscription_options(this)); srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); @@ -148,45 +123,59 @@ void MotionVelocityPlannerNode::on_unload_plugin( } // NOTE: argument planner_data must not be referenced for multithreading -bool MotionVelocityPlannerNode::is_data_ready( - const nav_msgs::msg::Odometry::ConstSharedPtr ego_state_ptr) 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); return false; + is_ready = false; } return true; }; - return check_with_msg(ego_state_ptr, "Waiting for current odometry") && - 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 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 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> +MotionVelocityPlannerNode::process_no_ground_pointcloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { geometry_msgs::msg::TransformStamped transform; @@ -195,7 +184,7 @@ 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 pc; @@ -203,21 +192,8 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - if (!pc.empty()) { - tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); - } - - { - std::lock_guard lock(mutex_); - planner_data_.no_ground_pointcloud = pc_transformed; - } -} - -void MotionVelocityPlannerNode::on_acceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - std::lock_guard 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() @@ -235,7 +211,7 @@ 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 lock(mutex_); @@ -272,26 +248,14 @@ void MotionVelocityPlannerNode::on_traffic_signals( } } -void MotionVelocityPlannerNode::on_virtual_traffic_light_states( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) -{ - std::lock_guard 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 lk(mutex_); - const auto ego_state_ptr = sub_vehicle_odometry_.takeData(); - if (!is_data_ready(ego_state_ptr)) { + if (!update_planner_data()) { return; } - planner_data_.current_odometry.header = ego_state_ptr->header; - planner_data_.current_odometry.pose = ego_state_ptr->pose.pose; - planner_data_.current_velocity.header = ego_state_ptr->header; - planner_data_.current_velocity.twist = ego_state_ptr->twist.twist; if (input_trajectory_msg->points.empty()) { RCLCPP_WARN(get_logger(), "Input trajectory message is empty"); @@ -354,9 +318,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_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index b2481655580a4..799f80db25ad2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -61,31 +62,33 @@ class MotionVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr - sub_predicted_objects_; - rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::PredictedObjects> + sub_predicted_objects_{this, "~/input/dynamic_objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"}; tier4_autoware_utils::InterProcessPollingSubscriber sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; - rclcpp::Subscription::SharedPtr sub_acceleration_; + tier4_autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_acceleration_{this, "~/input/accel"}; + tier4_autoware_utils::InterProcessPollingSubscriber + 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::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr - sub_traffic_signals_; - rclcpp::Subscription::SharedPtr - sub_virtual_traffic_light_states_; - rclcpp::Subscription::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_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + std::optional> 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::SharedPtr trajectory_pub_; @@ -119,7 +122,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - bool is_data_ready(const nav_msgs::msg::Odometry::ConstSharedPtr ego_state_ptr) 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; From 09e57496f602b9c2aa5c11d4080b1c06e6161d19 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 4 Jun 2024 09:10:10 +0900 Subject: [PATCH 3/4] remove blocking mutex lock when processing traffic lights Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index d99a5489975e7..e929d0388e332 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -214,8 +214,6 @@ void MotionVelocityPlannerNode::on_lanelet_map( void MotionVelocityPlannerNode::process_traffic_signals( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { - std::lock_guard lock(mutex_); - // clear previous observation planner_data_.traffic_light_id_map_raw_.clear(); const auto traffic_light_id_map_last_observed_old = From 6cbbbc0b8dd5b8105ca90fc401e3c2f806087433 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 5 Jun 2024 16:02:23 +0900 Subject: [PATCH 4/4] fix assign after return Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index e929d0388e332..49036b3279399 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -131,8 +131,8 @@ bool MotionVelocityPlannerNode::update_planner_data() constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); - return false; is_ready = false; + return false; } return true; };