From ad7b42cae8c2d98ba81e24e42e8ef49ea6c72bed Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 23 Apr 2022 21:52:18 -0600 Subject: [PATCH 1/4] Add support for u16; colormaps --- include/web_video_server/image_streamer.h | 4 +++ src/image_streamer.cpp | 36 +++++++++++++++-------- 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index b5caba2..848b52b 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -74,6 +74,10 @@ class ImageTransportImageStreamer : public ImageStreamer bool initialized_; void imageCallback(const sensor_msgs::ImageConstPtr &msg); + + + float min_v_ = NAN, max_v_ = NAN; + int colormap_ = -1; }; class ImageStreamerType diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index bbe45bb..4b7c982 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -24,6 +24,14 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_ output_height_ = request.get_query_param_value_or_default("height", -1); invert_ = request.has_query_param("invert"); default_transport_ = request.get_query_param_value_or_default("default_transport", "raw"); + + min_v_ = request.get_query_param_value_or_default("min", min_v_); + max_v_ = request.get_query_param_value_or_default("max", max_v_); + colormap_ = request.get_query_param_value_or_default("colormap", colormap_); + + if(std::isnan(min_v_) && !std::isnan(max_v_)) { + min_v_ = 0; + } } ImageTransportImageStreamer::~ImageTransportImageStreamer() @@ -89,19 +97,23 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr cv::Mat img; try { - if (msg->encoding.find("F") != std::string::npos) - { - // scale floating point images - cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; - cv::Mat_ float_image = float_image_bridge; - double max_val; - cv::minMaxIdx(float_image, 0, &max_val); - - if (max_val > 0) - { - float_image *= (255 / max_val); + if (msg->encoding.find("16") != std::string::npos || msg->encoding.find("F") != std::string::npos) { + cv::Mat image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; + + double max_val, min_val; + if(std::isnan(min_v_) || std::isnan(min_v_)) { + cv::minMaxIdx(image_bridge, &min_val, &max_val); + } else { + min_val = min_v_; + max_val = max_v_; + } + + float scale = 255. / (max_val - min_val); + image_bridge.convertTo(img, CV_8U, scale, -min_val * scale); + + if(colormap_ >= 0) { + cv::applyColorMap(img, img, colormap_); } - img = float_image; } else { From 5eab98cfc141663fc1ef306bfff570965aefb494 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 24 Apr 2022 22:39:56 -0600 Subject: [PATCH 2/4] Add JSON endpoint --- include/web_video_server/web_video_server.h | 3 + src/web_video_server.cpp | 67 ++++++++++++++++++++- 2 files changed, 69 insertions(+), 1 deletion(-) diff --git a/include/web_video_server/web_video_server.h b/include/web_video_server/web_video_server.h index e4e0d73..138f587 100644 --- a/include/web_video_server/web_video_server.h +++ b/include/web_video_server/web_video_server.h @@ -49,6 +49,9 @@ class WebVideoServer bool handle_list_streams(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + bool handle_list_streams_json(const async_web_server_cpp::HttpRequest &request, + async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); + private: void restreamFrames(double max_age); void cleanup_inactive_streams(); diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index a1e76e3..f3fc99e 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -74,6 +74,7 @@ WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) stream_types_["vp9"] = boost::shared_ptr(new Vp9StreamerType()); handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2, _3, _4)); + handler_group_.addHandlerForPath("/json", boost::bind(&WebVideoServer::handle_list_streams_json, this, _1, _2, _3, _4)); handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2, _3, _4)); handler_group_.addHandlerForPath("/stream_viewer", boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2, _3, _4)); @@ -342,8 +343,72 @@ bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest return true; } -} +bool WebVideoServer::handle_list_streams_json(const async_web_server_cpp::HttpRequest &request, + 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) + { + image_topics.push_back(topic.name); + } + else if (topic.datatype == camera_info_message_type) + { + camera_info_topics.push_back(topic.name); + } + } + 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("Pragma", "no-cache"). + header("Content-type", "application/json;"). + header("Access-Control-Allow-Headers", "*"). + header("Keep-Alive", "timeout=1"). + header("Access-Control-Allow-Origin", "*").write(connection); + + connection->write("["); + bool first = true; + BOOST_FOREACH(std::string & camera_info_topic, camera_info_topics) + { + if (boost::algorithm::ends_with(camera_info_topic, "/camera_info")) + { + std::string base_topic = camera_info_topic.substr(0, camera_info_topic.size() - strlen("camera_info")); + std::vector::iterator image_topic_itr = image_topics.begin(); + for (; image_topic_itr != image_topics.end();) + { + if (boost::starts_with(*image_topic_itr, base_topic)) + { + if(!first) + connection->write(","); + first = false; + connection->write("\""); + connection->write(*image_topic_itr); + connection->write("\""); + + image_topic_itr = image_topics.erase(image_topic_itr); + } + else + { + ++image_topic_itr; + } + } + } + } + connection->write("]"); + return true; +} +} int main(int argc, char **argv) { ros::init(argc, argv, "web_video_server"); From d00eedb6d43374f4bc4b10eb5c67f9717ee886e4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 24 Apr 2022 22:41:30 -0600 Subject: [PATCH 3/4] Use wall clock in places where the connection can leak if the sim time is looping --- include/web_video_server/image_streamer.h | 2 +- include/web_video_server/multipart_stream.h | 2 +- src/image_streamer.cpp | 4 ++-- src/multipart_stream.cpp | 13 +++++++------ 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/include/web_video_server/image_streamer.h b/include/web_video_server/image_streamer.h index 848b52b..80ce843 100644 --- a/include/web_video_server/image_streamer.h +++ b/include/web_video_server/image_streamer.h @@ -65,7 +65,7 @@ class ImageTransportImageStreamer : public ImageStreamer bool invert_; std::string default_transport_; - ros::Time last_frame; + ros::WallTime last_frame; cv::Mat output_size_image; boost::mutex send_mutex_; diff --git a/include/web_video_server/multipart_stream.h b/include/web_video_server/multipart_stream.h index 476ee73..4b5139d 100644 --- a/include/web_video_server/multipart_stream.h +++ b/include/web_video_server/multipart_stream.h @@ -10,7 +10,7 @@ namespace web_video_server { struct PendingFooter { - ros::Time timestamp; + ros::WallTime timestamp; boost::weak_ptr contents; }; diff --git a/src/image_streamer.cpp b/src/image_streamer.cpp index 4b7c982..76e7230 100644 --- a/src/image_streamer.cpp +++ b/src/image_streamer.cpp @@ -63,7 +63,7 @@ void ImageTransportImageStreamer::restreamFrame(double max_age) if (inactive_ || !initialized_ ) return; try { - if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) { + if ( last_frame + ros::WallDuration(max_age) < ros::WallTime::now() ) { boost::mutex::scoped_lock lock(send_mutex_); sendImage(output_size_image, ros::Time::now() ); // don't update last_frame, it may remain an old value. } @@ -154,7 +154,7 @@ void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr initialized_ = true; } - last_frame = ros::Time::now(); + last_frame = ros::WallTime::now(); sendImage(output_size_image, msg->header.stamp); } catch (cv_bridge::Exception &e) diff --git a/src/multipart_stream.cpp b/src/multipart_stream.cpp index c38056d..b63076f 100644 --- a/src/multipart_stream.cpp +++ b/src/multipart_stream.cpp @@ -15,9 +15,10 @@ void MultipartStream::sendInitialHeader() { 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( - "Pragma", "no-cache").header("Content-type", "multipart/x-mixed-replace;boundary="+boundry_).header( - "Access-Control-Allow-Origin", "*").header("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,HEAD,OPTIONS").header( - "Access-Control-Allow-Headers", "Origin, Authorization, Accept, Content-Type").header("Access-Control-Max-Age", "3600").write(connection_); + "Pragma", "no-cache").header("Content-type", "multipart/x-mixed-replace;boundary="+boundry_) + .header("Access-Control-Allow-Origin", "*").header("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,HEAD,OPTIONS") + .header("Access-Control-Allow-Headers", "Origin, Authorization, Accept, Content-Type") + .header("Access-Control-Max-Age", "3600").write(connection_); connection_->write("--"+boundry_+"\r\n"); } @@ -40,7 +41,7 @@ void MultipartStream::sendPartHeader(const ros::Time &time, const std::string& t void MultipartStream::sendPartFooter(const ros::Time &time) { boost::shared_ptr str(new std::string("\r\n--"+boundry_+"\r\n")); PendingFooter pf; - pf.timestamp = time; + pf.timestamp = ros::WallTime::now(); pf.contents = str; connection_->write(boost::asio::buffer(*str), str); if (max_queue_size_ > 0) pending_footers_.push(pf); @@ -68,13 +69,13 @@ void MultipartStream::sendPart(const ros::Time &time, const std::string& type, } bool MultipartStream::isBusy() { - ros::Time currentTime = ros::Time::now(); + auto currentTime = ros::WallTime::now(); while (!pending_footers_.empty()) { if (pending_footers_.front().contents.expired()) { pending_footers_.pop(); } else { - ros::Time footerTime = pending_footers_.front().timestamp; + auto footerTime = pending_footers_.front().timestamp; if ((currentTime - footerTime).toSec() > 0.5) { pending_footers_.pop(); } else { From e5bf5250bee6e9032294c2ad9b2d9d5a3846f428 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 15 Jul 2022 22:57:43 -0600 Subject: [PATCH 4/4] Additional verbose logging --- src/web_video_server.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/web_video_server.cpp b/src/web_video_server.cpp index f3fc99e..691ae2e 100644 --- a/src/web_video_server.cpp +++ b/src/web_video_server.cpp @@ -33,6 +33,11 @@ static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler try { forward(request, connection, begin, end); + if (__verbose) + { + ROS_INFO_STREAM("Request forwarded: " << request.uri); + } + return true; } catch (std::exception &e) @@ -64,6 +69,9 @@ WebVideoServer::WebVideoServer(ros::NodeHandle &nh, ros::NodeHandle &private_nh) private_nh.param("ros_threads", ros_threads_, 2); private_nh.param("publish_rate", publish_rate_, -1.0); + if(__verbose) { + ROS_INFO_STREAM("Starting with args server threads: " << server_threads << " ros_threads: " << ros_threads_); + } private_nh.param("default_stream_type", __default_stream_type, "mjpeg"); stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); @@ -101,7 +109,7 @@ WebVideoServer::~WebVideoServer() void WebVideoServer::spin() { server_->run(); - ROS_INFO_STREAM("Waiting For connections on " << address_ << ":" << port_); + ROS_INFO_STREAM("Waiting For connections on " << address_ << ":" << port_); ros::AsyncSpinner spinner(ros_threads_); spinner.start(); @@ -184,6 +192,10 @@ bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &requ streamer->start(); boost::mutex::scoped_lock lock(subscriber_mutex_); image_subscribers_.push_back(streamer); + if (__verbose) + { + ROS_INFO_STREAM("Serving " << image_subscribers_.size() << " streams"); + } } else {