From 0c44d5182fef232034666dd03fa8925c011f0171 Mon Sep 17 00:00:00 2001
From: Alexey Rogachevskiy <sfalexrog@gmail.com>
Date: Thu, 26 Dec 2019 17:52:31 +0300
Subject: [PATCH] ros_compressed: Reconfigure compressed topic from request

---
 CMakeLists.txt                                |  2 +-
 .../ros_compressed_streamer.h                 |  5 ++
 package.xml                                   |  2 +
 src/ros_compressed_streamer.cpp               | 46 +++++++++++++++++++
 4 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dcb702f..0356811 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -4,7 +4,7 @@ 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(catkin REQUIRED COMPONENTS roscpp roslib cv_bridge image_transport async_web_server_cpp sensor_msgs dynamic_reconfigure)
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS thread)
 
diff --git a/include/web_video_server/ros_compressed_streamer.h b/include/web_video_server/ros_compressed_streamer.h
index 7e8af37..91b3501 100644
--- a/include/web_video_server/ros_compressed_streamer.h
+++ b/include/web_video_server/ros_compressed_streamer.h
@@ -25,11 +25,16 @@ class RosCompressedStreamer : public ImageStreamer
 
 private:
   void imageCallback(const sensor_msgs::CompressedImageConstPtr &msg);
+  void updateImageParameters();
   MultipartStream stream_;
   ros::Subscriber image_sub_;
   ros::Time last_frame;
   sensor_msgs::CompressedImageConstPtr last_msg;
   boost::mutex send_mutex_;
+
+  int jpeg_quality_;
+  int png_level_;
+  std::string format_;
 };
 
 class RosCompressedStreamerType : public ImageStreamerType
diff --git a/package.xml b/package.xml
index 6c2a71c..7bc037e 100644
--- a/package.xml
+++ b/package.xml
@@ -22,6 +22,7 @@
   <build_depend>async_web_server_cpp</build_depend>
   <build_depend>ffmpeg</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>roslib</exec_depend>
@@ -30,4 +31,5 @@
   <exec_depend>async_web_server_cpp</exec_depend>
   <exec_depend>ffmpeg</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
 </package>
diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp
index eedcf0d..d86ec0b 100644
--- a/src/ros_compressed_streamer.cpp
+++ b/src/ros_compressed_streamer.cpp
@@ -1,12 +1,23 @@
 #include "web_video_server/ros_compressed_streamer.h"
 
+#include <dynamic_reconfigure/IntParameter.h>
+#include <dynamic_reconfigure/StrParameter.h>
+#include <dynamic_reconfigure/Reconfigure.h>
+#include <dynamic_reconfigure/Config.h>
+
 namespace web_video_server
 {
 
+const int PARAM_NO_CHANGE_INT = -1;
+
 RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request,
                              async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh) :
   ImageStreamer(request, connection, nh), stream_(connection)
 {
+  jpeg_quality_ = request.get_query_param_value_or_default<int>("quality", PARAM_NO_CHANGE_INT);
+  png_level_ = request.get_query_param_value_or_default<int>("level", PARAM_NO_CHANGE_INT);
+  format_ = request.get_query_param_value_or_default<std::string>("format", "");
+
   stream_.sendInitialHeader();
 }
 
@@ -18,6 +29,7 @@ RosCompressedStreamer::~RosCompressedStreamer()
 
 void RosCompressedStreamer::start() {
   std::string compressed_topic = topic_ + "/compressed";
+  updateImageParameters();
   image_sub_ = nh_.subscribe(compressed_topic, 1, &RosCompressedStreamer::imageCallback, this);
 }
 
@@ -79,6 +91,40 @@ void RosCompressedStreamer::imageCallback(const sensor_msgs::CompressedImageCons
 }
 
 
+void RosCompressedStreamer::updateImageParameters()
+{
+  dynamic_reconfigure::Config reconf_config;
+  if (jpeg_quality_ > 0 && jpeg_quality_ <= 100) {
+    dynamic_reconfigure::IntParameter param;
+    param.name = "jpeg_quality";
+    param.value = jpeg_quality_;
+    ROS_DEBUG("setting jpeg quality to %d for topic %s", jpeg_quality_, topic_.c_str());
+    reconf_config.ints.push_back(param);
+  }
+  if (png_level_ > 0 && png_level_ <= 9) {
+    dynamic_reconfigure::IntParameter param;
+    param.name = "png_level";
+    param.value = png_level_;
+    ROS_DEBUG("setting png compression level to %d for topic %s", png_level_, topic_.c_str());
+    reconf_config.ints.push_back(param);
+  }
+  if (format_ == "jpeg" || format_ == "png") { // Only allow jpeg/png compression
+    dynamic_reconfigure::StrParameter param;
+    param.name = "format";
+    param.value = format_;
+    ROS_DEBUG("setting compression format to %s for topic %s", format_.c_str(), topic_.c_str());
+    reconf_config.strs.push_back(param);
+  }
+  if (reconf_config.ints.size() > 0 || reconf_config.strs.size() > 0)
+  {
+    dynamic_reconfigure::ReconfigureRequest reconf_request;
+    dynamic_reconfigure::ReconfigureResponse reconf_response;
+    reconf_request.config = reconf_config;
+    ROS_DEBUG("applying settings for %s", format_.c_str());
+    ros::service::call(topic_ + "/compressed/set_parameters", reconf_request, reconf_response);
+  }
+}
+
 boost::shared_ptr<ImageStreamer> RosCompressedStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request,
 										 async_web_server_cpp::HttpConnectionPtr connection,
 										 ros::NodeHandle& nh)