Launching multiple instances of node, not showing up in ros2 node list
I am trying to replicate the ros1
nodelets for pcl
passthrough filters. I have written my own passthrough_filter
class utilizing similar parameters for clarity.
In my context I would like to run a passthrough filter on the x
and z
channels of a pointcloud in sequence: original -> x filter -> z filter -> result. Which I do with the Launch file at the end. In the passthrough_filter.cpp
I introduce the inputs and outputs as /filters_input
and /filters_output
such that I can dynamically remap it in the launch file.
The problem is, that running ros2 node list
only shows the passthrough_filter_x
and not the z
, the same is true of ros2 topic list
where I only see /velodyne_points/output_x
and not output_z
. That being said, I can open /velodyne_points
output_x
and output_z
in rviz
and see both. If I view output_z
then it shows up as a topic, though missing a publisher.
I've included both the launch file and cpp file below. Note, this is in ros2 foxy
EDIT I think this is happening because output_z
depends on output_x
existing. And launch files don't guarantee execution order. Everything works as expected if passthrough_x
is started from the above mentioned launch file and passthrough_z
is started from a different launch file. How could I combine these?
LAUNCH FILE:
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
# pointcloud filters
passthrough_x = Node(
package="pointcloud_filters",
executable="passthrough_filter_node",
name="passthrough_filter_x",
parameters=[
{"passthrough_filter/field_name": "x"},
{"passthrough_filter/limit_min": 5.0},
{"passthrough_filter/limit_max": 30.0},
{"passthrough_filter/limit_negative": False},
],
remappings=[
("/filters_input", "/velodyne_points/transformed"),
("/filters_output", "/velodyne_points/output_x"),
],
output="screen",
)
passthrough_z = Node(
package="pointcloud_filters",
executable="passthrough_filter_node",
name="passthrough_filter_z",
parameters=[
{"passthrough_filter/field_name": "z"},
{"passthrough_filter/limit_min": 0.5},
{"passthrough_filter/limit_max": 3.5},
{"passthrough_filter/limit_negative": False},
],
remappings=[
("/filters_input", "/velodyne_points/output_x"),
("/filters_output", "/velodyne_points/output_z"),
],
output="screen",
)
# defining other nodes
ld = LaunchDescription()
ld.add_action(passthrough_x)
ld.add_action(passthrough_z)
# launching other nodes
return ld
CPP FILE
#include "rclcpp/rclcpp.hpp"
#include "pcl_conversions/pcl_conversions.h"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "pcl/filters/passthrough.h"
#include "pcl/point_types.h"
class PassthroughFilter : public rclcpp::Node {
public:
PassthroughFilter() : Node("passthrough_filter") {
pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
"/filters_output", 1);
sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/filters_input", 1,
std::bind(&PassthroughFilter::cloud_callback, this,
std::placeholders::_1));
this->declare_parameter("passthrough_filter/field_name", "x");
this->declare_parameter("passthrough_filter/limit_min", 0.0);
this->declare_parameter("passthrough_filter/limit_max", 10.0);
this->declare_parameter("passthrough_filter/limit_negative", false);
std::string name =
this->get_parameter("passthrough_filter/field_name").as_string();
double limit_min =
this->get_parameter("passthrough_filter/limit_min").as_double();
double limit_max =
this->get_parameter("passthrough_filter/limit_max").as_double();
bool limit =
this->get_parameter("passthrough_filter/limit_negative").as_bool();
RCLCPP_INFO(this->get_logger(),
"Filter "
"pararms:\n\t field:\t%s\n\t min:\t%.1f\n\t max:\t%"
".1f\n\t negative:\t%s",
name.c_str(), limit_min, limit_max, limit ? "true" : "false");
pass_.setFilterFieldName(name);
pass_.setFilterLimits(limit_min, limit_max);
pass_.setFilterLimitsNegative(limit);
}
private:
void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(*msg, *cloud);
pass_.setInputCloud(cloud);
pass_.filter(*cloud_filtered);
sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(*cloud_filtered, output ...