ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to initialize image_transport using rclcpp

asked 2022-07-20 02:34:25 -0600

kyubot gravatar image

updated 2022-07-20 02:35:50 -0600

I am writing a camera node to publish compressed image whenever new image captured from a camera. To do this, I realized that I need to initialize image_transport with node handle however I just don't know how to do that.

Here is the code inside a class

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "image_transport/image_transport.hpp"

class CamPublisher_: public rclcpp::Node
{
  public:
    CamPublisher_()
    : Node("camera")  {
        setvbuf(stdout, NULL, _IONBF, BUFSIZ);
        initialize();
    }
    ~CamPublisher_() {
        cap.release();
    }
  private:
void initialize()
    {
        topic_image = "/camera/raw";
        topic_compressed = "/camera/compressed";
        auto qos = rclcpp::QoS(rclcpp::KeepLast(10));
        pub_image = this->create_publisher<sensor_msgs::msg::Image>(
            topic_image, qos
        );
        // pub_compressed = this->create_publisher<sensor_msgs::msg::CompressedImage>(
        //  topic_compressed, qos
        // );
        pub_transport = it.advertise(topic_compressed, 10);

        cap.open(0);

        if (!cap.isOpened()) {
            RCLCPP_ERROR(this->get_logger(), "Could not open video stream");
            throw std::runtime_error("Could not open video stream");
        }
        cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
        cap.set(cv::CAP_PROP_FRAME_HEIGHT, 640);

        timer_ = this->create_wall_timer(
                std::chrono::milliseconds(static_cast<int>(1000 / 30)), // 30 fps
                std::bind(&CamPublisher_::timerCallback, this));
    }

    void timerCallback()
    {
        cv::Mat frame;
        cap >> frame;
        if (frame.empty()) {
            return;
        }
        convert_and_publish(frame);
    }

    std::string mat2encoding(int mat_type)
    {
        switch (mat_type) {
            case CV_8UC1:
                return "mono8";
            case CV_8UC3:
                return "bgr8";
            case CV_16SC1:
                return "mono16";
            case CV_8UC4:
                return "rgba8";
            default:
                std::runtime_error("Unsupported encoding type");
        }
        return 0;
    }

    void convert_and_publish(const cv::Mat& frame)
    {
        auto msg = sensor_msgs::msg::Image();
        msg.height = frame.rows;
        msg.width = frame.cols;
        msg.encoding = mat2encoding(std::move(frame.type()));
        msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);

        size_t size = frame.step * frame.rows;
        msg.data.resize(size);
        memcpy(&msg.data[0], frame.data, size);

        pub_image->publish(msg);
        sensor_msgs::msg::CompressedImage img_compressed_msg;
    }

    //vars
    cv::VideoCapture cap;

    std::string topic_image;
    std::string topic_compressed;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_image;
    //rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr pub_compressed;
    image_transport::ImageTransport it;
    image_transport::Publisher pub_transport;
    rclcpp::TimerBase::SharedPtr timer_;
};

Here is the error when I tried colcon build

ros2_ws/src/stream/src/camera.cpp: In constructor ‘CamPublisher_::CamPublisher_()’:
ros2_ws/src/stream/src/camera.cpp:27:20: error: no matching function for call to ‘image_transport::ImageTransport::ImageTransport()’
   27 |     : Node("camera")
      |                    ^
In file included from ros2_ws/src/stream/src/camera.cpp:18:
/opt/ros/foxy/include/image_transport/image_transport.hpp:116:12: note: candidate: ‘image_transport::ImageTransport::ImageTransport(rclcpp::Node::SharedPtr)’
  116 |   explicit ImageTransport(rclcpp::Node::SharedPtr node);
      |            ^~~~~~~~~~~~~~
/opt/ros/foxy/include/image_transport/image_transport.hpp:116:12: note:   candidate expects 1 argument, 0 provided
make[2]: *** [CMakeFiles/camera.dir/build.make:63: CMakeFiles/camera.dir/src/camera.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:107: CMakeFiles/camera.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2022-08-09 19:33:03 -0600

ChuiV gravatar image

With image transport, you don't actually have to manage all the publishers yourself. The ImageTransport object will do that for you. As for your question though... You can create a shared pointer with an empty deleter function to be able to pass to Image Transport:

class CamPublisher_ : public rclcpp::Node
{
protected:
    rclcpp::Node::SharedPtr node_handle_;
    image_transport::ImageTransport image_transport_;
    image_transport::Publisher image_publisher_;
    ...
public:
    CamPublisher_() :
            Node("camera"),
            // create the shared_ptr node handle for this node.
            node_handle_(std::shared_ptr<CamPublisher_>(this, [](auto *) {})),
            image_transport_(node_handle_),
            image_publisher_(image_transport.advertise("image", 10)
    {
        ....
    }
    void convert_and_publish(const cv::Mat& frame)
    {
        sensor_msgs::msg::Image msg;
        ....
        image_publisher_->publish(msg);
    }
}
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-07-20 02:34:25 -0600

Seen: 734 times

Last updated: Aug 09 '22