Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
.cproject
.project
.vscode/
11 changes: 11 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
45 changes: 14 additions & 31 deletions include/web_video_server/image_streamer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{

Expand All @@ -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_;
Expand All @@ -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()
{
Expand All @@ -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);
};

Expand Down
4 changes: 2 additions & 2 deletions include/web_video_server/jpeg_streamers.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
namespace web_video_server
{

class MjpegStreamer : public ImageTransportImageStreamer
class MjpegStreamer : public ImageStreamer
{
public:
MjpegStreamer(
Expand All @@ -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(
Expand Down
2 changes: 1 addition & 1 deletion include/web_video_server/libav_streamer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ extern "C"
namespace web_video_server
{

class LibavStreamer : public ImageTransportImageStreamer
class LibavStreamer : public ImageStreamer
{
public:
LibavStreamer(
Expand Down
4 changes: 2 additions & 2 deletions include/web_video_server/png_streamers.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
namespace web_video_server
{

class PngStreamer : public ImageTransportImageStreamer
class PngStreamer : public ImageStreamer
{
public:
PngStreamer(
Expand Down Expand Up @@ -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(
Expand Down
8 changes: 4 additions & 4 deletions include/web_video_server/ros_compressed_streamer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
};

Expand All @@ -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);
};

Expand Down
Original file line number Diff line number Diff line change
@@ -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
80 changes: 80 additions & 0 deletions include/web_video_server/subscribers/pointcloud2_subscriber.hpp
Original file line number Diff line number Diff line change
@@ -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
49 changes: 49 additions & 0 deletions include/web_video_server/subscribers/ros_subscriber.hpp
Original file line number Diff line number Diff line change
@@ -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
5 changes: 3 additions & 2 deletions include/web_video_server/web_video_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand All @@ -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
Empty file added log/COLCON_IGNORE
Empty file.
Loading