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.

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

update pub/sub interface #15

Merged
merged 1 commit into from
Jan 20, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -249,8 +249,7 @@ class AEB : public rclcpp::Node
explicit AEB(const rclcpp::NodeOptions & node_options);

// subscriber
agnocast::PollingSubscriber<PointCloud2> sub_point_cloud_{
this->get_node_topics_interface()->resolve_topic_name("~/input/pointcloud")};
agnocast::PollingSubscriber<PointCloud2> sub_point_cloud_{this, "~/input/pointcloud"};
autoware::universe_utils::InterProcessPollingSubscriber<VelocityReport> sub_velocity_{
this, "~/input/velocity"};
autoware::universe_utils::InterProcessPollingSubscriber<Imu> sub_imu_{this, "~/input/imu"};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,7 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
"input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1));

object_pub_ = agnocast::create_publisher<autoware_perception_msgs::msg::DetectedObjects>(
this->get_node_base_interface(),
this->get_node_topics_interface()->resolve_topic_name("output/object"), rclcpp::QoS{1});
// object_pub_ = this->create_publisher<autoware_perception_msgs::msg::DetectedObjects>(
// "output/object", rclcpp::QoS{1});
this, "output/object", rclcpp::QoS{1});

debug_publisher_ =
std::make_unique<autoware::universe_utils::DebugPublisher>(this, "object_lanelet_filter");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -220,12 +220,7 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1);

sub_objects_ = agnocast::create_subscription<DetectedObjects>(
node_.get_node_base_interface(),
node_.get_node_topics_interface()->resolve_topic_name(input_channels[i].input_topic),
rclcpp::QoS{1}, func);

// sub_objects_array_.at(i) = node_.create_subscription<DetectedObjects>(
// input_channels[i].input_topic, rclcpp::QoS{1}, func);
&node_, input_channels[i].input_topic, rclcpp::QoS{1}, func);
}

// Check if any spawn enabled input streams
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -252,10 +252,8 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
sync_ptr_->registerCallback(std::bind(
&OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2, this,
std::placeholders::_1, std::placeholders::_2));
pointcloud_pub_ = agnocast::create_publisher<PointCloud2>(
this->get_node_base_interface(),
this->get_node_topics_interface()->resolve_topic_name("~/output/pointcloud"),
rclcpp::SensorDataQoS());
pointcloud_pub_ =
agnocast::create_publisher<PointCloud2>(this, "~/output/pointcloud", rclcpp::SensorDataQoS());

/* Radius search 2d filter */
if (use_radius_search_2d_filter) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -201,9 +201,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
sub_objects_ = this->create_subscription<autoware_perception_msgs::msg::PredictedObjects>(
"~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1));
sub_points_ = agnocast::create_subscription<sensor_msgs::msg::PointCloud2>(
get_node_base_interface(),
this->get_node_topics_interface()->resolve_topic_name("~/input/points_no_ground"),
rclcpp::SensorDataQoS(), std::bind(&CostmapGenerator::onPoints, this, _1));
this, "~/input/points_no_ground", rclcpp::SensorDataQoS(),
std::bind(&CostmapGenerator::onPoints, this, _1));
sub_lanelet_bin_map_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
"~/input/vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&CostmapGenerator::onLaneletMapBin, this, _1));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node
sub_predicted_objects_{this, "~/input/dynamic_objects"};

agnocast::PollingSubscriber<sensor_msgs::msg::PointCloud2> sub_no_ground_pointcloud_{
this->get_node_topics_interface()->resolve_topic_name("~/input/no_ground_pointcloud")};
this, "~/input/no_ground_pointcloud"};

autoware::universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node
autoware_perception_msgs::msg::PredictedObjects>
sub_predicted_objects_{this, "~/input/dynamic_objects"};
agnocast::PollingSubscriber<sensor_msgs::msg::PointCloud2> sub_no_ground_pointcloud_{
this->get_node_topics_interface()->resolve_topic_name("~/input/no_ground_pointcloud")};
this, "~/input/no_ground_pointcloud"};
autoware::universe_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry>
sub_vehicle_odometry_{this, "~/input/vehicle_odometry"};
autoware::universe_utils::InterProcessPollingSubscriber<
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -162,15 +162,14 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
cloud_stdmap_tmp_ = cloud_stdmap_;

// CAN'T use auto type here.
std::function<void(const agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> msg)> cb = std::bind(
&PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this,
std::placeholders::_1, input_topics_[d]);
std::function<void(const agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> msg)> cb =
std::bind(
&PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this,
std::placeholders::_1, input_topics_[d]);

filters_[d].reset();
filters_[d] = agnocast::create_subscription<sensor_msgs::msg::PointCloud2>(
get_node_base_interface(),
this->get_node_topics_interface()->resolve_topic_name(input_topics_[d]),
rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb);
this, input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb);
}

if (input_twist_topic_type_ == "twist") {
Expand Down Expand Up @@ -544,7 +543,8 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new
}

void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
const agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> input_ptr, const std::string & topic_name)
const agnocast::ipc_shared_ptr<sensor_msgs::msg::PointCloud2> input_ptr,
const std::string & topic_name)
{
std::lock_guard<std::mutex> lock(mutex_);
sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
Expand Down
7 changes: 2 additions & 5 deletions sensing/pointcloud_preprocessor/src/filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,7 @@ pointcloud_preprocessor::Filter::Filter(
{
if (use_agnocast_publish_) {
pub_output_agnocast_ = agnocast::create_publisher<PointCloud2>(
this->get_node_base_interface(),
this->get_node_topics_interface()->resolve_topic_name("output"),
rclcpp::SensorDataQoS().keep_last(max_queue_size_));
this, "output", rclcpp::SensorDataQoS().keep_last(max_queue_size_));
} else {
pub_output_ = this->create_publisher<PointCloud2>(
"output", rclcpp::SensorDataQoS().keep_last(max_queue_size_));
Expand Down Expand Up @@ -181,8 +179,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name)
&Filter::input_indices_callback_agnocast, this, std::placeholders::_1,
PointIndicesConstPtr());
sub_input_agnocast_ = agnocast::create_subscription<PointCloud2>(
get_node_base_interface(), this->get_node_topics_interface()->resolve_topic_name("input"),
rclcpp::SensorDataQoS().keep_last(max_queue_size_), cb);
this, "input", rclcpp::SensorDataQoS().keep_last(max_queue_size_), cb);
} else {
std::function<void(const PointCloud2ConstPtr msg)> cb =
std::bind(callback, this, std::placeholders::_1, PointIndicesConstPtr());
Expand Down