Skip to content

Commit 77a0db1

Browse files
roehlingroehling
committed
Implement logic to resize or drop frames on demand
1 parent aa8bd94 commit 77a0db1

File tree

4 files changed

+65
-61
lines changed

4 files changed

+65
-61
lines changed

‎webrtc_ros/include/webrtc_ros/ros_video_capturer.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ class RosVideoCapturer :
2222
RosVideoCapturer(const ImageTransportFactory& it, const std::string& topic, const std::string& transport);
2323
virtual ~RosVideoCapturer();
2424

25-
void imageCallback(const std::shared_ptr<webrtc::VideoFrame>& frame);
25+
void imageCallback(const sensor_msgs::ImageConstPtr& msg);
2626

2727
virtual cricket::CaptureState Start(const cricket::VideoFormat& capture_format) override;
2828
virtual void Stop() override;

‎webrtc_ros/include/webrtc_ros/webrtc_client.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ class WebrtcClient : private boost::noncopyable
6565

6666
bool initPeerConnection();
6767

68-
void ping_timer_callback(const ros::TimerEvent&);
68+
void ping_timer_callback(const ros::WallTimerEvent&);
6969

7070
void handle_message(MessageHandler::Type type, const std::string& message);
7171

@@ -92,7 +92,7 @@ class WebrtcClient : private boost::noncopyable
9292

9393
std::map<std::string, std::map<std::string, std::string>> expected_streams_;
9494

95-
ros::Timer ping_timer_;
95+
ros::WallTimer ping_timer_;
9696

9797
friend WebrtcClientObserverProxy;
9898
friend MessageHandlerImpl;

‎webrtc_ros/src/ros_video_capturer.cpp

Lines changed: 57 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -15,17 +15,6 @@ namespace webrtc_ros
1515
RosVideoCapturer::RosVideoCapturer(const ImageTransportFactory& it, const std::string& topic, const std::string& transport)
1616
: impl_(new RosVideoCapturerImpl(it, topic, transport))
1717
{
18-
19-
// Default supported formats. Use ResetSupportedFormats to over write.
20-
std::vector<cricket::VideoFormat> formats;
21-
formats.push_back(cricket::VideoFormat(1280, 720,
22-
cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420));
23-
formats.push_back(cricket::VideoFormat(640, 480,
24-
cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420));
25-
formats.push_back(cricket::VideoFormat(320, 240,
26-
cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420));
27-
formats.push_back(cricket::VideoFormat(160, 120,
28-
cricket::VideoFormat::FpsToInterval(30), cricket::FOURCC_I420));
2918
}
3019

3120

@@ -56,14 +45,63 @@ void RosVideoCapturer::Stop()
5645
SetCaptureState(cricket::CS_STOPPED);
5746
}
5847

59-
void RosVideoCapturer::imageCallback(const std::shared_ptr<webrtc::VideoFrame>& frame)
48+
void RosVideoCapturer::imageCallback(const sensor_msgs::ImageConstPtr& msg)
6049
{
61-
// Apparently, the OnFrame() method could not be called from arbitrary threads in ancient times, and there
62-
// used to be all kinds of shenanigans here to make sure it is called from the original thread, causing
63-
// a subtle deadlock bug on object destruction.
64-
//
65-
// So I decided to be blunt and just call it like it is:
66-
OnFrame(*frame, frame->width(), frame->height());
50+
cv::Mat bgr;
51+
if (msg->encoding.find("F") != std::string::npos)
52+
{
53+
// scale floating point images
54+
cv::Mat float_image_bridge = cv_bridge::toCvShare(msg, msg->encoding)->image;
55+
cv::Mat_<float> float_image = float_image_bridge;
56+
double max_val;
57+
cv::minMaxIdx(float_image, 0, &max_val);
58+
59+
if (max_val > 0)
60+
{
61+
float_image *= (255 / max_val);
62+
}
63+
cv::Mat orig;
64+
float_image.convertTo(orig, CV_8U);
65+
cv::cvtColor(orig, bgr, CV_GRAY2BGR);
66+
}
67+
else
68+
{
69+
bgr = cv_bridge::toCvShare(msg, "bgr8")->image;
70+
}
71+
int64_t camera_time_us = msg->header.stamp.toNSec() / 1000;
72+
int64_t system_time_us = ros::WallTime::now().toNSec() / 1000;
73+
cv::Rect roi;
74+
int out_width, out_height;
75+
int64_t translated_camera_time_us;
76+
if (AdaptFrame(bgr.cols, bgr.rows, camera_time_us, system_time_us, &out_width, &out_height, &roi.width, &roi.height, &roi.x, &roi.y, &translated_camera_time_us))
77+
{
78+
cv::Mat yuv;
79+
if (out_width == roi.width && out_height == roi.height)
80+
{
81+
cv::cvtColor(bgr(roi), yuv, CV_BGR2YUV_I420);
82+
}
83+
else
84+
{
85+
cv::Mat m;
86+
cv::resize(bgr(roi), m, cv::Size2i(out_width, out_height), 0, 0, out_width < roi.width ? cv::INTER_AREA : cv::INTER_LINEAR);
87+
cv::cvtColor(m, yuv, CV_BGR2YUV_I420);
88+
}
89+
uint8_t* y = yuv.data;
90+
uint8_t* u = y + (out_width * out_height);
91+
uint8_t* v = u + (out_width * out_height) / 4;
92+
93+
std::shared_ptr<webrtc::VideoFrame> frame = std::make_shared<webrtc::VideoFrame>(
94+
webrtc::I420Buffer::Copy(out_width, out_height, y, out_width, u, out_width / 2, v, out_width / 2),
95+
webrtc::kVideoRotation_0,
96+
translated_camera_time_us
97+
);
98+
// Apparently, the OnFrame() method could not be called from arbitrary threads in ancient times, and there
99+
// used to be all kinds of shenanigans here to make sure it is called from the original thread, causing
100+
// a subtle deadlock bug on object destruction.
101+
//
102+
// So I decided to be blunt and just call it like it is:
103+
OnFrame(*frame, frame->width(), frame->height());
104+
}
67105
}
68106

