diff --git a/launch/common.launch b/launch/common.launch index 292504b5..1ed5c7d7 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -67,6 +67,7 @@ + diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index f92558ad..92c7e02f 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -39,10 +39,18 @@ class OusterCloud : public nodelet::Nodelet { private: virtual void onInit() override { - create_imu_pub_sub(); - create_point_cloud_pubs(); - create_laser_scan_pubs(); - create_lidar_packets_sub(); + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"}); + auto tokens = impl::parse_tokens(proc_mask, '|'); + if (impl::check_token(tokens, "IMU")) + create_imu_pub_sub(); + if (impl::check_token(tokens, "PCL")) + create_point_cloud_pubs(); + if (impl::check_token(tokens, "SCAN")) + create_laser_scan_pubs(); + if (impl::check_token(tokens, "PCL") || + impl::check_token(tokens, "SCAN")) + create_lidar_packets_sub(); create_metadata_subscriber(); NODELET_INFO("OusterCloud: nodelet created!"); } diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 66ebb8a9..3eea44a1 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -40,15 +40,21 @@ class OusterDriver : public OusterSensor { protected: virtual void onInit() override { - create_imu_pub(); - create_point_cloud_pubs(); - create_laser_scan_pubs(); - create_image_pubs(); + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"}); + auto tokens = impl::parse_tokens(proc_mask, '|'); + if (impl::check_token(tokens, "IMU")) + create_imu_pub(); + if (impl::check_token(tokens, "PCL")) + create_point_cloud_pubs(); + if (impl::check_token(tokens, "SCAN")) + create_laser_scan_pubs(); + if (impl::check_token(tokens, "IMG")) + create_image_pubs(); OusterSensor::onInit(); } private: - virtual void on_metadata_updated(const sensor::sensor_info& info) override { OusterSensor::on_metadata_updated(info); // for OusterDriver we are going to always assume static broadcast diff --git a/src/os_image_nodelet.cpp b/src/os_image_nodelet.cpp index 2bfbbd44..ff8e8685 100644 --- a/src/os_image_nodelet.cpp +++ b/src/os_image_nodelet.cpp @@ -33,8 +33,13 @@ using ouster_ros::PacketMsg; class OusterImage : public nodelet::Nodelet { private: virtual void onInit() override { - create_lidar_packets_subscriber(); - create_image_publishers(); + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = pnh.param("proc_mask", std::string{"IMG"}); + auto tokens = impl::parse_tokens(proc_mask, '|'); + if (impl::check_token(tokens, "IMG")) { + create_lidar_packets_subscriber(); + create_image_publishers(); + } create_metadata_subscriber(); NODELET_INFO("OusterImage: node initialized!"); } @@ -71,7 +76,12 @@ class OusterImage : public nodelet::Nodelet { void metadata_handler(const std_msgs::String::ConstPtr& metadata_msg) { NODELET_INFO("OusterImage: retrieved new sensor metadata!"); auto info = sensor::parse_metadata(metadata_msg->data); - create_handlers(info); + + auto& pnh = getPrivateNodeHandle(); + auto proc_mask = pnh.param("proc_mask", std::string{"IMG"}); + auto tokens = impl::parse_tokens(proc_mask, '|'); + if (impl::check_token(tokens, "IMG")) + create_handlers(info); } void create_handlers(const sensor::sensor_info& info) {