image_streamer.cpp
Go to the documentation of this file.
2 #include <cv_bridge/cv_bridge.h>
3 #include <iostream>
4 
5 namespace web_video_server
6 {
7 
10  request_(request), connection_(connection), nh_(nh), inactive_(false)
11 {
12  topic_ = request.get_query_param_value_or_default("topic", "");
13 }
14 
16 {
17 }
18 
21  ImageStreamer(request, connection, nh), it_(nh), initialized_(false)
22 {
23  output_width_ = request.get_query_param_value_or_default<int>("width", -1);
24  output_height_ = request.get_query_param_value_or_default<int>("height", -1);
25  invert_ = request.has_query_param("invert");
26  default_transport_ = request.get_query_param_value_or_default("default_transport", "raw");
27 }
28 
30 {
31 }
32 
34 {
36  ros::master::V_TopicInfo available_topics;
37  ros::master::getTopics(available_topics);
38  inactive_ = true;
39  for (size_t it = 0; it<available_topics.size(); it++){
40  std::string available_topic_name = available_topics[it].name;
41  if(available_topic_name == topic_ || (available_topic_name.find("/") == 0 &&
42  available_topic_name.substr(1) == topic_)) {
43  inactive_ = false;
44  }
45  }
47 }
48 
50 {
51 }
52 
54 {
55  if (inactive_ || !initialized_ )
56  return;
57  try {
58  if ( last_frame + ros::Duration(max_age) < ros::Time::now() ) {
59  boost::mutex::scoped_lock lock(send_mutex_);
60  sendImage(output_size_image, ros::Time::now() ); // don't update last_frame, it may remain an old value.
61  }
62  }
63  catch (boost::system::system_error &e)
64  {
65  // happens when client disconnects
66  ROS_DEBUG("system_error exception: %s", e.what());
67  inactive_ = true;
68  return;
69  }
70  catch (std::exception &e)
71  {
72  ROS_ERROR_THROTTLE(30, "exception: %s", e.what());
73  inactive_ = true;
74  return;
75  }
76  catch (...)
77  {
78  ROS_ERROR_THROTTLE(30, "exception");
79  inactive_ = true;
80  return;
81  }
82 }
83 
84 void ImageTransportImageStreamer::imageCallback(const sensor_msgs::ImageConstPtr &msg)
85 {
86  if (inactive_)
87  return;
88 
89  cv::Mat img;
90  try
91  {
92  if (msg->encoding.find("F") != std::string::npos)
93  {
94  // scale floating point images
95  cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image;
96  cv::Mat_<float> float_image = float_image_bridge;
97  double max_val;
98  cv::minMaxIdx(float_image, 0, &max_val);
99 
100  if (max_val > 0)
101  {
102  float_image *= (255 / max_val);
103  }
104  img = float_image;
105  }
106  else
107  {
108  // Convert to OpenCV native BGR color
109  img = cv_bridge::toCvCopy(msg, "bgr8")->image;
110  }
111 
112  int input_width = img.cols;
113  int input_height = img.rows;
114 
115  if (output_width_ == -1)
116  output_width_ = input_width;
117  if (output_height_ == -1)
118  output_height_ = input_height;
119 
120  if (invert_)
121  {
122  // Rotate 180 degrees
123  cv::flip(img, img, false);
124  cv::flip(img, img, true);
125  }
126 
127  boost::mutex::scoped_lock lock(send_mutex_); // protects output_size_image
128  if (output_width_ != input_width || output_height_ != input_height)
129  {
130  cv::Mat img_resized;
131  cv::Size new_size(output_width_, output_height_);
132  cv::resize(img, img_resized, new_size);
133  output_size_image = img_resized;
134  }
135  else
136  {
137  output_size_image = img;
138  }
139 
140  if (!initialized_)
141  {
143  initialized_ = true;
144  }
145 
148 
149  }
150  catch (cv_bridge::Exception &e)
151  {
152  ROS_ERROR_THROTTLE(30, "cv_bridge exception: %s", e.what());
153  inactive_ = true;
154  return;
155  }
156  catch (cv::Exception &e)
157  {
158  ROS_ERROR_THROTTLE(30, "cv_bridge exception: %s", e.what());
159  inactive_ = true;
160  return;
161  }
162  catch (boost::system::system_error &e)
163  {
164  // happens when client disconnects
165  ROS_DEBUG("system_error exception: %s", e.what());
166  inactive_ = true;
167  return;
168  }
169  catch (std::exception &e)
170  {
171  ROS_ERROR_THROTTLE(30, "exception: %s", e.what());
172  inactive_ = true;
173  return;
174  }
175  catch (...)
176  {
177  ROS_ERROR_THROTTLE(30, "exception");
178  inactive_ = true;
179  return;
180  }
181 }
182 
183 }
web_video_server::ImageTransportImageStreamer::sendImage
virtual void sendImage(const cv::Mat &, const ros::Time &time)=0
ROS_ERROR_THROTTLE
#define ROS_ERROR_THROTTLE(period,...)
web_video_server::ImageTransportImageStreamer::initialize
virtual void initialize(const cv::Mat &)
Definition: image_streamer.cpp:49
web_video_server::ImageTransportImageStreamer::invert_
bool invert_
Definition: image_streamer.h:65
web_video_server::ImageStreamer
Definition: image_streamer.h:13
web_video_server::ImageTransportImageStreamer::default_transport_
std::string default_transport_
Definition: image_streamer.h:66
boost::shared_ptr< HttpConnection >
web_video_server::ImageStreamer::inactive_
bool inactive_
Definition: image_streamer.h:43
image_streamer.h
cv_bridge::Exception
ros::master::getTopics
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
web_video_server::ImageTransportImageStreamer::last_frame
ros::Time last_frame
Definition: image_streamer.h:68
web_video_server::ImageStreamer::~ImageStreamer
virtual ~ImageStreamer()
Definition: image_streamer.cpp:15
web_video_server::ImageTransportImageStreamer::restreamFrame
virtual void restreamFrame(double max_age)
Definition: image_streamer.cpp:53
web_video_server::ImageStreamer::topic_
std::string topic_
Definition: image_streamer.h:45
async_web_server_cpp::HttpRequest::has_query_param
bool has_query_param(const std::string &name) const
web_video_server::ImageTransportImageStreamer::imageCallback
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
Definition: image_streamer.cpp:84
web_video_server::ImageTransportImageStreamer::output_height_
int output_height_
Definition: image_streamer.h:64
web_video_server::ImageTransportImageStreamer::start
virtual void start()
Definition: image_streamer.cpp:33
cv_bridge::toCvCopy
CvImagePtr toCvCopy(const sensor_msgs::CompressedImage &source, const std::string &encoding=std::string())
ROS_DEBUG
#define ROS_DEBUG(...)
image_transport::ImageTransport::subscribe
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
web_video_server::ImageTransportImageStreamer::initialized_
bool initialized_
Definition: image_streamer.h:74
ros::master::V_TopicInfo
std::vector< TopicInfo > V_TopicInfo
web_video_server::ImageTransportImageStreamer::~ImageTransportImageStreamer
virtual ~ImageTransportImageStreamer()
Definition: image_streamer.cpp:29
async_web_server_cpp::HttpRequest
async_web_server_cpp::HttpRequest::get_query_param_value_or_default
std::string get_query_param_value_or_default(const std::string &name, const std::string &default_value) const
web_video_server::ImageTransportImageStreamer::output_width_
int output_width_
Definition: image_streamer.h:63
web_video_server::ImageTransportImageStreamer::image_sub_
image_transport::Subscriber image_sub_
Definition: image_streamer.h:62
web_video_server::ImageTransportImageStreamer::ImageTransportImageStreamer
ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle &nh)
Definition: image_streamer.cpp:19
web_video_server
Definition: h264_streamer.h:9
web_video_server::ImageTransportImageStreamer::it_
image_transport::ImageTransport it_
Definition: image_streamer.h:73
web_video_server::ImageTransportImageStreamer::send_mutex_
boost::mutex send_mutex_
Definition: image_streamer.h:70
cv_bridge.h
web_video_server::ImageTransportImageStreamer::output_size_image
cv::Mat output_size_image
Definition: image_streamer.h:69
web_video_server::ImageStreamer::ImageStreamer
ImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, ros::NodeHandle &nh)
Definition: image_streamer.cpp:8
image_transport::TransportHints
ros::Duration
ros::NodeHandle
ros::Time::now
static Time now()


web_video_server
Author(s): Mitchell Wills
autogenerated on Wed Mar 2 2022 01:13:35