update code to use ROS 2 API

RobotWebTools · Aug 20, 2018 · 1a5f938 · 1a5f938
1 parent 4e90279
commit 1a5f938
Show file tree

Hide file tree

Showing 18 changed files with 151 additions and 122 deletions.
diff --git a/include/web_video_server/h264_streamer.h b/include/web_video_server/h264_streamer.h
@@ -13,7 +13,7 @@ class H264Streamer : public LibavStreamer
 {
 public:
   H264Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection,
-              ros::NodeHandle& nh);
+              rclcpp::Node::SharedPtr nh);
   ~H264Streamer();
 protected:
   virtual void initializeEncoder();
@@ -26,7 +26,7 @@ class H264StreamerType : public LibavStreamerType
   H264StreamerType();
   virtual boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest& request,
                                                            async_web_server_cpp::HttpConnectionPtr connection,
-                                                           ros::NodeHandle& nh);
+                                                           rclcpp::Node::SharedPtr nh);
 };
 
 }

diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h
@@ -1,8 +1,9 @@
 #ifndef IMAGE_STREAMER_H_
 #define IMAGE_STREAMER_H_
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <image_transport/image_transport.h>
+#include <image_transport/transport_hints.h>
 #include <opencv2/opencv.hpp>
 #include "async_web_server_cpp/http_server.hpp"
 #include "async_web_server_cpp/http_request.hpp"
@@ -15,7 +16,7 @@ class ImageStreamer
 public:
   ImageStreamer(const async_web_server_cpp::HttpRequest &request,
 		async_web_server_cpp::HttpConnectionPtr connection,
-		ros::NodeHandle& nh);
+		rclcpp::Node::SharedPtr nh);
 
   virtual void start() = 0;
 
@@ -33,7 +34,7 @@ class ImageStreamer
 protected:
   async_web_server_cpp::HttpConnectionPtr connection_;
   async_web_server_cpp::HttpRequest request_;
-  ros::NodeHandle nh_;
+  rclcpp::Node::SharedPtr nh_;
   bool inactive_;
   image_transport::Subscriber image_sub_;
   std::string topic_;
@@ -44,12 +45,12 @@ class ImageTransportImageStreamer : public ImageStreamer
 {
 public:
   ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection,
-			      ros::NodeHandle& nh);
+			      rclcpp::Node::SharedPtr nh);
 
   virtual void start();
 
 protected:
-  virtual void sendImage(const cv::Mat &, const ros::Time &time) = 0;
+  virtual void sendImage(const cv::Mat &, const rclcpp::Time &time) = 0;
 
   virtual void initialize(const cv::Mat &);
 
@@ -62,15 +63,15 @@ class ImageTransportImageStreamer : public ImageStreamer
   image_transport::ImageTransport it_;
   bool initialized_;
 
-  void imageCallback(const sensor_msgs::ImageConstPtr &msg);
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg);
 };
 
 class ImageStreamerType
 {
 public:
   virtual boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest &request,
                                                            async_web_server_cpp::HttpConnectionPtr connection,
-                                                           ros::NodeHandle& nh) = 0;
+                                                           rclcpp::Node::SharedPtr nh) = 0;
 
   virtual std::string create_viewer(const async_web_server_cpp::HttpRequest &request) = 0;
 };

diff --git a/include/web_video_server/jpeg_streamers.h b/include/web_video_server/jpeg_streamers.h
@@ -14,10 +14,10 @@ class MjpegStreamer : public ImageTransportImageStreamer
 {
 public:
   MjpegStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection,
-                ros::NodeHandle& nh);
+                rclcpp::Node::SharedPtr nh);
 
 protected:
-  virtual void sendImage(const cv::Mat &, const ros::Time &time);
+  virtual void sendImage(const cv::Mat &, const rclcpp::Time &time);
 
 private:
   MultipartStream stream_;
@@ -29,18 +29,18 @@ class MjpegStreamerType : public ImageStreamerType
 public:
   boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest &request,
                                                    async_web_server_cpp::HttpConnectionPtr connection,
-                                                   ros::NodeHandle& nh);
+                                                   rclcpp::Node::SharedPtr nh);
   std::string create_viewer(const async_web_server_cpp::HttpRequest &request);
 };
 
 class JpegSnapshotStreamer : public ImageTransportImageStreamer
 {
 public:
   JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request,
-                       async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh);
+                       async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh);
 
 protected:
-  virtual void sendImage(const cv::Mat &, const ros::Time &time);
+  virtual void sendImage(const cv::Mat &, const rclcpp::Time &time);
 
 private:
   int quality_;

diff --git a/include/web_video_server/libav_streamer.h b/include/web_video_server/libav_streamer.h
@@ -25,14 +25,14 @@ class LibavStreamer : public ImageTransportImageStreamer
 {
 public:
   LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection,
-                ros::NodeHandle& nh, const std::string &format_name, const std::string &codec_name,
+                rclcpp::Node::SharedPtr nh, const std::string &format_name, const std::string &codec_name,
                 const std::string &content_type);
 
   ~LibavStreamer();
 
 protected:
   virtual void initializeEncoder();
