ros2 发布订阅image和PointCloud2

目录

一.通过cv_bridge实现ros图像消息与opencv图像之间的转换

1.从cv::mat ==> cv_bridge::CvImage ==> sensor_msgs::msg::Image

2.从sensor_msgs::msg::Image::SharedPtr ==> cv_bridge::CvImagePtr ==> cv::Mat 

二.pcl点云与ros2点云消息之间的转换

1.从pcl::PointCloud cloud ==> sensor_msgs::msg::PointCloud2 output_msg;

2.从sensor_msgs::msg::PointCloud2::SharedPtr==> pcl::PointCloud::Ptr;

三.发布

四. 订阅

---

一.通过cv_bridge实现ros图像消息与opencv图像之间的转换

1.从cv::mat ==> cv_bridge::CvImage ==> sensor_msgs::msg::Image

cv::Mat cv_rgb(IMAGE_HEIGHT_480, IMAGE_WIDTH_640, CV_8UC3);
dabai->get_dabai_stream_data(cv_rgb,aligned_depth);
cv_bridge::CvImage cvi_rgb;
cvi_rgb.header.stamp = this->get_clock()->now(); //获取当前时间戳
cvi_rgb.header.frame_id = "dabai_rgb"; //标记图像id类，用户订阅时区分不同的图像
cvi_rgb.encoding = "bgr8";
cvi_rgb.image = cv_rgb;
sensor_msgs::msg::Image im_rgb;
cvi_rgb.toImageMsg(im_rgb);

2.从sensor_msgs::msg::Image::SharedPtr ==> cv_bridge::CvImagePtr ==> cv::Mat 

void topic_rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg) const
{
    cv::Mat img;
    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
    cv_ptr->image.copyTo(img);
        
    cv::imshow("myHello",img);
    cv::waitKey(3);
}

二.pcl点云与ros2点云消息之间的转换

1.从pcl::PointCloud cloud ==> sensor_msgs::msg::PointCloud2 output_msg;

        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::PointCloud cloud;
        // 点云初始化

        int num_points = aligned_depth.rows * aligned_depth.cols;
        cloud.points.resize(num_points);
        for(int i = 0; i < aligned_depth.rows; i++)
        {
            for(int j = 0; j< aligned_depth.cols; j++)
            {
                float depth = (float)aligned_depth.at(i,j);

                float tx = (j - RGB_U0) / RGB_FX;
                float ty = (i - RGB_V0) / RGB_FY;
                int index = aligned_depth.cols * i + j;
                cloud.points[index].x = depth * tx/1000;
                cloud.points[index].y = depth * ty/1000;
                cloud.points[index].z = depth/1000;
                cloud.points[index].r = 0;
                cloud.points[index].g = 255;
                cloud.points[index].b = 0;
                
            }
        }

        //将pcl点云转换到ros消息对象
        pcl::toROSMsg(cloud, output_msg);
    
        // 发布的点云坐标系
        output_msg.header.frame_id="dabai_points";
        output_msg.header.stamp = this->get_clock()->now()

2.从sensor_msgs::msg::PointCloud2::SharedPtr==> pcl::PointCloud::Ptr;

    void topic_points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        pcl::PointCloud::Ptr cloud =
        pcl::make_shared>();
        pcl::fromROSMsg(*msg, *cloud);
        pcl::io::savePCDFileASCII ("asd.pcd", *cloud);
        RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
        
    }

三.发布

#include 
#include 
#include 
#include "dabai.h"

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"
#include 

#include 
#include 
#include "pcl_conversions/pcl_conversions.h"

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 


using namespace std;
using namespace std::chrono_literals;

class CameraPublisher : public rclcpp::Node
{
private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher::SharedPtr mPublisherDabaiRgb;
    rclcpp::Publisher::SharedPtr mPublisherDabaiPoints;

    size_t count;
    Draw mDraw;
    DaBai *dabai;
    
