Skip to content

Commit f0e9c81

Browse files
linyicheng1linyicheng1
authored
添加对opencv读取video设备的简单示例 #20 
1 parent 1e6cfa7 commit f0e9c81

File tree

8 files changed

+490
-0
lines changed

8 files changed

+490
-0
lines changed

‎example/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,4 +2,5 @@ cmake_minimum_required(VERSION 3.10)
22
project(example)
33

44
add_subdirectory(NetCam)
5+
add_subdirectory(VideoCam)
56
add_subdirectory(CustomCam)

‎example/VideoCam/CMakeLists.txt‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
3+
project(video_cam VERSION 1.0)
4+
message(STATUS "System Processor: ${CMAKE_SYSTEM_PROCESSOR}")
5+
6+
find_package(Threads REQUIRED)
7+
find_package(OpenCV REQUIRED)
8+
if(NOT OpenCV_FOUND)
9+
message(FATAL_ERROR "OpenCV not found! Please install OpenCV and make sure it's visible to CMake.")
10+
endif()
11+
12+
message(STATUS "Found OpenCV version: ${OpenCV_VERSION}")
13+
message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
14+
message(STATUS "OpenCV libraries: ${OpenCV_LIBS}")
15+
16+
if(OpenCV_FOUND)
17+
# **************************编译普通节点*****************************
18+
add_executable(${PROJECT_NAME}
19+
main.cpp
20+
video_cam.cpp
21+
video_cam.h
22+
)
23+
target_link_directories(${PROJECT_NAME} PRIVATE )
24+
target_link_libraries(${PROJECT_NAME} PRIVATE
25+
infinite_sense_core
26+
${OpenCV_LIBS}
27+
Threads::Threads
28+
)
29+
target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS})
30+
set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "$ORIGIN")
31+
32+
# **************************编译ROS2节点*****************************
33+
find_package(rclcpp QUIET)
34+
if (NOT rclcpp_FOUND)
35+
message(STATUS "rclcpp not found. ROS2 demo will not be built")
36+
else ()
37+
message(STATUS "rclcpp found. ROS2 demo will be built")
38+
find_package(ament_cmake REQUIRED)
39+
find_package(std_msgs REQUIRED)
40+
find_package(sensor_msgs REQUIRED)
41+
find_package(image_transport REQUIRED)
42+
find_package(OpenCV REQUIRED)
43+
find_package(cv_bridge REQUIRED)
44+
add_executable(${PROJECT_NAME}_ros2
45+
main_ros2.cpp
46+
video_cam.cpp
47+
video_cam.h
48+
)
49+
target_link_directories(${PROJECT_NAME}_ros2 PRIVATE ${OpenCV_LIBS})
50+
target_include_directories(${PROJECT_NAME}_ros2 PUBLIC ${OpenCV_INCLUDE_DIRS})
51+
target_link_libraries(${PROJECT_NAME}_ros2
52+
infinite_sense_core
53+
${OpenCV_LIBRARIES}
54+
)
55+
set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
56+
ament_target_dependencies(${PROJECT_NAME}_ros2
57+
rclcpp
58+
std_msgs
59+
sensor_msgs
60+
image_transport
61+
OpenCV
62+
cv_bridge
63+
)
64+
install(TARGETS ${PROJECT_NAME}_ros2
65+
DESTINATION lib/${PROJECT_NAME}
66+
)
67+
install(TARGETS infinite_sense_core
68+
DESTINATION lib/${PROJECT_NAME}
69+
)
70+
ament_package()
71+
endif ()
72+
73+
# **************************编译ROS1节点*****************************
74+
find_package(catkin QUIET)
75+
if (NOT catkin_FOUND)
76+
message(STATUS "catkin not found, ROS1 demo will not be built")
77+
else ()
78+
message(STATUS "catkin found. ROS1 demo will be built")
79+
find_package(catkin REQUIRED COMPONENTS
80+
roscpp
81+
sensor_msgs
82+
cv_bridge
83+
image_transport
84+
geometry_msgs
85+
)
86+
file(GLOB_RECURSE SOURCE_FILES
87+
main_ros.cpp
88+
video_cam.cpp
89+
video_cam.h
90+
)
91+
add_executable(${PROJECT_NAME}_ros ${SOURCE_FILES})
92+
target_link_directories(${PROJECT_NAME}_ros PRIVATE ${OpenCV_LIBS})
93+
target_link_libraries(${PROJECT_NAME}_ros
94+
infinite_sense_core
95+
Threads::Threads
96+
${catkin_LIBRARIES}
97+
)
98+
target_include_directories(${PROJECT_NAME}_ros PRIVATE
99+
${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}
100+
)
101+
target_compile_definitions(${PROJECT_NAME}_ros PUBLIC -DROS_PLATFORM)
102+
endif ()
103+
endif()

