Skip to content

Commit cf8a677

Browse files
linyicheng1linyicheng1
committed
add net camera demo, lvi mode
1 parent 7cce941 commit cf8a677

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

70 files changed

+7490
-0
lines changed

‎example/CMakeLists.txt‎

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

44
add_subdirectory(NetCam)
5+
add_subdirectory(NetCam_LVI)
56
add_subdirectory(VideoCam)
67
add_subdirectory(VideoCam_LVI)
78
add_subdirectory(CustomCam)

‎example/NetCam_LVI/CMakeLists.txt‎

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

‎example/NetCam_LVI/main.cpp‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
// 加入SDK头文件
2+
#include "infinite_sense.h"
3+
// 加入工业相机头文件
4+
#include "mv_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(int argc, char* argv[]) {
22+
// 1.创建同步器
23+
Synchronizer synchronizer;
24+
25+
if (argc < 2) {
26+
// 默认使用串口
27+
std::cout << "未传入参数，默认使用 USB 串口 /dev/ttyACM0" << std::endl;
28+
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
29+
} else {
30+
std::string mode = argv[1];
31+
if (mode == "uart") {
32+
std::cout << "参数为 uart，使用 USB 串口 /dev/ttyACM0" << std::endl;
33+
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
34+
} else if (mode == "net") {
35+
std::cout << "参数为 net，使用网口 192.168.1.188:8888" << std::endl;
36+
synchronizer.SetNetLink("192.168.1.188", 8888);
37+
} else {
38+
std::cerr << "未知参数: " << mode
39+
<< "，请使用 uart / net 或不传参数" << std::endl;
40+
return -1;
41+
}
42+
}
43+
44+
// 2.配置同步接口
45+
auto mv_cam = std::make_shared<MvCam>();
46+
mv_cam->SetParams({{"cam_1", CAM_1}});
47+
synchronizer.UseSensor(mv_cam);
48+
49+
// 3.开启同步
50+
synchronizer.Start();
51+
52+
// 4.接收数据
53+
Messenger::GetInstance().SubStruct("imu_1", ImuCallback);
54+
Messenger::GetInstance().SubStruct("cam_1", ImageCallback);
55+
56+
while (true) {
57+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
58+
}
59+
60+
// 5.停止同步
61+
synchronizer.Stop();
62+
return 0;
63+
}

