Feature/instances

hydra_ros/include/hydra_ros/input/image_receiver.h

@@ -105,6 +105,22 @@ struct LabelSubscriber {
   std::shared_ptr<FilterSub<sensor_msgs::msg::Image>> impl_;
 };
 
+struct InstanceSubscriber {
+ public:
+  using MsgType = sensor_msgs::msg::Image;
+  using Filter = message_filters::SimpleFilter<MsgType>;
+
+  InstanceSubscriber();
+  explicit InstanceSubscriber(ianvs::NodeHandle nh, uint32_t queue_size = 1);
+  virtual ~InstanceSubscriber();
+
+  Filter& getFilter() const;
+  void fillInput(const sensor_msgs::msg::Image& img, ImageInputPacket& packet) const;
+
+ private:
+  std::shared_ptr<FilterSub<sensor_msgs::msg::Image>> impl_;
+};
+
 struct ColormappedLabelSubscriber {
  public:
   using MsgType = sensor_msgs::msg::Image;
@@ -235,6 +251,15 @@ class ClosedSetImageReceiver : public ImageReceiverImpl<LabelSubscriber> {
 
 void declare_config(ClosedSetImageReceiver::Config& config);
 
+class InstanceImageReceiver : public ImageReceiverImpl<InstanceSubscriber> {
+ public:
+  struct Config : RosDataReceiver::Config {};
+  InstanceImageReceiver(const Config& config, const std::string& sensor_name);
+  virtual ~InstanceImageReceiver() = default;
+};
+
+void declare_config(InstanceImageReceiver::Config& config);
+
 class OpenSetImageReceiver : public ImageReceiverImpl<FeatureSubscriber> {
  public:
   struct Config : RosDataReceiver::Config {};

hydra_ros/launch/hydra.launch.yaml

@@ -39,7 +39,7 @@ launch:
       args: >
         --config-utilities-file $(var input_config_path)
         --config-utilities-file $(var hydra_config_path)
-        --config-utilities-file $(var labelspace_path)
+        --config-utilities-file $(var labelspace_path)@labelspace
         --config-utilities-file $(find-pkg-share hydra_ros)/config/sinks/mesh_segmenter_sinks.yaml@frontend/objects
         --config-utilities-file $(find-pkg-share hydra_ros)/config/sinks/gvd_places_sinks.yaml@frontend/freespace_places
         --config-utilities-file $(find-pkg-share hydra_ros)/config/sinks/active_window_sinks.yaml@active_window

hydra_ros/src/input/image_receiver.cpp

@@ -122,6 +122,41 @@ void LabelSubscriber::fillInput(const Image& img, ImageInputPacket& packet) cons
   }
 }
 
+InstanceSubscriber::InstanceSubscriber() = default;
+
+InstanceSubscriber::InstanceSubscriber(ianvs::NodeHandle nh, uint32_t queue_size)
+    : impl_(std::make_shared<FilterSub<Image>>(nh, "semantic/image_raw", queue_size)) {}
+
+InstanceSubscriber::~InstanceSubscriber() = default;
+
+InstanceSubscriber::Filter& InstanceSubscriber::getFilter() const {
+  return *CHECK_NOTNULL(impl_);
+}
+
+void InstanceSubscriber::fillInput(const Image& img, ImageInputPacket& packet) const {
+  cv::Mat mat;
+  try {
+    mat = cv_bridge::toCvCopy(img)->image;
+  } catch (const cv_bridge::Exception& e) {
+    LOG(ERROR) << "Failed to convert label image: " << e.what();
+  }
+
+  if (mat.type() != CV_32SC1) {
+    LOG(ERROR) << "Invalid encoding for instance+label image";
+    return;
+  }
+
+  packet.labels = cv::Mat(mat.size(), CV_32SC1);
+  packet.instances = cv::Mat(mat.size(), CV_16SC1);
+  for (int r = 0; r < mat.rows; ++r) {
+    for (int c = 0; c < mat.cols; ++c) {
+      const auto original = mat.at<int32_t>(r, c);
+      packet.labels.at<int32_t>(r, c) = original >> 16;
+      packet.instances.at<int16_t>(r, c) = original & 0xFFFF;
+    }
+  }
+}
+
 ColormappedLabelSubscriber::ColormappedLabelSubscriber()
     : default_label_(-1), colormap_(nullptr) {}
 
@@ -244,6 +279,16 @@ void declare_config(ClosedSetImageReceiver::Config& config) {
   base<RosDataReceiver::Config>(config);
 }
 
+InstanceImageReceiver::InstanceImageReceiver(const Config& config,
+                                             const std::string& sensor_name)
+    : ImageReceiverImpl<InstanceSubscriber>(config, sensor_name) {}
+
+void declare_config(InstanceImageReceiver::Config& config) {
+  using namespace config;
+  name("InstanceImageReceiver::Config");
+  base<RosDataReceiver::Config>(config);
+}
+
 OpenSetImageReceiver::OpenSetImageReceiver(const Config& config,
                                            const std::string& sensor_name)
     : ImageReceiverImpl<FeatureSubscriber>(config, sensor_name) {}
@@ -292,6 +337,12 @@ static const auto closed_registration =
                                    ClosedSetImageReceiver::Config,
                                    std::string>("ClosedSetImageReceiver");
 
+static const auto instance_registration =
+    config::RegistrationWithConfig<DataReceiver,
+                                   InstanceImageReceiver,
+                                   InstanceImageReceiver::Config,
+                                   std::string>("InstanceImageReceiver");
+
 static const auto open_registration =
     config::RegistrationWithConfig<hydra::DataReceiver,
                                    OpenSetImageReceiver,