Add Pointcloud2 Subscriber

.gitignore

@@ -1,2 +1,3 @@
 .cproject
 .project
+.vscode/

CMakeLists.txt

@@ -14,6 +14,11 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 
@@ -60,8 +65,14 @@ add_library(${PROJECT_NAME} SHARED
   src/jpeg_streamers.cpp
   src/png_streamers.cpp
   src/utils.cpp
+
+  src/subscribers/ros_subscriber.cpp
+  src/subscribers/pointcloud2_subscriber.cpp
+  src/subscribers/image_transport_subscriber.cpp
 )
 
+ament_target_dependencies(${PROJECT_NAME} rclcpp tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs)
+
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}
   async_web_server_cpp::async_web_server_cpp

README.md

@@ -15,6 +15,7 @@ This node provides HTTP streaming of ROS image topics in various formats, making
 - Web interface to browse available image topics
 - Single image snapshot capability
 - Support for different QoS profiles in ROS 2
+- Support for PointCloud2 topics (converted to depth images)
 
 ## Installation
 

include/web_video_server/image_streamer.hpp

@@ -37,12 +37,13 @@
 #include <opencv2/opencv.hpp>
 
 #include "rclcpp/rclcpp.hpp"
-#include "image_transport/image_transport.hpp"
-#include "image_transport/transport_hints.hpp"
-#include "web_video_server/utils.hpp"
+
 #include "async_web_server_cpp/http_server.hpp"
 #include "async_web_server_cpp/http_request.hpp"
 
+#include "web_video_server/subscribers/pointcloud2_subscriber.hpp"
+#include "web_video_server/subscribers/image_transport_subscriber.hpp"
+
 namespace web_video_server
 {
 
@@ -54,9 +55,11 @@ class ImageStreamer
     async_web_server_cpp::HttpConnectionPtr connection,
     rclcpp::Node::SharedPtr node);
 
-  virtual void start() = 0;
+  virtual void start();
   virtual ~ImageStreamer();
 
+  std::map<std::string, std::shared_ptr<SubscriberType> > subscriber_types_;
+
   bool isInactive()
   {
     return inactive_;
@@ -65,7 +68,7 @@ class ImageStreamer
   /**
    * Restreams the last received image frame if older than max_age.
    */
-  virtual void restreamFrame(std::chrono::duration<double> max_age) = 0;
+  virtual void restreamFrame(std::chrono::duration<double> max_age);
 
   std::string getTopic()
   {
@@ -76,44 +79,24 @@ class ImageStreamer
   async_web_server_cpp::HttpConnectionPtr connection_;
   async_web_server_cpp::HttpRequest request_;
   rclcpp::Node::SharedPtr node_;
-  bool inactive_;
-  image_transport::Subscriber image_sub_;
-  std::string topic_;
-};
-
-
-class ImageTransportImageStreamer : public ImageStreamer
-{
-public:
-  ImageTransportImageStreamer(
-    const async_web_server_cpp::HttpRequest & request,
-    async_web_server_cpp::HttpConnectionPtr connection,
-    rclcpp::Node::SharedPtr node);
-  virtual ~ImageTransportImageStreamer();
+  std::shared_ptr<RosSubscriber> subscriber_;
 
-  virtual void start();
-
-protected:
   virtual cv::Mat decodeImage(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
-  virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time) = 0;
-  virtual void restreamFrame(std::chrono::duration<double> max_age);
+  virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point &time) = 0;
   virtual void initialize(const cv::Mat &);
 
-  image_transport::Subscriber image_sub_;
+  std::string topic_;
+  bool inactive_;
   int output_width_;
   int output_height_;
   bool invert_;
-  std::string default_transport_;
-  std::string qos_profile_name_;
+  bool initialized_;
 
   std::chrono::steady_clock::time_point last_frame_;
-  cv::Mat output_size_image;
+  cv::Mat output_size_image_;
   std::mutex send_mutex_;
 
 private:
-  image_transport::ImageTransport it_;
-  bool initialized_;
-
   void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
 };
 

include/web_video_server/jpeg_streamers.hpp