    void timer_callback()
    {
        cv::Mat cv_rgb(IMAGE_HEIGHT_480, IMAGE_WIDTH_640, CV_8UC3);
        cv::Mat aligned_depth(IMAGE_HEIGHT_480, IMAGE_WIDTH_640, CV_16UC1);
        cv::Mat CaliDepthHistogram(IMAGE_HEIGHT_480, IMAGE_WIDTH_640, CV_16UC1);     //分配的内存和aligned_depth一样

        dabai->get_dabai_stream_data(cv_rgb,aligned_depth);

        //发布rgb彩色图像
        cv_bridge::CvImage cvi_rgb;
        cvi_rgb.header.stamp = this->get_clock()->now();
        cvi_rgb.header.frame_id = "dabai_rgb";
        cvi_rgb.encoding = "bgr8";
        cvi_rgb.image = cv_rgb;
        sensor_msgs::msg::Image im_rgb;
        cvi_rgb.toImageMsg(im_rgb);
        printf("im.width = %d\n", im_rgb.width);
        mPublisherDabaiRgb->publish(im_rgb);

        //发布点云图像

        mDraw.GetDepthHistogram(aligned_depth, CaliDepthHistogram);
        Mat imgROI = cv_rgb(Rect(0, 0, CaliDepthHistogram.cols, CaliDepthHistogram.rows));
        cv::addWeighted(imgROI, 0.5, CaliDepthHistogram, 0.5, 0.0, imgROI);
        cv::imshow("hello",cv_rgb);
        		//按ESC，退出程序
		int c = cv::waitKey(3);

        // //发布rgbd彩色深度混合图像
        // cv_bridge::CvImage cvi_rgbd;
        // cvi_rgbd.header.stamp = this->get_clock()->now();
        // cvi_rgbd.header.frame_id = "dabai_rgbd";
        // cvi_rgbd.encoding = "bgr8";
        // cvi_rgbd.image = imgROI;
        // sensor_msgs::msg::Image im_rgbd;
        // cvi_rgbd.toImageMsg(im_rgbd);
        // printf("im.width = %d\n", im_rgbd.width);
        // mPublisherDabaiRgb->publish(im_rgbd);


        //发布点云数据
        //建立ros点云
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::PointCloud cloud;
        // 点云初始化

        int num_points = aligned_depth.rows * aligned_depth.cols;
        cloud.points.resize(num_points);
        for(int i = 0; i < aligned_depth.rows; i++)
        {
            for(int j = 0; j< aligned_depth.cols; j++)
            {
                float depth = (float)aligned_depth.at(i,j);

                float tx = (j - RGB_U0) / RGB_FX;
                float ty = (i - RGB_V0) / RGB_FY;
                int index = aligned_depth.cols * i + j;
                cloud.points[index].x = depth * tx/1000;
                cloud.points[index].y = depth * ty/1000;
                cloud.points[index].z = depth/1000;
                cloud.points[index].r = 0;
                cloud.points[index].g = 255;
                cloud.points[index].b = 0;
                
            }
        }

        //将pcl点云转换到ros消息对象
        pcl::toROSMsg(cloud, output_msg);
    
        // 发布的点云坐标系
        output_msg.header.frame_id="dabai_points";
        output_msg.header.stamp = this->get_clock()->now();
        mPublisherDabaiPoints->publish(output_msg);


    }
public:
    CameraPublisher() : Node("node_dabai_cam"), count(0)
    {
        dabai = new DaBai();
        mPublisherDabaiRgb = this->create_publisher("topic_dabai_image", 10);
        mPublisherDabaiPoints = this->create_publisher("topic_dabai_cloud", 10);
        timer_ = this->create_wall_timer(
        200ms, std::bind(&CameraPublisher::timer_callback, this));
    }
    ~CameraPublisher(){
        delete dabai;
    }

};


int main(int argc, char *argv[])
{

    rclcpp::init(argc, argv);           //ros2节点主入口main函数

    std::shared_ptr node = std::make_shared();
    
    rclcpp::spin(node);  //循环等待ros2推出

    // rclcpp::spin(std::make_shared());

    rclcpp::shutdown();             //关闭ros2

    return 0;
}

cmake_minimum_required(VERSION 3.5)
set(TARGET dabai_cam)
project(${TARGET})

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

add_definitions(-DBOOST_ERROR_CODE_HEADER_ONLY)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED) 
find_package(cv_bridge REQUIRED) 
find_package(pcl_conversions REQUIRED) 

INCLUDE_DIRECTORIES(
    ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/d2c/Include
    ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/OpenNI2/Include
    ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/UvcSwapper/Include
    ${CMAKE_CURRENT_SOURCE_DIR}/include/dabai_cam/
    /usr/include/pcl-1.10/
    /usr/include/boost/
    /usr/include/eigen3/

)

LINK_DIRECTORIES(
                ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/OpenNI2/linux/x64
                ${PCL_LIBRARY_DIRS}
                /usr/lib/robot-x64/
)


file(GLOB robot_sources
    ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
)

    
add_executable(
      ${TARGET}
      ${robot_sources}
)

