diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index f2bff60..0000000 --- a/.travis.yml +++ /dev/null @@ -1,40 +0,0 @@ -# This config file for Travis CI utilizes ros-industrial/industrial_ci package. -# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst - -dist: trusty -sudo: required -services: - - docker -language: generic -python: - - "2.7" -compiler: - - gcc - -notifications: - email: - on_success: always - on_failure: always - -env: - matrix: - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true - - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu USE_DEB=true - - ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 - - ROS_DISTRO="indigo" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 - -matrix: - allow_failures: - - env: ROS_DISTRO="kinetic" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 - - env: ROS_DISTRO="indigo" PRERELEASE=true PRERELEASE_DOWNSTREAM_DEPTH=0 - -branches: - only: - - master - - develop - -install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config - -script: - - source .ci_config/travis.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index dd1b7d3..d291cbc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,14 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(web_video_server) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp roslib cv_bridge image_transport async_web_server_cpp sensor_msgs) +find_package(ament_cmake_ros REQUIRED) + +find_package(async_web_server_cpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) @@ -14,22 +18,20 @@ pkg_check_modules(avformat libavformat REQUIRED) pkg_check_modules(avutil libavutil REQUIRED) pkg_check_modules(swscale libswscale REQUIRED) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + ################################################### ## Declare things to be passed to other projects ## ################################################### -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package() - ########### ## Build ## ########### ## Specify additional locations of header files include_directories(include - ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${avcodec_INCLUDE_DIRS} ${avformat_INCLUDE_DIRS} @@ -49,9 +51,11 @@ add_executable(${PROJECT_NAME} src/ros_compressed_streamer.cpp src/jpeg_streamers.cpp) +ament_target_dependencies(${PROJECT_NAME} + async_web_server_cpp cv_bridge image_transport rclcpp sensor_msgs) + ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS} ${avcodec_LIBRARIES} @@ -60,18 +64,18 @@ target_link_libraries(${PROJECT_NAME} ${swscale_LIBRARIES} ) +ament_package() + ############# ## Install ## ############# ## Mark executables and/or libraries for installation install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" ) diff --git a/include/web_video_server/h264_streamer.h b/include/web_video_server/h264_streamer.h index 28c67f2..1d9fa31 100644 --- 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 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 index 80aa980..23ca030 100644 --- 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 +#include #include +#include #include #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,7 +63,7 @@ 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 @@ -70,7 +71,7 @@ class ImageStreamerType public: virtual boost::shared_ptr 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 index 6f17e48..8dead91 100644 --- 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,7 +29,7 @@ class MjpegStreamerType : public ImageStreamerType public: boost::shared_ptr 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); }; @@ -37,10 +37,10 @@ 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 index e8dc12a..87ef1c7 100644 --- 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 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 index 95826ff..0a867ec 100644 --- 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 +#include #include #include @@ -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 &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 &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 > pending_footers_; + std::queue > pending_footers_; }; } diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h index 72165c2..2b1d9e3 100644 --- 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 +#include #include "web_video_server/image_streamer.h" #include "async_web_server_cpp/http_request.hpp" #include "async_web_server_cpp/http_connection.hpp" @@ -14,14 +14,14 @@ 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::SharedPtr image_sub_; }; class RosCompressedStreamerType : public ImageStreamerType @@ -29,7 +29,7 @@ class RosCompressedStreamerType : public ImageStreamerType public: boost::shared_ptr 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 index f1632ae..0f4cd7d 100644 --- 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 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 index 041f466..d7f0a2d 100644 --- 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 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 index d5ed833..50c5dfc 100644 --- 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 +#include #include #include #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 @@ -35,6 +35,8 @@ class WebVideoServer */ void spin(); + void setup_cleanup_inactive_streams(); + bool handle_stream(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); @@ -50,12 +52,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::SharedPtr cleanup_timer_; int ros_threads_; int port_; std::string address_; diff --git a/package.xml b/package.xml index a538c2e..60de407 100644 --- a/package.xml +++ b/package.xml @@ -13,18 +13,16 @@ https://github.com/RobotWebTools/web_video_server/issues https://github.com/RobotWebTools/web_video_server - catkin + ament_cmake_ros - roscpp - roslib + rclcpp cv_bridge image_transport async_web_server_cpp ffmpeg sensor_msgs - roscpp - roslib + rclcpp cv_bridge image_transport async_web_server_cpp diff --git a/src/h264_streamer.cpp b/src/h264_streamer.cpp index 5477369..a866aa1 100644 --- 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 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(new H264Streamer(request, connection, nh)); } diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index bfa6861..603d85f 100644 --- 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("width", -1); @@ -23,23 +23,28 @@ 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); + image_transport::TransportHints hints(nh_.get(), default_transport_); + auto tnat = nh_->get_topic_names_and_types(); inactive_ = true; - for (size_t it = 0; it 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); + image_sub_ = it_.subscribe(topic_, 1, &ImageTransportImageStreamer::imageCallback, this, &hints); } 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 +110,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; } diff --git a/src/jpeg_streamers.cpp b/src/jpeg_streamers.cpp index 1f5d6c9..c05feb3 100644 --- a/src/jpeg_streamers.cpp +++ b/src/jpeg_streamers.cpp @@ -5,14 +5,14 @@ namespace web_video_server { MjpegStreamer::MjpegStreamer(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) : ImageTransportImageStreamer(request, connection, nh), stream_(connection) { quality_ = request.get_query_param_value_or_default("quality", 95); stream_.sendInitialHeader(); } -void MjpegStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) { std::vector encode_params; encode_params.push_back(CV_IMWRITE_JPEG_QUALITY); @@ -26,7 +26,7 @@ void MjpegStreamer::sendImage(const cv::Mat &img, const ros::Time &time) boost::shared_ptr MjpegStreamerType::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(new MjpegStreamer(request, connection, nh)); } @@ -42,13 +42,13 @@ std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpReq JpegSnapshotStreamer::JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - ros::NodeHandle& nh) : + rclcpp::Node::SharedPtr nh) : ImageTransportImageStreamer(request, connection, nh) { quality_ = request.get_query_param_value_or_default("quality", 95); } -void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) { std::vector encode_params; encode_params.push_back(CV_IMWRITE_JPEG_QUALITY); @@ -58,7 +58,7 @@ void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const ros::Time &time) cv::imencode(".jpeg", img, encoded_buffer, encode_params); char stamp[20]; - sprintf(stamp, "%.06lf", time.toSec()); + sprintf(stamp, "%.06lf", time.seconds()); async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( "Server", "web_video_server").header("Cache-Control", "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header( diff --git a/src/libav_streamer.cpp b/src/libav_streamer.cpp index 7a4d67c..28d0425 100644 --- a/src/libav_streamer.cpp +++ b/src/libav_streamer.cpp @@ -49,7 +49,7 @@ static int ffmpeg_boost_mutex_lock_manager(void **mutex, enum AVLockOp op) } LibavStreamer::LibavStreamer(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, const std::string &format_name, const std::string &codec_name, const std::string &content_type) : ImageTransportImageStreamer(request, connection, nh), output_format_(0), format_context_(0), codec_(0), codec_context_(0), video_stream_( @@ -240,10 +240,10 @@ void LibavStreamer::initializeEncoder() { } -void LibavStreamer::sendImage(const cv::Mat &img, const ros::Time &time) +void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) { boost::mutex::scoped_lock lock(encode_mutex_); - if (first_image_timestamp_.isZero()) + if (0 == first_image_timestamp_.nanoseconds()) { first_image_timestamp_ = time; } @@ -323,7 +323,7 @@ void LibavStreamer::sendImage(const cv::Mat &img, const ros::Time &time) std::size_t size; uint8_t *output_buf; - double seconds = (time - first_image_timestamp_).toSec(); + double seconds = (time - first_image_timestamp_).seconds(); // Encode video at 1/0.95 to minimize delay pkt.pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); if (pkt.pts <= 0) @@ -365,7 +365,7 @@ LibavStreamerType::LibavStreamerType(const std::string &format_name, const std:: boost::shared_ptr LibavStreamerType::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( new LibavStreamer(request, connection, nh, format_name_, codec_name_, content_type_)); diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index 8f999c4..8afed99 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -20,10 +20,10 @@ void MultipartStream::sendInitialHeader() { connection_->write("--"+boundry_+"\r\n"); } -void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& type, size_t payload_size) { +void MultipartStream::sendPartHeader(const rclcpp::Time &time, const std::string& type, size_t payload_size) { char stamp[20]; - sprintf(stamp, "%.06lf", time.toSec()); - boost::shared_ptr > headers( + sprintf(stamp, "%.06lf", time.seconds()); + std::shared_ptr > headers( new std::vector()); headers->push_back(async_web_server_cpp::HttpHeader("Content-type", type)); headers->push_back(async_web_server_cpp::HttpHeader("X-Timestamp", stamp)); @@ -33,12 +33,12 @@ void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& t } void MultipartStream::sendPartFooter() { - boost::shared_ptr str(new std::string("\r\n--"+boundry_+"\r\n")); + std::shared_ptr str(new std::string("\r\n--"+boundry_+"\r\n")); connection_->write(boost::asio::buffer(*str), str); if (max_queue_size_ > 0) pending_footers_.push(str); } -void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& type, +void MultipartStream::sendPartAndClear(const rclcpp::Time &time, const std::string& type, std::vector &data) { if (!isBusy()) { @@ -48,7 +48,7 @@ void MultipartStream::sendPartAndClear(const ros::Time &time, const std::string& } } -void MultipartStream::sendPart(const ros::Time &time, const std::string& type, +void MultipartStream::sendPart(const rclcpp::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, async_web_server_cpp::HttpConnection::ResourcePtr resource) { if (!isBusy()) diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index 4fbcb17..1797da7 100644 --- a/src/ros_compressed_streamer.cpp +++ b/src/ros_compressed_streamer.cpp @@ -4,7 +4,7 @@ namespace web_video_server { RosCompressedStreamer::RosCompressedStreamer(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), stream_(connection) { stream_.sendInitialHeader(); @@ -12,10 +12,11 @@ RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpReq void RosCompressedStreamer::start() { std::string compressed_topic = topic_ + "/compressed"; - image_sub_ = nh_.subscribe(compressed_topic, 1, &RosCompressedStreamer::imageCallback, this); + image_sub_ = nh_->create_subscription( + compressed_topic, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1), 1); } -void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageConstPtr &msg) { +void RosCompressedStreamer::imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg) { try { std::string content_type; if(msg->format.find("jpeg") != std::string::npos) { @@ -25,28 +26,30 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageCons content_type = "image/png"; } else { - ROS_WARN_STREAM("Unknown ROS compressed image format: " << msg->format); + RCLCPP_WARN(nh_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); return; } - stream_.sendPart(msg->header.stamp, content_type, boost::asio::buffer(msg->data), msg); + stream_.sendPart(rclcpp::Time(msg->header.stamp), content_type, boost::asio::buffer(msg->data), msg); } 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; } @@ -55,7 +58,7 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageCons boost::shared_ptr RosCompressedStreamerType::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(new RosCompressedStreamer(request, connection, nh)); } diff --git a/src/vp8_streamer.cpp b/src/vp8_streamer.cpp index e0c6808..f45b7ca 100644 --- a/src/vp8_streamer.cpp +++ b/src/vp8_streamer.cpp @@ -40,7 +40,7 @@ namespace web_video_server { Vp8Streamer::Vp8Streamer(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, "webm", "libvpx", "video/webm") { quality_ = request.get_query_param_value_or_default("quality", "realtime"); @@ -83,7 +83,7 @@ Vp8StreamerType::Vp8StreamerType() : boost::shared_ptr Vp8StreamerType::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(new Vp8Streamer(request, connection, nh)); } diff --git a/src/vp9_streamer.cpp b/src/vp9_streamer.cpp index b487092..8fcefd8 100644 --- a/src/vp9_streamer.cpp +++ b/src/vp9_streamer.cpp @@ -4,7 +4,7 @@ namespace web_video_server { Vp9Streamer::Vp9Streamer(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, "webm", "libvpx-vp9", "video/webm") { } @@ -30,7 +30,7 @@ Vp9StreamerType::Vp9StreamerType() : boost::shared_ptr Vp9StreamerType::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(new Vp9Streamer(request, connection, nh)); } diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index 416a1a0..89787ba 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -1,8 +1,9 @@ #include #include #include +#include #include -#include +#include #include #include "web_video_server/web_video_server.h" @@ -13,6 +14,8 @@ #include "web_video_server/vp9_streamer.h" #include "async_web_server_cpp/http_reply.hpp" +using namespace std::chrono_literals; + namespace web_video_server { @@ -25,7 +28,8 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler { if (__verbose) { - ROS_INFO_STREAM("Handling Request: " << request.uri); + // TODO reenable + // RCLCPP_INFO(nh->get_logger(), "Handling Request: %s", request.uri.c_str()); } try { @@ -34,31 +38,47 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler } catch (std::exception &e) { - ROS_WARN_STREAM("Error Handling Request: " << e.what()); + // TODO reenable + // RCLCPP_WARN(nh->get_logger(), "Error Handling Request: %s", e.what()); return false; } return false; } -WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) : +WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh) : nh_(nh), handler_group_( async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) { -#if ROS_VERSION_MINIMUM(1, 13, 1) || defined USE_STEADY_TIMER - cleanup_timer_ = nh.createSteadyTimer(ros::WallDuration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this)); -#else - cleanup_timer_ = nh.createTimer(ros::Duration(0.5), boost::bind(&WebVideoServer::cleanup_inactive_streams, this)); -#endif - - private_nh.param("port", port_, 8080); - private_nh.param("verbose", __verbose, true); + rclcpp::Parameter parameter; + if (private_nh->get_parameter("port", parameter)) { + port_ = parameter.as_int(); + } else { + port_ = 8080; + } + if (private_nh->get_parameter("verbose", parameter)) { + __verbose = parameter.as_bool(); + } else { + __verbose = true; + } - private_nh.param("address", address_, "0.0.0.0"); + if (private_nh->get_parameter("address", parameter)) { + address_ = parameter.as_string(); + } else { + address_ = "0.0.0.0"; + } int server_threads; - private_nh.param("server_threads", server_threads, 1); + if (private_nh->get_parameter("server_threads", parameter)) { + server_threads = parameter.as_int(); + } else { + server_threads = 1; + } - private_nh.param("ros_threads", ros_threads_, 2); + if (private_nh->get_parameter("ros_threads", parameter)) { + ros_threads_ = parameter.as_int(); + } else { + ros_threads_ = 2; + } stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); stream_types_["ros_compressed"] = boost::shared_ptr(new RosCompressedStreamerType()); @@ -81,7 +101,7 @@ WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) } catch(boost::exception& e) { - ROS_ERROR("Exception when creating the web server! %s:%d", address_.c_str(), port_); + RCLCPP_ERROR(nh_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); throw; } } @@ -90,11 +110,18 @@ WebVideoServer::~WebVideoServer() { } +void WebVideoServer::setup_cleanup_inactive_streams() +{ + std::function callback = std::bind(&WebVideoServer::cleanup_inactive_streams, this); + cleanup_timer_ = nh_->create_wall_timer(500ms, callback); +} + void WebVideoServer::spin() { server_->run(); - ROS_INFO_STREAM("Waiting For connections on " << address_ << ":" << port_); - ros::MultiThreadedSpinner spinner(ros_threads_); + RCLCPP_INFO(nh_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); + rclcpp::executors::MultiThreadedExecutor spinner(rclcpp::executor::create_default_executor_arguments(), ros_threads_); + spinner.add_node(nh_); spinner.spin(); server_->stop(); } @@ -111,7 +138,7 @@ void WebVideoServer::cleanup_inactive_streams() { for (itr_type itr = new_end; itr < image_subscribers_.end(); ++itr) { - ROS_INFO_STREAM("Removed Stream: " << (*itr)->getTopic()); + RCLCPP_INFO(nh_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); } } image_subscribers_.erase(new_end, image_subscribers_.end()); @@ -181,24 +208,25 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end) { - std::string image_message_type = ros::message_traits::datatype(); - std::string camera_info_message_type = ros::message_traits::datatype(); - - ros::master::V_TopicInfo topics; - ros::master::getTopics(topics); - ros::master::V_TopicInfo::iterator it; std::vector image_topics; std::vector camera_info_topics; - for (it = topics.begin(); it != topics.end(); ++it) - { - const ros::master::TopicInfo &topic = *it; - if (topic.datatype == image_message_type) + auto tnat = nh_->get_topic_names_and_types(); + 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; + auto & topic_type = topic_and_types.second[0]; // explicitly take the first + // TODO debugging + fprintf(stderr, "topic_type: %s\n", topic_type.c_str()); + if (topic_type == "sensor_msgs/Image") { - image_topics.push_back(topic.name); + image_topics.push_back(topic_name); } - else if (topic.datatype == camera_info_message_type) + else if (topic_type == "sensor_msgs/CameraInfo") { - camera_info_topics.push_back(topic.name); + camera_info_topics.push_back(topic_name); } } @@ -270,12 +298,12 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest int main(int argc, char **argv) { - ros::init(argc, argv, "web_video_server"); - - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); + rclcpp::init(argc, argv); + auto nh = std::make_shared("web_video_server"); + auto private_nh = std::make_shared("_web_video_server"); web_video_server::WebVideoServer server(nh, private_nh); + server.setup_cleanup_inactive_streams(); server.spin(); return (0);