Program Listing for File camera_node.hpp
↰ Return to documentation for file (/tmp/ws/src/demos/intra_process_demo/include/image_pipeline/camera_node.hpp
)
// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef IMAGE_PIPELINE__CAMERA_NODE_HPP_
#define IMAGE_PIPELINE__CAMERA_NODE_HPP_
#include <chrono>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include "opencv2/highgui/highgui.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "common.hpp"
class CameraNode final : public rclcpp::Node
{
public:
explicit CameraNode(
const std::string & output, const std::string & node_name = "camera_node",
bool watermark = true, int device = 0, int width = 320, int height = 240)
: Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(true)),
canceled_(false), watermark_(watermark)
{
// Initialize OpenCV
cap_.open(device);
// TODO(jacobperron): Remove pre-compiler check when we drop support for Xenial
#if CV_MAJOR_VERSION < 3
cap_.set(CV_CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(CV_CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#else
cap_.set(cv::CAP_PROP_FRAME_WIDTH, static_cast<double>(width));
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, static_cast<double>(height));
#endif
if (!cap_.isOpened()) {
throw std::runtime_error("Could not open video stream!");
}
// Create a publisher on the output topic.
pub_ = this->create_publisher<sensor_msgs::msg::Image>(output, rclcpp::SensorDataQoS());
// Create the camera reading loop.
thread_ = std::thread([this]() {return this->loop();});
}
~CameraNode()
{
// Make sure to join the thread on shutdown.
canceled_.store(true);
if (thread_.joinable()) {
thread_.join();
}
}
void loop()
{
// While running...
while (rclcpp::ok() && !canceled_.load()) {
// Capture a frame from OpenCV.
cap_ >> frame_;
if (frame_.empty()) {
continue;
}
// Create a new unique_ptr to an Image message for storage.
sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
if (watermark_) {
std::stringstream ss;
// Put this process's id and the msg's pointer address on the image.
ss << "pid: " << GETPID() << ", ptr: " << msg.get();
draw_on_image(frame_, ss.str(), 20);
}
// Pack the OpenCV image into the ROS image.
set_now(msg->header.stamp);
msg->header.frame_id = "camera_frame";
msg->height = frame_.rows;
msg->width = frame_.cols;
msg->encoding = mat_type2encoding(frame_.type());
msg->is_bigendian = false;
msg->step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
msg->data.assign(frame_.datastart, frame_.dataend);
pub_->publish(std::move(msg)); // Publish.
}
}
private:
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_;
std::thread thread_;
std::atomic<bool> canceled_;
bool watermark_;
cv::VideoCapture cap_;
cv::Mat frame_;
};
#endif // IMAGE_PIPELINE__CAMERA_NODE_HPP_