‎example/NetCam_LVI/main_ros.cpp‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
#include "infinite_sense.h"
2+
#include "mv_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+
10+
struct cam_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> CAM1 = {"cam_1", 1};
17+
std::pair<std::string, int> CAM2 = {"cam_2", -1};
18+
std::pair<std::string, int> CAM3 = {"cam_3", -1};
19+
std::pair<std::string, int> CAM4 = {"cam_4", -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+
cam_config cfg;
28+
29+
using namespace infinite_sense;
30+
inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
31+
uint32_t nsec_per_second = 1e9;
32+
auto u64 = mico_sec * 1000;
33+
uint32_t sec = u64 / nsec_per_second;
34+
uint32_t nsec = u64 - (sec * nsec_per_second);
35+
return {sec, nsec};
36+
}
37+
38+
class CamDriver {
39+
public:
40+
CamDriver(ros::NodeHandle &nh) : node_(nh), it_(node_), camera_name_("cam_1"), imu_name_("imu_1") {}
41+
void ImuCallback(const void *msg, size_t) {
42+
const auto *imu_data = static_cast<const ImuData *>(msg);
43+
sensor_msgs::Imu imu_msg_data;
44+
imu_msg_data.header.frame_id = "map";
45+
imu_msg_data.header.stamp = CreateRosTimestamp(imu_data->time_stamp_us);
46+
47+
imu_msg_data.angular_velocity.x = imu_data->g[0];
48+
imu_msg_data.angular_velocity.y = imu_data->g[1];
49+
imu_msg_data.angular_velocity.z = imu_data->g[2];
50+
51+
imu_msg_data.linear_acceleration.x = imu_data->a[0];
52+
imu_msg_data.linear_acceleration.y = imu_data->a[1];
53+
imu_msg_data.linear_acceleration.z = imu_data->a[2];
54+
55+
imu_msg_data.orientation.w = imu_data->q[0];
56+
imu_msg_data.orientation.x = imu_data->q[1];
57+
imu_msg_data.orientation.y = imu_data->q[2];
58+
imu_msg_data.orientation.z = imu_data->q[3];
59+
imu_pub_.publish(imu_msg_data);
60+
}
61+
62+
// 自定义回调函数
63+
void ImageCallback(const void *msg, size_t) {
64+
const auto *cam_data = static_cast<const CamData *>(msg);
65+
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
66+
sensor_msgs::ImagePtr image_msg =
67+
// mono8:灰度类型,bgr8:彩图，具体需要根据相机类型进行修改
68+
cv_bridge::CvImage(std_msgs::Header(), "mono8", image_mat).toImageMsg();
69+
image_msg->header.stamp = CreateRosTimestamp(cam_data->time_stamp_us);
70+
image_pubs_[cam_data->name].publish(image_msg);
71+
}
72+
void Init() {
73+
if (cfg.type == 0) {
74+
synchronizer_.SetUsbLink(cfg.uart_dev, 921600);
75+
} else if (cfg.type == 1) {
76+
synchronizer_.SetNetLink(cfg.net_ip, cfg.net_port);
77+
}
78+
79+
mv_cam_ = std::make_shared<MvCam>();
80+
synchronizer_.UseSensor(mv_cam_);
81+
82+
if (cfg.onboard_imu) {
83+
imu_pub_ = node_.advertise<sensor_msgs::Imu>(cfg.imu_name, 1000);
84+
}
85+
86+
if (cfg.CAM1.second >= 0) mv_cam_->SetParams({{cfg.CAM1.first, CAM_1}});
87+
if (cfg.CAM2.second >= 0) mv_cam_->SetParams({{cfg.CAM2.first, CAM_2}});
88+
if (cfg.CAM3.second >= 0) mv_cam_->SetParams({{cfg.CAM3.first, CAM_3}});
89+
if (cfg.CAM4.second >= 0) mv_cam_->SetParams({{cfg.CAM4.first, CAM_4}});
90+
91+
92+
if (cfg.CAM1.second >= 0) image_pubs_[cfg.CAM1.first] = it_.advertise(cfg.CAM1.first, 30);
93+
if (cfg.CAM2.second >= 0) image_pubs_[cfg.CAM2.first] = it_.advertise(cfg.CAM2.first, 30);
94+
if (cfg.CAM3.second >= 0) image_pubs_[cfg.CAM3.first] = it_.advertise(cfg.CAM3.first, 30);
95+
if (cfg.CAM4.second >= 0) image_pubs_[cfg.CAM4.first] = it_.advertise(cfg.CAM4.first, 30);
96+
97+
synchronizer_.Start();
98+
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
99+
100+
if (cfg.onboard_imu) {
101+
Messenger::GetInstance().SubStruct(
102+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
103+
}
104+
105+
if (cfg.CAM1.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
106+
if (cfg.CAM2.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
107+
if (cfg.CAM3.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM3.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
108+
if (cfg.CAM4.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM4.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
109+
}
110+
111+
void Run() {
112+
ros::Rate loop_rate(1000);
113+
while (ros::ok()) {
114+
ros::spinOnce();
115+
loop_rate.sleep();
116+
}
117+
synchronizer_.Stop();
118+
}
119+
120+
private:
121+
ros::NodeHandle &node_;
122+
ros::Publisher imu_pub_;
123+
image_transport::ImageTransport it_;
124+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
125+
std::shared_ptr<MvCam> mv_cam_;
126+
Synchronizer synchronizer_;
127+
std::string camera_name_;
128+
std::string imu_name_;
129+
};
130+
131+
int main(int argc, char **argv) {
132+
ros::init(argc, argv, "ros_demo", ros::init_options::NoSigintHandler);
133+
ros::NodeHandle node;
134+
CamDriver demo_node(node);
135+
demo_node.Init();
136+
demo_node.Run();
137+
138+
return 0;
139+
}

0 commit comments

Comments
 (0)