Skip to content

Commit

Permalink
Fix ament_uncrustify errors on humble
Browse files Browse the repository at this point in the history
  • Loading branch information
bjsowa committed Oct 2, 2024
1 parent 1c9ef2e commit f13160c
Show file tree
Hide file tree
Showing 11 changed files with 128 additions and 101 deletions.
3 changes: 2 additions & 1 deletion include/web_video_server/multipart_stream.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@ struct PendingFooter
std::weak_ptr<std::string> contents;
};

class MultipartStream {
class MultipartStream
{
public:
MultipartStream(
std::function<rclcpp::Time()> get_now,
Expand Down
4 changes: 2 additions & 2 deletions src/h264_streamer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ namespace web_video_server
H264Streamer::H264Streamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4")
: LibavStreamer(request, connection, node, "mp4", "libx264", "video/mp4")
{
/* possible quality presets:
* ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo
Expand Down Expand Up @@ -36,7 +36,7 @@ void H264Streamer::initializeEncoder()
}

H264StreamerType::H264StreamerType()
:LibavStreamerType("mp4", "libx264", "video/mp4")
: LibavStreamerType("mp4", "libx264", "video/mp4")
{
}

Expand Down
17 changes: 9 additions & 8 deletions src/image_streamer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ namespace web_video_server
ImageStreamer::ImageStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:request_(request), connection_(connection), node_(node), inactive_(false)
: request_(request), connection_(connection), node_(node), inactive_(false)
{
topic_ = request.get_query_param_value_or_default("topic", "");
}
Expand All @@ -26,7 +26,7 @@ ImageStreamer::~ImageStreamer()
ImageTransportImageStreamer::ImageTransportImageStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:ImageStreamer(request, connection, node), it_(node), initialized_(false)
: ImageStreamer(request, connection, node), it_(node), initialized_(false)
{
output_width_ = request.get_query_param_value_or_default<int>("width", -1);
output_height_ = request.get_query_param_value_or_default<int>("height", -1);
Expand All @@ -50,15 +50,16 @@ void ImageTransportImageStreamer::start()
continue;
}
auto & topic_name = topic_and_types.first;
if(topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)) {
if (topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)) {
inactive_ = false;
break;
}
}

// Get QoS profile from query parameter
RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(),
qos_profile_name_.c_str());
RCLCPP_INFO(
node_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(),
qos_profile_name_.c_str());
auto qos_profile = get_qos_profile_from_name(qos_profile_name_);
if (!qos_profile) {
qos_profile = rmw_qos_profile_default;
Expand All @@ -70,9 +71,9 @@ void ImageTransportImageStreamer::start()

// Create subscriber
image_sub_ = image_transport::create_subscription(
node_.get(), topic_,
std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1),
default_transport_, qos_profile.value());
node_.get(), topic_,
std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1),
default_transport_, qos_profile.value());
}

void ImageTransportImageStreamer::initialize(const cv::Mat &)
Expand Down
9 changes: 5 additions & 4 deletions src/jpeg_streamers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ namespace web_video_server
MjpegStreamer::MjpegStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:ImageTransportImageStreamer(request, connection, node),
: ImageTransportImageStreamer(request, connection, node),
stream_(std::bind(&rclcpp::Node::now, node), connection)
{
quality_ = request.get_query_param_value_or_default<int>("quality", 95);
Expand Down Expand Up @@ -53,7 +53,7 @@ JpegSnapshotStreamer::JpegSnapshotStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection,
rclcpp::Node::SharedPtr node)
:ImageTransportImageStreamer(request, connection, node)
: ImageTransportImageStreamer(request, connection, node)
{
quality_ = request.get_query_param_value_or_default<int>("quality", 95);
}
Expand All @@ -78,8 +78,9 @@ void JpegSnapshotStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & t
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(
"Cache-Control",
"no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0")
.header("X-Timestamp", stamp)
.header("Pragma", "no-cache")
.header("Content-type", "image/jpeg")
Expand Down
56 changes: 31 additions & 25 deletions src/libav_streamer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ LibavStreamer::LibavStreamer(
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node,
const std::string & format_name, const std::string & codec_name,
const std::string & content_type)
:ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0),
: ImageTransportImageStreamer(request, connection, node), format_context_(0), codec_(0),
codec_context_(0), video_stream_(0), frame_(0), sws_context_(0), first_image_timestamp_(0),
format_name_(format_name), codec_name_(codec_name), content_type_(content_type), opt_(0),
io_buffer_(0)
Expand Down Expand Up @@ -63,25 +63,26 @@ void LibavStreamer::initialize(const cv::Mat & img)
format_context_ = avformat_alloc_context();
if (!format_context_) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error allocating ffmpeg format context");
}

format_context_->oformat = av_guess_format(format_name_.c_str(), NULL, NULL);
if (!format_context_->oformat) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error looking up output format");
}

// Set up custom IO callback.
size_t io_buffer_size = 3 * 1024; // 3M seen elsewhere and adjudged good
io_buffer_ = new unsigned char[io_buffer_size];
AVIOContext * io_ctx = avio_alloc_context(io_buffer_, io_buffer_size, AVIO_FLAG_WRITE,
&connection_, NULL, dispatch_output_packet, NULL);
AVIOContext * io_ctx = avio_alloc_context(
io_buffer_, io_buffer_size, AVIO_FLAG_WRITE,
&connection_, NULL, dispatch_output_packet, NULL);
if (!io_ctx) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error setting up IO context");
}
io_ctx->seekable = 0; // no seeking, it's a stream
Expand All @@ -96,13 +97,13 @@ void LibavStreamer::initialize(const cv::Mat & img)
}
if (!codec_) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error looking up codec");
}
video_stream_ = avformat_new_stream(format_context_, codec_);
if (!video_stream_) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error creating video stream");
}

Expand Down Expand Up @@ -138,15 +139,16 @@ void LibavStreamer::initialize(const cv::Mat & img)
// Open Codec
if (avcodec_open2(codec_context_, codec_, NULL) < 0) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Could not open video codec");
}

// Allocate frame buffers
frame_ = av_frame_alloc();

av_image_alloc(frame_->data, frame_->linesize, output_width_, output_height_,
codec_context_->pix_fmt, 1);
av_image_alloc(
frame_->data, frame_->linesize, output_width_, output_height_,
codec_context_->pix_fmt, 1);

frame_->width = output_width_;
frame_->height = output_height_;
Expand All @@ -160,8 +162,9 @@ void LibavStreamer::initialize(const cv::Mat & img)
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(
"Cache-Control",
"no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0")
.header("Pragma", "no-cache")
.header("Expires", "0")
.header("Max-Age", "0")
Expand All @@ -173,7 +176,7 @@ void LibavStreamer::initialize(const cv::Mat & img)
// Send video stream header
if (avformat_write_header(format_context_, &opt_) < 0) {
async_web_server_cpp::HttpReply::stock_reply(
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
async_web_server_cpp::HttpReply::internal_server_error)(request_, connection_, NULL, NULL);
throw std::runtime_error("Error openning dynamic buffer");
}
}
Expand All @@ -191,24 +194,27 @@ void LibavStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time)

AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24;

AVFrame *raw_frame = av_frame_alloc();
av_image_fill_arrays(raw_frame->data, raw_frame->linesize,
img.data, input_coding_format, output_width_, output_height_, 1);
AVFrame * raw_frame = av_frame_alloc();
av_image_fill_arrays(
raw_frame->data, raw_frame->linesize,
img.data, input_coding_format, output_width_, output_height_, 1);

// Convert from opencv to libav
if (!sws_context_) {
static int sws_flags = SWS_BICUBIC;
sws_context_ = sws_getContext(output_width_, output_height_, input_coding_format, output_width_,
output_height_, codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL);
sws_context_ = sws_getContext(
output_width_, output_height_, input_coding_format, output_width_,
output_height_, codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL);
if (!sws_context_) {
throw std::runtime_error("Could not initialize the conversion context");
}
}


int ret = sws_scale(sws_context_,
(const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0,
output_height_, frame_->data, frame_->linesize);
int ret = sws_scale(
sws_context_,
(const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0,
output_height_, frame_->data, frame_->linesize);

av_frame_free(&raw_frame);

Expand Down Expand Up @@ -236,7 +242,7 @@ void LibavStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time)

if (got_packet) {
std::size_t size;
uint8_t *output_buf;
uint8_t * output_buf;

double seconds = (time - first_image_timestamp_).seconds();
// Encode video at 1/0.95 to minimize delay
Expand All @@ -263,7 +269,7 @@ void LibavStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & time)
LibavStreamerType::LibavStreamerType(
const std::string & format_name, const std::string & codec_name,
const std::string & content_type)
:format_name_(format_name), codec_name_(codec_name), content_type_(content_type)
: format_name_(format_name), codec_name_(codec_name), content_type_(content_type)
{
}

Expand All @@ -273,7 +279,7 @@ boost::shared_ptr<ImageStreamer> LibavStreamerType::create_streamer(
rclcpp::Node::SharedPtr node)
{
return boost::shared_ptr<ImageStreamer>(
new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_));
new LibavStreamer(request, connection, node, format_name_, codec_name_, content_type_));
}

std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest & request)
Expand Down
8 changes: 5 additions & 3 deletions src/multipart_stream.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,9 @@ 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(
"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", "*")
Expand All @@ -37,7 +38,8 @@ void MultipartStream::sendPartHeader(
headers->push_back(async_web_server_cpp::HttpHeader("Content-type", type));
headers->push_back(async_web_server_cpp::HttpHeader("X-Timestamp", stamp));
headers->push_back(
async_web_server_cpp::HttpHeader("Content-Length",
async_web_server_cpp::HttpHeader(
"Content-Length",
boost::lexical_cast<std::string>(payload_size)));
connection_->write(async_web_server_cpp::HttpReply::to_buffers(*headers), headers);
}
Expand Down
9 changes: 5 additions & 4 deletions src/png_streamers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ namespace web_video_server
PngStreamer::PngStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:ImageTransportImageStreamer(request, connection, node),
: ImageTransportImageStreamer(request, connection, node),
stream_(std::bind(&rclcpp::Node::now, node), connection)
{
quality_ = request.get_query_param_value_or_default<int>("quality", 3);
Expand Down Expand Up @@ -53,7 +53,7 @@ PngSnapshotStreamer::PngSnapshotStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection,
rclcpp::Node::SharedPtr node)
:ImageTransportImageStreamer(request, connection, node)
: ImageTransportImageStreamer(request, connection, node)
{
quality_ = request.get_query_param_value_or_default<int>("quality", 3);
}
Expand All @@ -78,8 +78,9 @@ void PngSnapshotStreamer::sendImage(const cv::Mat & img, const rclcpp::Time & ti
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(
"Cache-Control",
"no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0")
.header("X-Timestamp", stamp)
.header("Pragma", "no-cache")
.header("Content-type", "image/png")
Expand Down
25 changes: 14 additions & 11 deletions src/ros_compressed_streamer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ namespace web_video_server
RosCompressedStreamer::RosCompressedStreamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:ImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection)
: ImageStreamer(request, connection, node), stream_(std::bind(&rclcpp::Node::now, node), connection)
{
stream_.sendInitialHeader();
qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default");
Expand All @@ -23,8 +23,9 @@ void RosCompressedStreamer::start()
const std::string compressed_topic = topic_ + "/compressed";

// Get QoS profile from query parameter
RCLCPP_INFO(node_->get_logger(), "Streaming topic %s with QoS profile %s",
compressed_topic.c_str(), qos_profile_name_.c_str());
RCLCPP_INFO(
node_->get_logger(), "Streaming topic %s with QoS profile %s",
compressed_topic.c_str(), qos_profile_name_.c_str());
auto qos_profile = get_qos_profile_from_name(qos_profile_name_);
if (!qos_profile) {
qos_profile = rmw_qos_profile_default;
Expand All @@ -35,11 +36,12 @@ void RosCompressedStreamer::start()
}

// Create subscriber
const auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.value().history, 1),
qos_profile.value());
const auto qos = rclcpp::QoS(
rclcpp::QoSInitialization(qos_profile.value().history, 1),
qos_profile.value());
image_sub_ = node_->create_subscription<sensor_msgs::msg::CompressedImage>(
compressed_topic, qos,
std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1));
compressed_topic, qos,
std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1));
}

void RosCompressedStreamer::restreamFrame(double max_age)
Expand All @@ -60,13 +62,14 @@ void RosCompressedStreamer::sendImage(
{
try {
std::string content_type;
if(msg->format.find("jpeg") != std::string::npos) {
if (msg->format.find("jpeg") != std::string::npos) {
content_type = "image/jpeg";
} else if(msg->format.find("png") != std::string::npos) {
} else if (msg->format.find("png") != std::string::npos) {
content_type = "image/png";
} else {
RCLCPP_WARN(node_->get_logger(), "Unknown ROS compressed image format: %s",
msg->format.c_str());
RCLCPP_WARN(
node_->get_logger(), "Unknown ROS compressed image format: %s",
msg->format.c_str());
return;
}

Expand Down
4 changes: 2 additions & 2 deletions src/vp8_streamer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace web_video_server
Vp8Streamer::Vp8Streamer(
const async_web_server_cpp::HttpRequest & request,
async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr node)
:LibavStreamer(request, connection, node, "webm", "libvpx", "video/webm")
: LibavStreamer(request, connection, node, "webm", "libvpx", "video/webm")
{
quality_ = request.get_query_param_value_or_default("quality", "realtime");
}
Expand Down Expand Up @@ -77,7 +77,7 @@ void Vp8Streamer::initializeEncoder()
}

Vp8StreamerType::Vp8StreamerType()
:LibavStreamerType("webm", "libvpx", "video/webm")
: LibavStreamerType("webm", "libvpx", "video/webm")
{
}

Expand Down
Loading

0 comments on commit f13160c

Please sign in to comment.