Feature/openset places (#33)

* add openset feature publisher to uhumans example

* add verbosity control and fix remap

Author: nathanhhughes

hydra_ros/config/datasets/uhumans2.yaml

@@ -7,3 +7,4 @@ input:
     left_cam:
       receiver: {type: ClosedSetImageReceiver}
       sensor: {type: camera_info, min_range: 0.1, max_range: 5.0, extrinsics: {type: ros, robot_frame: base_link_gt}}
+features: {type: FeatureReceiver}

hydra_ros/include/hydra_ros/input/feature_receiver.h

@@ -54,6 +54,8 @@ class FeatureReceiver : public Module {
     TFLookup::Config tf_lookup;
     //! Sensors to not subscribe to
     std::vector<std::string> sensors_to_exclude;
+    //! Verbosity for receiver
+    size_t verbosity = 0;
   } const config;
 
   explicit FeatureReceiver(const Config& config);

hydra_ros/launch/datasets/uhumans2.launch.yaml

@@ -2,23 +2,30 @@
 launch:
   - arg: {name: scene, default: office, description: uhumans2 scene name}
   - arg: {name: use_gt_semantics, default: 'true'}
+  - arg: {name: use_openset_places, default: 'false'}
   - arg: {name: labelspace, default: $(if $(var use_gt_semantics) uhumans2_$(var scene) ade20k_full)}
   - arg: {name: log_prefix, default: uhumans2_$(var scene)}
   - arg: {name: label_remap_path, default: $(find-pkg-share hydra)/config/label_remaps/uhumans2_$(var scene).yaml}
   - set_remap: {from: hydra/input/left_cam/depth_registered/image_rect, to: /tesse/depth_cam/mono/image_raw}
   - set_remap: {from: hydra/input/left_cam/rgb/image_raw, to: /tesse/left_cam/rgb/image_raw}
   - set_remap: {from: hydra/input/left_cam/rgb/camera_info, to: /tesse/left_cam/camera_info}
-  - set_remap: {from: hydra/input/left_cam/semantic/image_raw, to: /tesse/seg_cam/converted/image_raw, if: $(var use_gt_semantics)}
-  - set_remap: {from: hydra/input/left_cam/semantic/image_raw, to: semantic/image_raw, if: $(not $(var use_gt_semantics))}
+  - set_remap: {from: hydra/input/left_cam/semantic/image_raw, to: $(if $(var use_gt_semantics) /tesse/seg_cam/converted/image_raw /tesse/left_cam/semantic/image_raw)}
+  - set_remap: {from: hydra/input/left_cam/feature, to: /tesse/left_cam/semantic/feature}
   - let: {name: extra_yaml_gt, value: '{semantic_label_remap_filepath: $(var label_remap_path)}'}
   - let: {name: extra_yaml_no_gt, value: '{}'}
   - group:
-    - set_remap: {from: color/image_raw, to: /tesse/left_cam/rgb/image_raw}
-    - include:
-        if: $(not $(var use_gt_semantics))
-        file: $(find-pkg-share semantic_inference_ros)/launch/closed_set.launch.yaml
-        arg:
-          - {name: labelspace_name, value: $(var labelspace)}
+      - push_ros_namespace: {namespace: tesse/left_cam}
+      - set_remap: {from: color/image_raw, to: rgb/image_raw}
+      - include:
+          if: $(not $(var use_gt_semantics))
+          file: $(find-pkg-share semantic_inference_ros)/launch/closed_set.launch.yaml
+          arg:
+            - {name: labelspace_name, value: $(var labelspace)}
+      - include:
+          if: $(var use_openset_places)
+          file: $(find-pkg-share semantic_inference_ros)/launch/image_embedding_node.launch.yaml
+          arg:
+            - {name: min_period_s, value: '0.2'}
   - include:
       file: $(find-pkg-share hydra_ros)/launch/hydra.launch.yaml
       arg:

hydra_ros/launch/hydra.launch.yaml

@@ -54,4 +54,8 @@ launch:
   # hydra visualizer
   - set_remap: {from: hydra_visualizer/dsg, to: hydra/backend/dsg}
   - arg: {name: start_visualizer, default: 'true', description: run hydra visualizer}
-  - include: {file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml, if: $(var start_visualizer)}
+  - include:
+      file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml
+      if: $(var start_visualizer)
+      arg:
+        - {name: external_plugins_path, value: $(find-pkg-share hydra_ros)/config/hydra_ros_visualizer_plugins.yaml}

hydra_ros/src/input/feature_receiver.cpp

@@ -55,6 +55,7 @@ void declare_config(FeatureReceiver::Config& config) {
   field(config.queue_size, "queue_size");
   field(config.tf_lookup, "tf_lookup");
   field(config.sensors_to_exclude, "sensors_to_exclude");
+  field(config.verbosity, "verbosity");
 }
 
 using semantic_inference_msgs::msg::FeatureVectorStamped;
@@ -66,20 +67,23 @@ struct FeatureSubscriber {
   FeatureSubscriber(ianvs::NodeHandle nh,
                     const std::string& sensor_name,
                     const Callback& pose_callback,
-                    size_t queue_size = 10);
+                    size_t queue_size = 10,
+                    size_t verbosity = 0);
 
   void callback(const FeatureVectorStamped& msg);
 
   rclcpp::Subscription<FeatureVectorStamped>::SharedPtr sub;
   const std::string sensor_name;
   const Callback pose_callback;
+  const size_t verbosity;
 };
 
 FeatureSubscriber::FeatureSubscriber(ianvs::NodeHandle nh,
                                      const std::string& sensor_name,
                                      const Callback& pose_callback,
-                                     size_t queue_size)
-    : sensor_name(sensor_name), pose_callback(pose_callback) {
+                                     size_t queue_size,
+                                     size_t verbosity)
+    : sensor_name(sensor_name), pose_callback(pose_callback), verbosity(verbosity) {
   const std::string topic = sensor_name + "/feature";
   sub = nh.create_subscription<FeatureVectorStamped>(
       topic, queue_size, &FeatureSubscriber::callback, this);
@@ -105,6 +109,8 @@ void FeatureSubscriber::callback(const FeatureVectorStamped& msg) {
       Eigen::Map<const Eigen::VectorXf>(vec.data(), vec.size()),
       GlobalInfo::instance().getSensor(sensor_name).get());
 
+  LOG_IF(INFO, verbosity >= 2) << "Pushing new feature to input queue @ "
+                               << timestamp_ns << "[ns] for '" << sensor_name << "'";
   PipelineQueues::instance().input_features_queue.push(std::move(packet));
 }
 
@@ -114,15 +120,20 @@ void FeatureReceiver::start() {
   const auto sensor_names = GlobalInfo::instance().getAvailableSensors();
   for (const auto& name : sensor_names) {
     if (to_exclude.count(name)) {
+      LOG_IF(INFO, config.verbosity >= 1)
+          << "Excluding '" << name << "' from feature receivers!";
       continue;
     }
 
     auto nh = getHydraNodeHandle(config.ns);
+    LOG_IF(INFO, config.verbosity >= 1)
+        << "Making feature receiver for '" << name << "'";
     subs_.push_back(std::make_unique<FeatureSubscriber>(
         nh,
         name,
         [this](uint64_t timestamp_ns) { return lookup_.getBodyPose(timestamp_ns); },
-        config.queue_size));
+        config.queue_size,
+        config.verbosity));
   }
 }