-  virtual void sendImage(const cv::Mat&, const ros::Time& time);
+  virtual void sendImage(const cv::Mat&, const rclcpp::Time& time);
   virtual void initialize(const cv::Mat&);
   AVOutputFormat* output_format_;
   AVFormatContext* format_context_;
@@ -45,7 +45,7 @@ class LibavStreamer : public ImageTransportImageStreamer
 private:
   AVFrame* frame_;
   struct SwsContext* sws_context_;
-  ros::Time first_image_timestamp_;
+  rclcpp::Time first_image_timestamp_;
   boost::mutex encode_mutex_;
 
   std::string format_name_;
@@ -66,7 +66,7 @@ class LibavStreamerType : public ImageStreamerType
 
   boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest &request,
                                                    async_web_server_cpp::HttpConnectionPtr connection,
-                                                   ros::NodeHandle& nh);
+                                                   rclcpp::Node::SharedPtr nh);
 
   std::string create_viewer(const async_web_server_cpp::HttpRequest &request);
 

diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.h
@@ -1,7 +1,7 @@
 #ifndef MULTIPART_STREAM_H_
 #define MULTIPART_STREAM_H_
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <async_web_server_cpp/http_connection.hpp>
 
 #include <queue>
@@ -16,10 +16,10 @@ class MultipartStream {
                   std::size_t max_queue_size=1);
 
   void sendInitialHeader();
-  void sendPartHeader(const ros::Time &time, const std::string& type, size_t payload_size);
+  void sendPartHeader(const rclcpp::Time &time, const std::string& type, size_t payload_size);
   void sendPartFooter();
-  void sendPartAndClear(const ros::Time &time, const std::string& type, std::vector<unsigned char> &data);
-  void sendPart(const ros::Time &time, const std::string& type, const boost::asio::const_buffer &buffer,
+  void sendPartAndClear(const rclcpp::Time &time, const std::string& type, std::vector<unsigned char> &data);
+  void sendPart(const rclcpp::Time &time, const std::string& type, const boost::asio::const_buffer &buffer,
 		async_web_server_cpp::HttpConnection::ResourcePtr resource);
 
 private:
@@ -29,7 +29,7 @@ class MultipartStream {
   const std::size_t max_queue_size_;
   async_web_server_cpp::HttpConnectionPtr connection_;
   std::string boundry_;
-  std::queue<boost::weak_ptr<const void> > pending_footers_;
+  std::queue<std::weak_ptr<const void> > pending_footers_;
 };
 
 }

diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h
@@ -1,7 +1,7 @@
 #ifndef ROS_COMPRESSED_STREAMERS_H_
 #define ROS_COMPRESSED_STREAMERS_H_
 
-#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/msg/compressed_image.hpp>
 #include "web_video_server/image_streamer.h"
 #include "async_web_server_cpp/http_request.hpp"
 #include "async_web_server_cpp/http_connection.hpp"
@@ -14,22 +14,22 @@ class RosCompressedStreamer : public ImageStreamer
 {
 public:
   RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection,
-			ros::NodeHandle& nh);
+			rclcpp::Node::SharedPtr nh);
   virtual void start();
 
 private:
-  void imageCallback(const sensor_msgs::CompressedImageConstPtr &msg);
+  void imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg);
 
   MultipartStream stream_;
-  ros::Subscriber image_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub_;
 };
 
 class RosCompressedStreamerType : public ImageStreamerType
 {
 public:
   boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest &request,
                                                    async_web_server_cpp::HttpConnectionPtr connection,
-                                                   ros::NodeHandle& nh);
+                                                   rclcpp::Node::SharedPtr nh);
   std::string create_viewer(const async_web_server_cpp::HttpRequest &request);
 };
 

diff --git a/include/web_video_server/vp8_streamer.h b/include/web_video_server/vp8_streamer.h
@@ -49,7 +49,7 @@ class Vp8Streamer : public LibavStreamer
 {
 public:
   Vp8Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection,
-              ros::NodeHandle& nh);
+              rclcpp::Node::SharedPtr nh);
   ~Vp8Streamer();
 protected:
   virtual void initializeEncoder();
@@ -63,7 +63,7 @@ class Vp8StreamerType : public LibavStreamerType
   Vp8StreamerType();
   virtual boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest& request,
                                                            async_web_server_cpp::HttpConnectionPtr connection,
-                                                           ros::NodeHandle& nh);
+                                                           rclcpp::Node::SharedPtr nh);
 };
 
 }

diff --git a/include/web_video_server/vp9_streamer.h b/include/web_video_server/vp9_streamer.h
@@ -13,7 +13,7 @@ class Vp9Streamer : public LibavStreamer
 {
 public:
   Vp9Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection,
-              ros::NodeHandle& nh);
+              rclcpp::Node::SharedPtr nh);
   ~Vp9Streamer();
 protected:
   virtual void initializeEncoder();
@@ -25,7 +25,7 @@ class Vp9StreamerType : public LibavStreamerType
   Vp9StreamerType();
   virtual boost::shared_ptr<ImageStreamer> create_streamer(const async_web_server_cpp::HttpRequest& request,
                                                            async_web_server_cpp::HttpConnectionPtr connection,
-                                                           ros::NodeHandle& nh);
+                                                           rclcpp::Node::SharedPtr nh);
 };
 
 }

diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h
@@ -1,7 +1,7 @@
 #ifndef WEB_VIDEO_SERVER_H_
 #define WEB_VIDEO_SERVER_H_
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <vector>
 #include "web_video_server/image_streamer.h"
@@ -23,7 +23,7 @@ class WebVideoServer
    * @brief  Constructor
    * @return
    */
-  WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh);
+  WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh);
 
   /**
    * @brief  Destructor - Cleans up
@@ -50,12 +50,8 @@ class WebVideoServer
 private:
   void cleanup_inactive_streams();
 
-  ros::NodeHandle nh_;
-#if ROS_VERSION_MINIMUM(1, 13, 1) || defined USE_STEADY_TIMER
-  ros::SteadyTimer cleanup_timer_;
-#else
-  ros::Timer cleanup_timer_;
-#endif
+  rclcpp::Node::SharedPtr nh_;
+  rclcpp::WallTimer<rclcpp::VoidCallbackType> cleanup_timer_;
   int ros_threads_;
   int port_;
   std::string address_;

diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp
@@ -4,7 +4,7 @@ namespace web_video_server
 {
 
 H264Streamer::H264Streamer(const async_web_server_cpp::HttpRequest& request,
-                         async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh) :
+                         async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) :
     LibavStreamer(request, connection, nh, "mp4", "libx264", "video/mp4")
 {
   /* possible quality presets:
@@ -41,7 +41,7 @@ H264StreamerType::H264StreamerType() :
 
 boost::shared_ptr<ImageStreamer> H264StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request,
                                                                   async_web_server_cpp::HttpConnectionPtr connection,
-                                                                  ros::NodeHandle& nh)
+                                                                  rclcpp::Node::SharedPtr nh)
 {
   return boost::shared_ptr<ImageStreamer>(new H264Streamer(request, connection, nh));
 }

diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp
@@ -5,14 +5,14 @@ namespace web_video_server
 {
 
 ImageStreamer::ImageStreamer(const async_web_server_cpp::HttpRequest &request,
-                             async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh) :
+                             async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) :
     request_(request), connection_(connection), nh_(nh), inactive_(false)
 {
   topic_ = request.get_query_param_value_or_default("topic", "");
 }
 
 ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request,
-                             async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh) :
+                             async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) :
   ImageStreamer(request, connection, nh), it_(nh), initialized_(false)
 {
   output_width_ = request.get_query_param_value_or_default<int>("width", -1);
@@ -24,22 +24,29 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_
 void ImageTransportImageStreamer::start()
 {
   image_transport::TransportHints hints(default_transport_);
-  ros::master::V_TopicInfo available_topics;
-  ros::master::getTopics(available_topics);
+  auto tnat = nh_->get_topic_names_and_types();
   inactive_ = true;
-  for (size_t it = 0; it<available_topics.size(); it++){
-    if(available_topics[it].name == topic_){
+  for (auto topic_and_types : tnat) {
+    if (topic_and_types.second.size() > 1) {
+      // explicitly avoid topics with more than one type
+      break;
+    }
+    auto & topic_name = topic_and_types.first;
+    if(topic_name == topic_){
       inactive_ = false;
+      break;
     }
   }
-  image_sub_ = it_.subscribe(topic_, 1, &ImageTransportImageStreamer::imageCallback, this, hints);
+  rmw_qos_profile_t custom_qos = rmw_qos_profile_default;
+  custom_qos.depth = 1;
+  image_sub_ = it_.subscribe(topic_, &ImageTransportImageStreamer::imageCallback, this, default_transport_, custom_qos);
 }
 
 void ImageTransportImageStreamer::initialize(const cv::Mat &)
 {
 }
 
-void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr &msg)
+void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg)
 {
   if (inactive_)
     return;
@@ -105,32 +112,36 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr
   }
   catch (cv_bridge::Exception &e)
   {
-    ROS_ERROR_THROTTLE(30, "cv_bridge exception: %s", e.what());
+    // TODO THROTTLE with 30
+    RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what());
     inactive_ = true;
     return;
   }
   catch (cv::Exception &e)
   {
-    ROS_ERROR_THROTTLE(30, "cv_bridge exception: %s", e.what());
+    // TODO THROTTLE with 30
+    RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what());
     inactive_ = true;
     return;
   }
   catch (boost::system::system_error &e)
   {
     // happens when client disconnects
-    ROS_DEBUG("system_error exception: %s", e.what());
+    RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what());
     inactive_ = true;
     return;
   }
   catch (std::exception &e)
   {
-    ROS_ERROR_THROTTLE(30, "exception: %s", e.what());
+    // TODO THROTTLE with 30
+    RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what());
     inactive_ = true;
     return;
   }
   catch (...)
   {
-    ROS_ERROR_THROTTLE(30, "exception");
+    // TODO THROTTLE with 30
+    RCLCPP_ERROR(nh_->get_logger(), "exception");
     inactive_ = true;
     return;
   }