ament_target_dependencies(${TARGET} rclcpp std_msgs sensor_msgs cv_bridge)      
target_link_libraries(${TARGET}
      uvc Uvc-Swapper OpenNI2 d2c
      opencv_core opencv_highgui opencv_imgcodecs opencv_imgproc opencv_videoio
)


# uncomment the following section in order to fill in
# further dependencies manually.
# find_package( REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

# install(TARGETS

#     ${TARGET}
#     DESTINATION lib/${PROJECT_NAME}
# )

ament_package()

四. 订阅

#include 

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"
#include 

#include 
#include 
#include 
#include 

#include 
#include 


#include "sensor_msgs/msg/point_cloud2.hpp"
#include 
#include 
#include 
#include 
#include 

class CameraSubscriber : public rclcpp::Node
{
	
private:
    rclcpp::Subscription::SharedPtr mSubscriptionRgb;                  //定义image对应的订阅指针
    rclcpp::Subscription::SharedPtr mSubscriptionDabaiPoints;    //定义PointCloud2对应的订阅指针
    void topic_rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg) const
    {
        cv::Mat img;
        cv_bridge::CvImagePtr cv_ptr;
        cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
        cv_ptr->image.copyTo(img);
        
        cv::imshow("myHello",img);
        cv::waitKey(3);
    }
    void topic_points_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        pcl::PointCloud::Ptr cloud =
        pcl::make_shared>();
        pcl::fromROSMsg(*msg, *cloud);
        pcl::io::savePCDFileASCII ("asd.pcd", *cloud);


        RCLCPP_INFO(this->get_logger(), "points_size(%d,%d)",msg->height,msg->width);
        
    }
public:
    CameraSubscriber(std::string name) : Node(name)
    {
        //创建image话题
        mSubscriptionRgb = this->create_subscription("topic_dabai_image", 10, 
        std::bind(&CameraSubscriber::topic_rgb_callback, this, std::placeholders::_1));
        mSubscriptionDabaiPoints = this->create_subscription("topic_dabai_cloud", 10, 
        std::bind(&CameraSubscriber::topic_points_callback, this, std::placeholders::_1));
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared("robot_analyse");
    rclcpp::spin(node);
    
    rclcpp::shutdown();
    return 0;
}

cmake_minimum_required(VERSION 3.5)
set(TARGET robot_analyse)
project(${TARGET})

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fPIC -O3 -fopenmp -lgomp -lpthread")

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED) 
find_package(cv_bridge REQUIRED) 
find_package(pcl_conversions REQUIRED) 
find_package(PCL REQUIRED COMPONENTS common io)

add_definitions(${PCL_DEFINITIONS})

# find_package(boost REQUIRED) 

INCLUDE_DIRECTORIES(

    /usr/include/pcl-1.10/
    /usr/include/boost/
    /usr/include/eigen3/
    ${CMAKE_CURRENT_SOURCE_DIR}/include/
    ${CMAKE_CURRENT_SOURCE_DIR}/src/yolov5/
    ${CMAKE_CURRENT_SOURCE_DIR}/src/pcl/
    ${CMAKE_CURRENT_SOURCE_DIR}/src/locate/
    ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/ncnn/include/

)

LINK_DIRECTORIES(

    ${PCL_LIBRARY_DIRS}
    ${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/ncnn/lib/

)

file(GLOB robot_sources
    ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/src/yolov5/*.h
    ${CMAKE_CURRENT_SOURCE_DIR}/src/yolov5/*.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/src/locate/*.h
    ${CMAKE_CURRENT_SOURCE_DIR}/src/locate/*.cpp
    ${CMAKE_CURRENT_SOURCE_DIR}/src/pcl/*.h
    ${CMAKE_CURRENT_SOURCE_DIR}/src/pcl/*.cpp
)

    
add_executable(
      ${TARGET}
      ${robot_sources}
)

file(GLOB PCL_LIBRARIES /usr/lib/x86_64-linux-gnu/libpcl_*)

ament_target_dependencies(${TARGET} rclcpp std_msgs sensor_msgs cv_bridge)      
target_link_libraries(${TARGET}
    ncnn
    opencv_core opencv_highgui opencv_imgcodecs opencv_imgproc opencv_videoio
    ${PCL_LIBRARIES}

)

install(TARGETS

    ${TARGET}
    DESTINATION lib/${PROJECT_NAME}
)

# uncomment the following section in order to fill in
# further dependencies manually.
# find_package( REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()