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 4c97288..7b96ffd 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ async_web_server_cpp ffmpeg sensor_msgs + dynamic_reconfigure roscpp roslib @@ -32,4 +33,5 @@ async_web_server_cpp ffmpeg sensor_msgs + dynamic_reconfigure diff --git a/src/ros_compressed_streamer.cpp b/src/ros_compressed_streamer.cpp index e1a142e..e76fa53 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 +#include +#include +#include + 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("quality", PARAM_NO_CHANGE_INT); + png_level_ = request.get_query_param_value_or_default("level", PARAM_NO_CHANGE_INT); + format_ = request.get_query_param_value_or_default("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 RosCompressedStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle& nh)