‎example/VideoCam/main.cpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// 加入SDK头文件
2+
#include "infinite_sense.h"
3+
// 加入工业相机头文件
4+
#include "video_cam.h"
5+
using namespace infinite_sense;
6+
// 自定义回调函数
7+
void ImuCallback(const void *msg, size_t) {
8+
const auto *imu_data = static_cast<const ImuData *>(msg);
9+
LOG(INFO) << imu_data->time_stamp_us << " " << "Accel: " << imu_data->a[0] << " " << imu_data->a[1] << " "
10+
<< imu_data->a[2] << " Gyro: " << imu_data->g[0] << " " << imu_data->g[1] << " " << imu_data->g[2]
11+
<< " Temp: " << imu_data->temperature;
12+
// 处理IMU数据
13+
}
14+
15+
// 自定义回调函数
16+
void ImageCallback(const void *msg, size_t) {
17+
const auto *cam_data = static_cast<const CamData *>(msg);
18+
// 处理图像数据
19+
}
20+
21+
int main() {
22+
// 1.创建同步器
23+
Synchronizer synchronizer;
24+
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
25+
// synchronizer.SetNetLink("192.168.1.188", 8888);
26+
// 2.配置同步接口
27+
auto video_cam = std::make_shared<VideoCam>();
28+
video_cam->SetParams({{"video_cam", CAM_1}});
29+
synchronizer.UseSensor(video_cam);
30+
31+
// 3.开启同步
32+
synchronizer.Start();
33+
34+
// 4.接收数据
35+
Messenger::GetInstance().SubStruct("imu_1", ImuCallback);
36+
Messenger::GetInstance().SubStruct("video_cam", ImageCallback);
37+
38+
while (true) {
39+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
40+
}
41+
// 5.停止同步
42+
synchronizer.Stop();
43+
return 0;
44+
}