@@ -42,7 +42,7 @@
 namespace web_video_server
 {
 
-class MjpegStreamer : public ImageTransportImageStreamer
+class MjpegStreamer : public ImageStreamer
 {
 public:
   MjpegStreamer(
@@ -69,7 +69,7 @@ class MjpegStreamerType : public ImageStreamerType
   std::string create_viewer(const async_web_server_cpp::HttpRequest & request);
 };
 
-class JpegSnapshotStreamer : public ImageTransportImageStreamer
+class JpegSnapshotStreamer : public ImageStreamer
 {
 public:
   JpegSnapshotStreamer(

include/web_video_server/libav_streamer.hpp

@@ -54,7 +54,7 @@ extern "C"
 namespace web_video_server
 {
 
-class LibavStreamer : public ImageTransportImageStreamer
+class LibavStreamer : public ImageStreamer
 {
 public:
   LibavStreamer(

include/web_video_server/png_streamers.hpp

@@ -41,7 +41,7 @@
 namespace web_video_server
 {
 
-class PngStreamer : public ImageTransportImageStreamer
+class PngStreamer : public ImageStreamer
 {
 public:
   PngStreamer(
@@ -69,7 +69,7 @@ class PngStreamerType : public ImageStreamerType
   std::string create_viewer(const async_web_server_cpp::HttpRequest & request);
 };
 
-class PngSnapshotStreamer : public ImageTransportImageStreamer
+class PngSnapshotStreamer : public ImageStreamer
 {
 public:
   PngSnapshotStreamer(

include/web_video_server/ros_compressed_streamer.hpp

@@ -54,17 +54,16 @@ class RosCompressedStreamer : public ImageStreamer
   virtual void restreamFrame(std::chrono::duration<double> max_age);
 
 protected:
-  virtual void sendImage(
+  virtual void sendImage(const cv::Mat &, const std::chrono::steady_clock::time_point & time);
+  virtual void sendCompressedImage(
     const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg,
     const std::chrono::steady_clock::time_point & time);
 
 private:
-  void imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg);
+  void compressedImageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg);
   MultipartStream stream_;
   rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
-  std::chrono::steady_clock::time_point last_frame_;
   sensor_msgs::msg::CompressedImage::ConstSharedPtr last_msg;
-  std::mutex send_mutex_;
   std::string qos_profile_name_;
 };
 
@@ -75,6 +74,7 @@ class RosCompressedStreamerType : public ImageStreamerType
     const async_web_server_cpp::HttpRequest & request,
     async_web_server_cpp::HttpConnectionPtr connection,
     rclcpp::Node::SharedPtr node);
+
   std::string create_viewer(const async_web_server_cpp::HttpRequest & request);
 };
 

include/web_video_server/subscribers/image_transport_subscriber.hpp

@@ -0,0 +1,36 @@
+
+#pragma once
+
+#include <image_transport/image_transport.hpp>
+#include <image_transport/transport_hints.hpp>
+
+#include <web_video_server/subscribers/ros_subscriber.hpp>
+
+namespace web_video_server
+{
+
+class ImageTransportSubscriber : public RosSubscriber
+{    
+  public:
+    ImageTransportSubscriber(rclcpp::Node::SharedPtr node);
+
+    ~ImageTransportSubscriber();
+
+    virtual void subscribe(const async_web_server_cpp::HttpRequest &request,
+                           const std::string& topic, 
+                           const ImageCallback& callback);    
+
+    void subscriberCallback(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg);
+
+  private:
+    image_transport::Subscriber ros_sub_;
+
+};
+
+class ImageTransportSubscriberType : public SubscriberType
+{
+  public:
+    std::shared_ptr<RosSubscriber> create_subscriber(rclcpp::Node::SharedPtr node);
+};
+
+} //web_video_server

include/web_video_server/subscribers/pointcloud2_subscriber.hpp

@@ -0,0 +1,80 @@
+
+#pragma once
+
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <tf2/exceptions.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include "web_video_server/subscribers/ros_subscriber.hpp"
+
+#ifdef CV_BRIDGE_USES_OLD_HEADERS
+#include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+
+
+namespace web_video_server
+{
+
+class PointCloud2Subscriber : public RosSubscriber
+{    
+  public:
+    PointCloud2Subscriber(rclcpp::Node::SharedPtr node);
+
+    ~PointCloud2Subscriber();
+
+    virtual void subscribe(const async_web_server_cpp::HttpRequest &request,
+                           const std::string& topic, 
+                           const ImageCallback& callback);    
+
+    void subscriberCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &input_msg);
+
+    static bool compareFieldsOffset(sensor_msgs::msg::PointField& field1, sensor_msgs::msg::PointField& field2);
+    static inline int sizeOfPointField(int datatype);
+
+    bool FindFields(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &input_msg, sensor_msgs::msg::PointField &userField, 
+            sensor_msgs::msg::PointField &xField, sensor_msgs::msg::PointField &yField, sensor_msgs::msg::PointField &zField);
+
+    sensor_msgs::msg::PointCloud2 TransformFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &input_msg, std::string frame_id);
+    void GatherCameraInfo(cv::Mat &intrinsic_matrix, cv::Mat &distortion_coefficients);           
+    bool CreateUserImage(const std_msgs::msg::Header &cloud_header, const sensor_msgs::msg::PointField &userField, cv_bridge::CvImage &userImage);
+    bool CreateDepthImage(const std_msgs::msg::Header &cloud_header, cv_bridge::CvImage& depthImage);
+    std::vector<cv::Point2f> ProjectPoints(const sensor_msgs::msg::PointCloud2 &output_cloud, 
+            const sensor_msgs::msg::PointField &xField, const sensor_msgs::msg::PointField &yField, const sensor_msgs::msg::PointField &zField, 
+            const cv::Mat &intrinsic_matrix, const cv::Mat &distortion_coefficients, std::vector<cv::Point3f> &obj_pts);
+    cv_bridge::CvImage NormalizeImage(const cv_bridge::CvImage &inputImage);
+    cv_bridge::CvImage ConvertToColor(const cv::Mat &depthMask, const cv_bridge::CvImage &inputImage);
+
+  private:
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr ros_sub_;
+
+    std::unique_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+
+    geometry_msgs::msg::TransformStamped transform_optical_;
+
+    std::string frame_id_;
+    double wait_for_tf_delay_;
+    std::string field_;
+    int height_, width_, pixel_size_;
+    double focal_length_;
+    bool normalize_;
+    bool colorize_;
+	  rclcpp::CallbackGroup::SharedPtr cbg_sub_;    
+};
+
+class PointCloud2SubscriberType : public SubscriberType
+{
+  public:
+    std::shared_ptr<RosSubscriber> create_subscriber(rclcpp::Node::SharedPtr node);
+};
+
+} //web_video_server

include/web_video_server/subscribers/ros_subscriber.hpp

@@ -0,0 +1,49 @@
+
+#pragma once
+
+#include <mutex>
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <opencv2/opencv.hpp>
+#include <async_web_server_cpp/http_connection.hpp>
+
+#include "web_video_server/utils.hpp"
+
+namespace web_video_server
+{
+  typedef std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr&)> ImageCallback;
+
+class RosSubscriber
+{
+  public:
+
+    RosSubscriber(rclcpp::Node::SharedPtr node); 
+
+    ~RosSubscriber();
+
+    virtual void subscribe(const async_web_server_cpp::HttpRequest &request,
+                           const std::string& topic,
+                           const ImageCallback& callback);
+
+    virtual void subscriberCallback(const sensor_msgs::msg::Image::ConstSharedPtr &input_msg);
+
+    std::mutex subscriber_mutex_;
+
+  protected:
+    rclcpp::Node::SharedPtr node_;
+    std::string qos_profile_name_;
+
+    ImageCallback callback_;
+
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr ros_sub_;
+};
+
+class SubscriberType
+{
+  public:
+    virtual std::shared_ptr<RosSubscriber> create_subscriber(rclcpp::Node::SharedPtr node) = 0;
+};
+
+} // web_video_server

include/web_video_server/web_video_server.hpp

@@ -96,6 +96,7 @@ class WebVideoServer : public rclcpp::Node
 private:
   void restreamFrames(std::chrono::duration<double> max_age);
   void cleanup_inactive_streams();
+  void initializeHttpServer(int server_threads);
 
   rclcpp::TimerBase::SharedPtr cleanup_timer_;
 
@@ -110,9 +111,9 @@ class WebVideoServer : public rclcpp::Node
   std::shared_ptr<async_web_server_cpp::HttpServer> server_;
   async_web_server_cpp::HttpRequestHandlerGroup handler_group_;
 
-  std::vector<std::shared_ptr<ImageStreamer>> image_subscribers_;
+  std::vector<std::shared_ptr<ImageStreamer>> image_streamers_;
   std::map<std::string, std::shared_ptr<ImageStreamerType>> stream_types_;
-  std::mutex subscriber_mutex_;
+  std::mutex streamer_mutex_;
 };
 
 }  // namespace web_video_server