Skip to content

Commit af45f5a

Browse files
linyicheng1linyicheng1
committed
add fast-livo demo
1 parent ec489f1 commit af45f5a

File tree

8 files changed

+662
-1
lines changed

8 files changed

+662
-1
lines changed

‎example/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,4 +3,5 @@ project(example)
33

44
add_subdirectory(NetCam)
55
add_subdirectory(VideoCam)
6-
add_subdirectory(CustomCam)
6+
add_subdirectory(VideoCam_LVI)
7+
add_subdirectory(CustomCam)

‎example/VideoCam_LVI/CMakeLists.txt‎

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

‎example/VideoCam_LVI/main.cpp‎

Lines changed: 95 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
// 加入SDK头文件
2+
#include "infinite_sense.h"
3+
// 加入工业相机头文件
4+
#include "video_cam.h"
5+
using namespace infinite_sense;
6+
7+
// TODO: 请按需要修改如下配置
8+
struct video_config {
9+
bool onboard_imu = true;
10+
11+
// 相机名称 + 设备ID
12+
std::pair<std::string, int> CAM3_Video = {"left_cam", 0};
13+
std::pair<std::string, int> CAM4_Video = {"right_cam", 1};
14+
std::pair<std::string, int> CAM1_Video = {"front_cam", 2};
15+
std::pair<std::string, int> CAM2_Video = {"rear_cam", -1}; // -1 代表没有
16+
};
17+
18+
19+
// 自定义回调函数
20+
void ImuCallback(const void *msg, size_t) {
21+
const auto *imu_data = static_cast<const ImuData *>(msg);
22+
LOG(INFO) << imu_data->time_stamp_us << " " << "Accel: " << imu_data->a[0] << " " << imu_data->a[1] << " "
23+
<< imu_data->a[2] << " Gyro: " << imu_data->g[0] << " " << imu_data->g[1] << " " << imu_data->g[2]
24+
<< " Temp: " << imu_data->temperature;
25+
// 处理IMU数据
26+
}
27+
28+
// 自定义回调函数
29+
void ImageCallback(const void *msg, size_t) {
30+
const auto *cam_data = static_cast<const CamData *>(msg);
31+
// 处理图像数据
32+
}
33+
34+
int main(int argc, char* argv[]) {
35+
// 1.创建同步器
36+
Synchronizer synchronizer;
37+
38+
if (argc < 2) {
39+
// 默认使用串口
40+
std::cout << "未传入参数，默认使用 USB 串口 /dev/ttyACM0" << std::endl;
41+
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
42+
} else {
43+
std::string mode = argv[1];
44+
if (mode == "uart") {
45+
std::cout << "参数为 uart，使用 USB 串口 /dev/ttyACM0" << std::endl;
46+
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
47+
} else if (mode == "net") {
48+
std::cout << "参数为 net，使用网口 192.168.1.188:8888" << std::endl;
49+
synchronizer.SetNetLink("192.168.1.188", 8888);
50+
} else {
51+
std::cerr << "未知参数: " << mode
52+
<< "，请使用 uart / net 或不传参数" << std::endl;
53+
return -1;
54+
}
55+
}
56+
57+
video_config cfg;
58+
// 把配置转成 cam_list，过滤掉 device_id = -1 的
59+
std::vector<std::pair<std::string, int>> cam_list;
60+
if (cfg.CAM1_Video.second >= 0) cam_list.push_back(cfg.CAM1_Video);
61+
if (cfg.CAM2_Video.second >= 0) cam_list.push_back(cfg.CAM2_Video);
62+
if (cfg.CAM3_Video.second >= 0) cam_list.push_back(cfg.CAM3_Video);
63+
if (cfg.CAM4_Video.second >= 0) cam_list.push_back(cfg.CAM4_Video);
64+
65+
// 2.配置同步接口
66+
auto video_cam = std::make_shared<VideoCam>(cam_list);
67+
// 设置触发参数（如果有的话）
68+
// 这里用相机名作为 key
69+
if (cfg.CAM1_Video.second >= 0) video_cam->SetParams({{cfg.CAM1_Video.first, CAM_1}});
70+
if (cfg.CAM2_Video.second >= 0) video_cam->SetParams({{cfg.CAM2_Video.first, CAM_2}});
71+
if (cfg.CAM3_Video.second >= 0) video_cam->SetParams({{cfg.CAM3_Video.first, CAM_3}});
72+
if (cfg.CAM4_Video.second >= 0) video_cam->SetParams({{cfg.CAM4_Video.first, CAM_4}});
73+
74+
synchronizer.UseSensor(video_cam);
75+
76+
// 3.开启同步
77+
synchronizer.Start();
78+
79+
// 4.接收数据
80+
if (cfg.onboard_imu) {
81+
Messenger::GetInstance().SubStruct("imu_1", ImuCallback);
82+
}
83+
84+
if (cfg.CAM1_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, ImageCallback);
85+
if (cfg.CAM2_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2_Video.first, ImageCallback);
86+
if (cfg.CAM3_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM3_Video.first, ImageCallback);
87+
if (cfg.CAM4_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM4_Video.first, ImageCallback);
88+
89+
while (true) {
90+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
91+
}
92+
// 5.停止同步
93+
synchronizer.Stop();
94+
return 0;
95+
}

