ROS2——realsense话题订阅_ros2 realsense

realsense-ros安装

https://github.com/IntelRealSense/realsense-ros

安装完成后，进行检查

ros2 launch realsense2_camera rs_launch.py
1

在新的terminal中输入

ros2 topic list
1

出现上面的结果，表示安装成功

C++

创建功能包

ros2 pkg create --build-type ament_cmake --dependencies rclcpp std_msgs cv_bridge
1

rclcpp订阅

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/string.hpp>

using std::placeholders::_1;
cv::Mat color_mat, res_mat;


class RealsenseSubscriber: public rclcpp::Node
{
    public:
       RealsenseSubscriber()
       : Node("realsense_subscriber")
       {
            auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
            subImg = this->create_subscription<sensor_msgs::msg::Image>(
                            "/camera/color/image_raw", default_qos, std::bind(&RealsenseSubscriber::imgCallBack, this, _1));
       } 
    
    private:
        void imgCallBack(sensor_msgs::msg::Image::SharedPtr msg)
        {
            if(!rclcpp::ok())
                return;
            cv_bridge::CvImagePtr cv_ptr;
            cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding);
            color_mat = cv_ptr->image;
            printf("subscribe image sucessfully!!!");
        }
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subImg;
};

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(orchard_object_detection)

# 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()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(OpenCV REQUIRED)
find_package(cv_bridge 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()

add_executable(object_detection ./src/main.cpp)
target_link_libraries(object_detection
  ${opencv_libraries}
)

ament_target_dependencies(object_detection rclcpp std_msgs cv_bridge)

install(TARGETS
  object_detection
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

Python

创建工作空间

mkdir -p ros2_ws/src
cd ros2_ws/src
1
2

创建功能包

在src文件夹内进行

ros2 pkg create yolo_python --build-type ament_python --dependencies rclpy cv_bridge
1

编写订阅逻辑代码

import rclpyfrom rclpy.node
import Nodefrom cv_bridge 
import CvBridge
from sensor_msgs.msg 
import Imageimport cv2
class ImageSubscriber(Node):    
	def __init__(self):        
		# initiate the Node class's constructor and give it a name        
		super().__init__("image_subsriber")        
		# Create the subscriber. This subsciber will receive an image        
		self.subscription = self.create_subscription(Image, "/camera/color/image_raw", self.image_callback, 10)        
		self.subscription # prevent unused variable warning        
		self.br = CvBridge()    

	def image_callback(self, data):        
		self.get_logger().info("Receiving realsense frame")        
		current_img = self.br.imgmsg_to_cv2(data)        
		cv2.imshow("camera", current_img)        
		cv2.waitKey(1)

def main(args=None):    
	rclpy.init(args=args)    
	yolo_node = ImageSubscriber()    
	# yolo_node.get_logger().info("print inof")   
	rclpy.spin(yolo_node)    
	rclpy.shutdown()
		
if __name__ == "__main__":       
	 main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

配置