69107
bool RosVideoCapturer::IsRunning()
@@ -133,42 +171,7 @@ void RosVideoCapturerImpl::imageCallback(const sensor_msgs::ImageConstPtr& msg)
133171
std::unique_lock<std::mutex> lock(state_mutex_);
134172
if(capturer_ == nullptr)
135173
return;
136-
cv::Mat bgr;
137-
if (msg->encoding.find("F") != std::string::npos)
138-
{
139-
// scale floating point images
140-
cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image;
141-
cv::Mat_<float> float_image = float_image_bridge;
142-
double max_val;
143-
cv::minMaxIdx(float_image, 0, &max_val);
144-
145-
if (max_val > 0)
146-
{
147-
float_image *= (255 / max_val);
148-
}
149-
cv::Mat orig;
150-
float_image.convertTo(orig, CV_8U);
151-
cv::cvtColor(orig, bgr, CV_GRAY2BGR);
152-
}
153-
else
154-
{
155-
bgr = cv_bridge::toCvCopy(msg, "bgr8")->image;
156-
}
157-
158-
cv::Mat yuv;
159-
cv::cvtColor(bgr, yuv, CV_BGR2YUV_I420);
160-
161-
uint8_t* y = yuv.data;
162-
uint8_t* u = y + (bgr.cols * bgr.rows);
163-
uint8_t* v = u + (bgr.cols * bgr.rows) / 4;
164-
165-
std::shared_ptr<webrtc::VideoFrame> frame = std::make_shared<webrtc::VideoFrame>(
166-
webrtc::I420Buffer::Copy(bgr.cols, bgr.rows, y, bgr.cols, u, bgr.cols / 2, v, bgr.cols / 2),
167-
msg->header.stamp.toNSec() / 1000000,
168-
msg->header.stamp.toNSec() / 1000000,
169-
webrtc::kVideoRotation_0
170-
);
171-
capturer_->imageCallback(frame);
174+
capturer_->imageCallback(msg);
172175
}
173176

174177
}

‎webrtc_ros/src/webrtc_client.cpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ WebrtcClient::WebrtcClient(ros::NodeHandle& nh, const ImageTransportFactory& itf
8080
peer_connection_constraints_.SetAllowDtlsSctpDataChannels();
8181
media_constraints_.AddOptional(webrtc::MediaConstraintsInterface::kOfferToReceiveAudio, true);
8282
media_constraints_.AddOptional(webrtc::MediaConstraintsInterface::kOfferToReceiveVideo, true);
83-
ping_timer_ = nh_.createTimer(ros::Duration(10.0), boost::bind(&WebrtcClient::ping_timer_callback, this, _1));
83+
ping_timer_ = nh_.createWallTimer(ros::WallDuration(10.0), boost::bind(&WebrtcClient::ping_timer_callback, this, _1));
8484
}
8585
WebrtcClient::~WebrtcClient()
8686
{
@@ -154,7 +154,7 @@ MessageHandler* WebrtcClient::createMessageHandler()
154154
return new MessageHandlerImpl(keep_alive_this_);
155155
}
156156

157-
void WebrtcClient::ping_timer_callback(const ros::TimerEvent& event)
157+
void WebrtcClient::ping_timer_callback(const ros::WallTimerEvent& event)
158158
{
159159
try
160160
{
@@ -213,6 +213,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string&
213213
{
214214
Json::Reader reader;
215215
Json::Value message_json;
216+
ROS_INFO("JSON: %s", raw.c_str());
216217
if (!reader.parse(raw, message_json))
217218
{
218219
ROS_WARN_STREAM("Could not parse message: " << raw);
@@ -300,7 +301,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string&
300301
stream->AddTrack(video_track);
301302
}
302303
else {
303-
ROS_WARN_STREAM("Unknwon video source type: " << video_type);
304+
ROS_WARN_STREAM("Unknown video source type: " << video_type);
304305
}
305306

306307
}
@@ -330,7 +331,7 @@ void WebrtcClient::handle_message(MessageHandler::Type type, const std::string&
330331
stream->AddTrack(audio_track);
331332
}
332333
else {
333-
ROS_WARN_STREAM("Unknwon video source type: " << audio_type);
334+
ROS_WARN_STREAM("Unknown video source type: " << audio_type);
334335
}
335336

336337
}

0 commit comments

Comments
 (0)