ros2发布和订阅C++模板

• 新建工作空间和包

source /opt/ros/humble/setup.bash
mkdir ws/src -p
cd ws/src
ros2 pkg create --build-type ament_cmake pub_img

• 以图像为例
• 发布

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

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

class Pub : public rclcpp::Node
{
public:
  Pub(): Node("pub")
  {
    publisher_ = this->create_publisher<sensor_msgs::msg::Image>("img", 10);
    cv::Mat src = cv::imread("0001.jpg");
    std_msgs::msg::Header header;
    header.frame_id = "hello";
    sensor_msgs::msg::Image ros_image;
    cv_bridge::CvImage(header, "bgr8", src).toImageMsg(ros_image);
    // 2 images per second
    rclcpp::WallRate loop_rate(2);
    while(rclcpp::ok()) {
    	publisher_->publish(ros_image);
    	loop_rate.sleep();
    }
  }

private:
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Pub>());
  rclcpp::shutdown();
  return 0;
}

• 订阅

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
using std::placeholders::_1;

class Subs : public rclcpp::Node
{
  public:
    Subs(): Node("subs")
    {
      subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
      "img", 1, std::bind(&Subs::callback, this, _1));
    }

  private:
    void callback(const sensor_msgs::msg::Image::SharedPtr msg) const
    {
      RCLCPP_INFO(this->get_logger(), "get: '%s'", msg->header.frame_id.c_str());
    }
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Subs>());
  rclcpp::shutdown();
  return 0;
}

• CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(pub_img)

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

set(OpenCV_DIR /usr/share/OpenCV/)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(OpenCV REQUIRED)
find_package(ament_index_cpp REQUIRED)

find_package(OpenCV REQUIRED)
message(STATUS "Find OpenCV include at ${OpenCV_INCLUDE_DIRS}")
message(STATUS "Find OpenCV libraries: ${OpenCV_LIBRARIES}")

include_directories(
  include
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(subs
	${PROJECT_SOURCE_DIR}/src/sub.cc
)
ament_target_dependencies(subs rclcpp sensor_msgs ament_index_cpp)

add_executable(pub
	${PROJECT_SOURCE_DIR}/src/pub.cc
)
ament_target_dependencies(pub rclcpp cv_bridge sensor_msgs ament_index_cpp)
target_link_libraries(pub 
  ${OpenCV_LIBRARIES}
)

install(TARGETS
  subs pub
  DESTINATION lib/${PROJECT_NAME}
)
  
#install(DIRECTORY 
#  config
#  DESTINATION share/${PROJECT_NAME}/
#)

ament_package()

• 编译运行

colcon build --packages-select pub_img
source install/setup.bash
ros2 run pub_img pub
ros2 run pub_img subs