Check proc_mask before enabling a topic

ouster-lidar · Aug 27, 2024 · f6fe276 · f6fe276
1 parent f329783
commit f6fe276
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 12 deletions.
diff --git a/launch/common.launch b/launch/common.launch
@@ -67,6 +67,7 @@
     <node pkg="nodelet" type="nodelet" name="img_node"
       output="screen" required="true"
       args="load ouster_ros/OusterImage os_nodelet_mgr $(arg _no_bond)">
+      <param name="~/proc_mask" value="$(arg proc_mask)"/>
     </node>
   </group>
 

diff --git 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
@@ -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
@@ -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) {