‎example/VideoCam/main_ros.cpp‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
#include "infinite_sense.h"
2+
#include "video_cam.h"
3+
4+
#include <ros/ros.h>
5+
#include <cv_bridge/cv_bridge.h>
6+
#include <image_transport/image_transport.h>
7+
#include <sensor_msgs/Imu.h>
8+
9+
using namespace infinite_sense;
10+
inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
11+
uint32_t nsec_per_second = 1e9;
12+
auto u64 = mico_sec * 1000;
13+
uint32_t sec = u64 / nsec_per_second;
14+
uint32_t nsec = u64 - (sec * nsec_per_second);
15+
return {sec, nsec};
16+
}
17+
18+
class CamDriver {
19+
public:
20+
CamDriver(ros::NodeHandle &nh) : node_(nh), it_(node_), camera_name_("video_cam"), imu_name_("imu_1") {}
21+
void ImuCallback(const void *msg, size_t) {
22+
const auto *imu_data = static_cast<const ImuData *>(msg);
23+
sensor_msgs::Imu imu_msg_data;
24+
imu_msg_data.header.frame_id = "map";
25+
imu_msg_data.header.stamp = CreateRosTimestamp(imu_data->time_stamp_us);
26+
27+
imu_msg_data.angular_velocity.x = imu_data->g[0];
28+
imu_msg_data.angular_velocity.y = imu_data->g[1];
29+
imu_msg_data.angular_velocity.z = imu_data->g[2];
30+
31+
imu_msg_data.linear_acceleration.x = imu_data->a[0];
32+
imu_msg_data.linear_acceleration.y = imu_data->a[1];
33+
imu_msg_data.linear_acceleration.z = imu_data->a[2];
34+
35+
imu_msg_data.orientation.w = imu_data->q[0];
36+
imu_msg_data.orientation.x = imu_data->q[1];
37+
imu_msg_data.orientation.y = imu_data->q[2];
38+
imu_msg_data.orientation.z = imu_data->q[3];
39+
imu_pub_.publish(imu_msg_data);
40+
}
41+
42+
// 自定义回调函数
43+
void ImageCallback(const void *msg, size_t) {
44+
const auto *cam_data = static_cast<const CamData *>(msg);
45+
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
46+
sensor_msgs::ImagePtr image_msg =
47+
// mono8:灰度类型,bgr8:彩图，具体需要根据相机类型进行修改
48+
cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_mat).toImageMsg();
49+
image_msg->header.stamp = CreateRosTimestamp(cam_data->time_stamp_us);
50+
image_pub_.publish(image_msg);
51+
}
52+
void Init() {
53+
synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
54+
video_cam_ = std::make_shared<VideoCam>();
55+
synchronizer_.UseSensor(video_cam_);
56+
imu_pub_ = node_.advertise<sensor_msgs::Imu>(imu_name_, 1000);
57+
image_pub_ = it_.advertise(camera_name_, 30);
58+
synchronizer_.Start();
59+
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
60+
Messenger::GetInstance().SubStruct(
61+
"imu_1", std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
62+
Messenger::GetInstance().SubStruct(
63+
"cam_1", std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
64+
}
65+
66+
void Run() {
67+
ros::Rate loop_rate(1000);
68+
while (ros::ok()) {
69+
ros::spinOnce();
70+
loop_rate.sleep();
71+
}
72+
synchronizer_.Stop();
73+
}
74+
75+
private:
76+
ros::NodeHandle &node_;
77+
ros::Publisher imu_pub_;
78+
image_transport::ImageTransport it_;
79+
image_transport::Publisher image_pub_;
80+
std::shared_ptr<VideoCam> video_cam_;
81+
Synchronizer synchronizer_;
82+
std::string camera_name_;
83+
std::string imu_name_;
84+
};
85+
86+
int main(int argc, char **argv) {
87+
ros::init(argc, argv, "ros_demo", ros::init_options::NoSigintHandler);
88+
ros::NodeHandle node;
89+
CamDriver demo_node(node);
90+
demo_node.Init();
91+
demo_node.Run();
92+
93+
return 0;
94+
}

‎example/VideoCam/main_ros2.cpp‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
#include "infinite_sense.h"
2+
#include "video_cam.h"
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
6+
#include <image_transport/image_transport.hpp>
7+
#include <cv_bridge/cv_bridge.h>
8+
9+
#include <sensor_msgs/msg/imu.hpp>
10+
11+
// 继承自 rclcpp::Nod
12+
class CamDriver final : public rclcpp::Node {
13+
public:
14+
CamDriver()
15+
: Node("ros2_cam_driver"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_) {
16+
// 相机名称，注意和video_cam.cpp 中的相机名称保持一致
17+
std::string camera_name_1 = "video_cam";
18+
// IMU 设备名称，自定义即可
19+
const std::string imu_name = "imu_1";
20+
// 使用串口设备进行同步，这里可以选择不同的设备
21+
synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
22+
// synchronizer_.SetNetLink("192.168.1.188", 8888);
23+
// 构建 VideoCam 类
24+
const auto video_cam = std::make_shared<infinite_sense::VideoCam>();
25+
// 重要！！！注册相机名称video_cam， 并且绑定 CAM_1 接口，也就是 video_cam对应的设备必须要接到 CAM_1
26+
video_cam->SetParams({{camera_name_1, infinite_sense::CAM_1}
27+
});
28+
// 同步类设置添加相机类
29+
synchronizer_.UseSensor(video_cam);
30+
31+
// 逐个调用开始函数
32+
synchronizer_.Start();
33+
34+
// IMU 和 图像信息的发布函数
35+
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
36+
img1_pub_ = transport_.advertise(camera_name_1, 10);
37+
// SDK 获取IMU和图像信息之后的回调函数
38+
{
39+
using namespace std::placeholders;
40+
infinite_sense::Messenger::GetInstance().SubStruct(imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
41+
infinite_sense::Messenger::GetInstance().SubStruct(camera_name_1,
42+
std::bind(&CamDriver::ImageCallback1, this, _1, _2));
43+
}
44+
}
45+
46+
// SDK 获取IMU信息后的回调函数
47+
void ImuCallback(const void *msg, size_t) const {
48+
// 按照ROS2的格式发布的IMU数据
49+
const auto *imu_data = static_cast<const infinite_sense::ImuData *>(msg);
50+
sensor_msgs::msg::Imu imu_msg;
51+
// 得到的时间单位us,需要转换为ms
52+
imu_msg.header.stamp = rclcpp::Time(imu_data->time_stamp_us * 1000);
53+
imu_msg.header.frame_id = "map";
54+
imu_msg.linear_acceleration.x = imu_data->a[0];
55+
imu_msg.linear_acceleration.y = imu_data->a[1];
56+
imu_msg.linear_acceleration.z = imu_data->a[2];
57+
imu_msg.angular_velocity.x = imu_data->g[0];
58+
imu_msg.angular_velocity.y = imu_data->g[1];
59+
imu_msg.angular_velocity.z = imu_data->g[2];
60+
imu_msg.orientation.w = imu_data->q[0];
61+
imu_msg.orientation.x = imu_data->q[1];
62+
imu_msg.orientation.y = imu_data->q[2];
63+
imu_msg.orientation.z = imu_data->q[3];
64+
imu_pub_->publish(imu_msg);
65+
}
66+
67+
// SDK 获取图像信息后的回调函数
68+
void ImageCallback1(const void *msg, size_t) const {
69+
// 原始的图像数据
70+
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
71+
std_msgs::msg::Header header;
72+
// 得到的时间单位us,需要转换为ms
73+
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
74+
header.frame_id = "map";
75+
// 构造opencv图像，这里是彩色图片因此是 CV_8UC3 和 bgr8
76+
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC3, cam_data->image.data);
77+
const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "bgr8", image_mat).toImageMsg();
78+
img1_pub_.publish(image_msg);
79+
}
80+
81+
82+
private:
83+
infinite_sense::Synchronizer synchronizer_;
84+
SharedPtr node_handle_;
85+
image_transport::ImageTransport transport_;
86+
image_transport::Publisher img1_pub_,img2_pub_;
87+
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
88+
};
89+
90+
// main 函数
91+
int main(const int argc, char *argv[]) {
92+
rclcpp::init(argc, argv);
93+
// 构造 CamDriver 类，用于读取相机、IMU等信息
94+
rclcpp::spin(std::make_shared<CamDriver>());
95+
return 0;
96+
}

‎example/VideoCam/package.xml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0"?>
2+
<package format="3">
3+
<name>video_cam</name>
4+
<version>1.0.0</version>
5+
<description>The infinite sense camera package</description>
6+
<maintainer email="www.qq837374741@foxmail.com">infinite sense</maintainer>
7+
<license>BSD</license>
8+
<buildtool_depend>ament_cmake</buildtool_depend>
9+
10+
<build_depend>rclcpp</build_depend>
11+
<build_depend>sensor_msgs</build_depend>
12+
<build_depend>std_msgs</build_depend>
13+
<exec_depend>rclcpp</exec_depend>
14+
<exec_depend>sensor_msgs</exec_depend>
15+
<exec_depend>std_msgs</exec_depend>
16+
<export>
17+
<build_type>ament_cmake</build_type>
18+
</export>
19+
20+
</package>
21+
22+
23+

0 commit comments

Comments
 (0)