Skip to content

Commit 0d356a8

Browse files
authored
fix(planning): set single depth sensor data qos for pointlcoud polling subscribers (autowarefoundation#7490)
set single depth sensor data qos for pointlcoud polling subscribers Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
1 parent 0f0751e commit 0d356a8

File tree

5 files changed

+13
-11
lines changed
  • common/tier4_autoware_utils/include/tier4_autoware_utils/ros
  • control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking
  • planning
    • autoware_behavior_velocity_planner/src
    • autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker
    • motion_velocity_planner/autoware_motion_velocity_planner_node/src

5 files changed

+13
-11
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp

+7
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,13 @@
2626
namespace tier4_autoware_utils
2727
{
2828

29+
inline rclcpp::SensorDataQoS SingleDepthSensorQoS()
30+
{
31+
rclcpp::SensorDataQoS qos;
32+
qos.get_rmw_qos_profile().depth = 1;
33+
return qos;
34+
}
35+
2936
template <typename T, int N = 1, typename Enable = void>
3037
class InterProcessPollingSubscriber;
3138

control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp

+1-8
Original file line numberDiff line numberDiff line change
@@ -229,21 +229,14 @@ class CollisionDataKeeper
229229
rclcpp::Clock::SharedPtr clock_;
230230
};
231231

232-
static rclcpp::SensorDataQoS SingleDepthSensorQoS()
233-
{
234-
rclcpp::SensorDataQoS qos;
235-
qos.get_rmw_qos_profile().depth = 1;
236-
return qos;
237-
}
238-
239232
class AEB : public rclcpp::Node
240233
{
241234
public:
242235
explicit AEB(const rclcpp::NodeOptions & node_options);
243236

244237
// subscriber
245238
tier4_autoware_utils::InterProcessPollingSubscriber<PointCloud2> sub_point_cloud_{
246-
this, "~/input/pointcloud", SingleDepthSensorQoS()};
239+
this, "~/input/pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()};
247240
tier4_autoware_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
248241
this, "~/input/velocity"};
249242
tier4_autoware_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};

planning/autoware_behavior_velocity_planner/src/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
7474
sub_predicted_objects_{this, "~/input/dynamic_objects"};
7575

7676
tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
77-
sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"};
77+
sub_no_ground_pointcloud_{
78+
this, "~/input/no_ground_pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()};
7879

7980
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
8081
sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};

planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
114114
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odometry_{
115115
this, "~/input/odometry"};
116116
tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
117-
sub_pointcloud_{this, "~/input/pointcloud"};
117+
sub_pointcloud_{this, "~/input/pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()};
118118
tier4_autoware_utils::InterProcessPollingSubscriber<PredictedObjects> sub_dynamic_objects_{
119119
this, "~/input/objects"};
120120
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_stop_reason_;

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@ class MotionVelocityPlannerNode : public rclcpp::Node
6666
autoware_perception_msgs::msg::PredictedObjects>
6767
sub_predicted_objects_{this, "~/input/dynamic_objects"};
6868
tier4_autoware_utils::InterProcessPollingSubscriber<sensor_msgs::msg::PointCloud2>
69-
sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"};
69+
sub_no_ground_pointcloud_{
70+
this, "~/input/no_ground_pointcloud", tier4_autoware_utils::SingleDepthSensorQoS()};
7071
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
7172
sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};
7273
tier4_autoware_utils::InterProcessPollingSubscriber<

0 commit comments

Comments
 (0)