add no semantic data receiver

Author: Schmluk

hydra_ros/include/hydra_ros/input/image_receiver.h

@@ -182,6 +182,30 @@ void ImageReceiverImpl<SemanticT>::callback(
   queue.push(packet);
 }
 
+class NoSemanticImageReceiver : public RosDataReceiver {
+ public:
+  struct Config : RosDataReceiver::Config {};
+  using Policy =
+      message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,
+                                                      sensor_msgs::msg::Image>;
+  using Synchronizer = message_filters::Synchronizer<Policy>;
+
+  NoSemanticImageReceiver(const Config& config, const std::string& sensor_name);
+  virtual ~NoSemanticImageReceiver() = default;
+
+ protected:
+  bool initImpl() override;
+
+  void callback(const sensor_msgs::msg::Image::ConstSharedPtr& color,
+                const sensor_msgs::msg::Image::ConstSharedPtr& depth);
+
+  ColorSubscriber color_sub_;
+  DepthSubscriber depth_sub_;
+  std::unique_ptr<Synchronizer> sync_;
+};
+
+void declare_config(NoSemanticImageReceiver::Config& config);
+
 class ClosedSetImageReceiver : public ImageReceiverImpl<LabelSubscriber> {
  public:
   struct Config : RosDataReceiver::Config {};

hydra_ros/src/input/image_receiver.cpp

@@ -130,6 +130,39 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet)
   }
 }
 
+void declare_config(NoSemanticImageReceiver::Config& config) {
+  using namespace config;
+  name("NoSemanticImageReceiver::Config");
+  base<RosDataReceiver::Config>(config);
+}
+
+NoSemanticImageReceiver::NoSemanticImageReceiver(const Config& config,
+                                                 const std::string& sensor_name)
+    : RosDataReceiver(config, sensor_name) {}
+
+bool NoSemanticImageReceiver::initImpl() {
+  color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_));
+  depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_));
+  sync_.reset(new Synchronizer(
+      Policy(config.queue_size), color_sub_.getFilter(), depth_sub_.getFilter()));
+  sync_->registerCallback(&NoSemanticImageReceiver::callback, this);
+  return true;
+}
+
+void NoSemanticImageReceiver::callback(
+    const sensor_msgs::msg::Image::ConstSharedPtr& color,
+    const sensor_msgs::msg::Image::ConstSharedPtr& depth) {
+  const auto timestamp_ns = rclcpp::Time(color->header.stamp).nanoseconds();
+  if (!checkInputTimestamp(timestamp_ns)) {
+    return;
+  }
+
+  auto packet = std::make_shared<ImageInputPacket>(timestamp_ns, sensor_name_);
+  color_sub_.fillInput(*color, *packet);
+  depth_sub_.fillInput(*depth, *packet);
+  queue.push(packet);
+}
+
 void declare_config(ClosedSetImageReceiver::Config& config) {
   using namespace config;
   name("ClosedSetImageReceiver::Config");
@@ -152,6 +185,12 @@ OpenSetImageReceiver::OpenSetImageReceiver(const Config& config,
 
 namespace {
 
+static const auto no_semantic_registration =
+    config::RegistrationWithConfig<DataReceiver,
+                                   NoSemanticImageReceiver,
+                                   NoSemanticImageReceiver::Config,
+                                   std::string>("NoSemanticImageReceiver");
+
 static const auto closed_registration =
     config::RegistrationWithConfig<DataReceiver,
                                    ClosedSetImageReceiver,