‎example/VideoCam_LVI/main_ros.cpp‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
#include "infinite_sense.h"
2+
#include "video_cam.h"
3+
#include <unordered_map>
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+
10+
struct video_config {
11+
bool onboard_imu = true;// 使用内部IMU
12+
bool extern_imu = false;// 使用外部IMU,TODO
13+
std::string imu_name = "onboard_imu";
14+
15+
// 相机名称 + 设备ID
16+
std::pair<std::string, int> CAM3_Video = {"left_cam", -1};
17+
std::pair<std::string, int> CAM4_Video = {"right_cam", -1};
18+
std::pair<std::string, int> CAM1_Video = {"front_cam", -1};
19+
std::pair<std::string, int> CAM2_Video = {"rear_cam", -1}; // -1 代表没有
20+
21+
// 通信方式
22+
int type = 0; // 0 --> 串口通信 1 --> 网口通信
23+
std::string uart_dev = "/dev/ttyACM0";
24+
std::string net_ip = "192.168.1.188";
25+
int net_port = 8888;
26+
};
27+
video_config cfg;
28+
29+
using namespace infinite_sense;
30+
31+
inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
32+
uint32_t nsec_per_second = 1e9;
33+
auto u64 = mico_sec * 1000;
34+
uint32_t sec = u64 / nsec_per_second;
35+
uint32_t nsec = u64 - (sec * nsec_per_second);
36+
return {sec, nsec};
37+
}
38+
39+
class CamDriver {
40+
public:
41+
CamDriver(ros::NodeHandle &nh) : node_(nh), it_(node_) {}
42+
void ImuCallback(const void *msg, size_t) {
43+
const auto *imu_data = static_cast<const ImuData *>(msg);
44+
sensor_msgs::Imu imu_msg_data;
45+
imu_msg_data.header.frame_id = "map";
46+
imu_msg_data.header.stamp = CreateRosTimestamp(imu_data->time_stamp_us);
47+
48+
imu_msg_data.angular_velocity.x = imu_data->g[0];
49+
imu_msg_data.angular_velocity.y = imu_data->g[1];
50+
imu_msg_data.angular_velocity.z = imu_data->g[2];
51+
52+
imu_msg_data.linear_acceleration.x = imu_data->a[0];
53+
imu_msg_data.linear_acceleration.y = imu_data->a[1];
54+
imu_msg_data.linear_acceleration.z = imu_data->a[2];
55+
56+
imu_msg_data.orientation.w = imu_data->q[0];
57+
imu_msg_data.orientation.x = imu_data->q[1];
58+
imu_msg_data.orientation.y = imu_data->q[2];
59+
imu_msg_data.orientation.z = imu_data->q[3];
60+
imu_pub_.publish(imu_msg_data);
61+
}
62+
63+
// 自定义回调函数
64+
void ImageCallback(const void *msg, size_t) {
65+
const auto *cam_data = static_cast<const CamData *>(msg);
66+
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
67+
sensor_msgs::ImagePtr image_msg =
68+
// mono8:灰度类型,bgr8:彩图，具体需要根据相机类型进行修改
69+
cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_mat).toImageMsg();
70+
image_msg->header.stamp = CreateRosTimestamp(cam_data->time_stamp_us);
71+
image_pubs_[cam_data->name].publish(image_msg);
72+
}
73+
74+
void Init() {
75+
76+
if (cfg.type == 0) {
77+
synchronizer_.SetUsbLink(cfg.uart_dev, 921600);
78+
} else if (cfg.type == 1) {
79+
synchronizer_.SetNetLink(cfg.net_ip, cfg.net_port);
80+
}
81+
82+
std::vector<std::pair<std::string, int>> cam_list;
83+
if (cfg.CAM1_Video.second >= 0) cam_list.push_back(cfg.CAM1_Video);
84+
if (cfg.CAM2_Video.second >= 0) cam_list.push_back(cfg.CAM2_Video);
85+
if (cfg.CAM3_Video.second >= 0) cam_list.push_back(cfg.CAM3_Video);
86+
if (cfg.CAM4_Video.second >= 0) cam_list.push_back(cfg.CAM4_Video);
87+
88+
video_cam_ = std::make_shared<VideoCam>(cam_list);
89+
synchronizer_.UseSensor(video_cam_);
90+
91+
92+
if (cfg.onboard_imu) {
93+
imu_pub_ = node_.advertise<sensor_msgs::Imu>(cfg.imu_name, 1000);
94+
}
95+
96+
if (cfg.CAM1_Video.second >= 0) image_pubs_[cfg.CAM1_Video.first] = it_.advertise(cfg.CAM1_Video.first, 30);
97+
if (cfg.CAM2_Video.second >= 0) image_pubs_[cfg.CAM2_Video.first] = it_.advertise(cfg.CAM2_Video.first, 30);
98+
if (cfg.CAM3_Video.second >= 0) image_pubs_[cfg.CAM3_Video.first] = it_.advertise(cfg.CAM3_Video.first, 30);
99+
if (cfg.CAM4_Video.second >= 0) image_pubs_[cfg.CAM4_Video.first] = it_.advertise(cfg.CAM4_Video.first, 30);
100+
101+
102+
// 设置触发参数（如果有的话）
103+
// 这里用相机名作为 key
104+
if (cfg.CAM1_Video.second >= 0) video_cam_->SetParams({{cfg.CAM1_Video.first, CAM_1}});
105+
if (cfg.CAM2_Video.second >= 0) video_cam_->SetParams({{cfg.CAM2_Video.first, CAM_2}});
106+
if (cfg.CAM3_Video.second >= 0) video_cam_->SetParams({{cfg.CAM3_Video.first, CAM_3}});
107+
if (cfg.CAM4_Video.second >= 0) video_cam_->SetParams({{cfg.CAM4_Video.first, CAM_4}});
108+
109+
110+
synchronizer_.Start();
111+
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
112+
113+
114+
Messenger::GetInstance().SubStruct(
115+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
116+
117+
118+
if (cfg.CAM1_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
119+
if (cfg.CAM2_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
120+
if (cfg.CAM3_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
121+
if (cfg.CAM4_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
122+
}
123+
124+
void Run() {
125+
ros::Rate loop_rate(1000);
126+
while (ros::ok()) {
127+
ros::spinOnce();
128+
loop_rate.sleep();
129+
}
130+
synchronizer_.Stop();
131+
}
132+
133+
private:
134+
ros::NodeHandle &node_;
135+
ros::Publisher imu_pub_;
136+
image_transport::ImageTransport it_;
137+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
138+
std::shared_ptr<VideoCam> video_cam_;
139+
Synchronizer synchronizer_;
140+
};
141+
142+
int main(int argc, char **argv) {
143+
ros::init(argc, argv, "ros_video_demo", ros::init_options::NoSigintHandler);
144+
ros::NodeHandle node;
145+
CamDriver demo_node(node);
146+
demo_node.Init();
147+
demo_node.Run();
148+
149+
return 0;
150+
}

0 commit comments

Comments
 (0)