Skip to content

Commit 33af8ff

Browse files
linyicheng1linyicheng1
committed
更新USB相机Demo
1 parent 2bcfef7 commit 33af8ff

File tree

5 files changed

+283
-120
lines changed

5 files changed

+283
-120
lines changed

‎example/VideoCam/main.cpp‎

Lines changed: 58 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,19 @@
33
// 加入工业相机头文件
44
#include "video_cam.h"
55
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+
619
// 自定义回调函数
720
void ImuCallback(const void *msg, size_t) {
821
const auto *imu_data = static_cast<const ImuData *>(msg);
@@ -18,22 +31,60 @@ void ImageCallback(const void *msg, size_t) {
1831
// 处理图像数据
1932
}
2033

21-
int main() {
34+
int main(int argc, char* argv[]) {
2235
// 1.创建同步器
2336
Synchronizer synchronizer;
24-
synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
25-
// synchronizer.SetNetLink("192.168.1.188", 8888);
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+
2665
// 2.配置同步接口
27-
auto video_cam = std::make_shared<VideoCam>();
28-
video_cam->SetParams({{"video_cam", CAM_1}});
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+
2974
synchronizer.UseSensor(video_cam);
3075

3176
// 3.开启同步
3277
synchronizer.Start();
3378

3479
// 4.接收数据
35-
Messenger::GetInstance().SubStruct("imu_1", ImuCallback);
36-
Messenger::GetInstance().SubStruct("video_cam", ImageCallback);
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);
3788

3889
while (true) {
3990
std::this_thread::sleep_for(std::chrono::milliseconds(100));

‎example/VideoCam/main_ros.cpp‎

Lines changed: 70 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,33 @@
11
#include "infinite_sense.h"
22
#include "video_cam.h"
3-
3+
#include <unordered_map>
44
#include <ros/ros.h>
55
#include <cv_bridge/cv_bridge.h>
66
#include <image_transport/image_transport.h>
77
#include <sensor_msgs/Imu.h>
88

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+
929
using namespace infinite_sense;
30+
1031
inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
1132
uint32_t nsec_per_second = 1e9;
1233
auto u64 = mico_sec * 1000;
@@ -17,7 +38,7 @@ inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
1738

1839
class CamDriver {
1940
public:
20-
CamDriver(ros::NodeHandle &nh) : node_(nh), it_(node_), camera_name_("video_cam"), imu_name_("imu_1") {}
41+
CamDriver(ros::NodeHandle &nh) : node_(nh), it_(node_) {}
2142
void ImuCallback(const void *msg, size_t) {
2243
const auto *imu_data = static_cast<const ImuData *>(msg);
2344
sensor_msgs::Imu imu_msg_data;
@@ -47,20 +68,57 @@ class CamDriver {
4768
// mono8:灰度类型,bgr8:彩图，具体需要根据相机类型进行修改
4869
cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_mat).toImageMsg();
4970
image_msg->header.stamp = CreateRosTimestamp(cam_data->time_stamp_us);
50-
image_pub_.publish(image_msg);
71+
image_pubs_[cam_data->name].publish(image_msg);
5172
}
73+
5274
void Init() {
53-
synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
54-
video_cam_ = std::make_shared<VideoCam>();
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);
5589
synchronizer_.UseSensor(video_cam_);
56-
imu_pub_ = node_.advertise<sensor_msgs::Imu>(imu_name_, 1000);
57-
image_pub_ = it_.advertise(camera_name_, 30);
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+
58110
synchronizer_.Start();
59111
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
112+
113+
60114
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));
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));
64122
}
65123

66124
void Run() {
@@ -76,15 +134,13 @@ class CamDriver {
76134
ros::NodeHandle &node_;
77135
ros::Publisher imu_pub_;
78136
image_transport::ImageTransport it_;
79-
image_transport::Publisher image_pub_;
137+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
80138
std::shared_ptr<VideoCam> video_cam_;
81139
Synchronizer synchronizer_;
82-
std::string camera_name_;
83-
std::string imu_name_;
84140
};
85141

86142
int main(int argc, char **argv) {
87-
ros::init(argc, argv, "ros_demo", ros::init_options::NoSigintHandler);
143+
ros::init(argc, argv, "ros_video_demo", ros::init_options::NoSigintHandler);
88144
ros::NodeHandle node;
89145
CamDriver demo_node(node);
90146
demo_node.Init();

‎example/VideoCam/main_ros2.cpp‎

Lines changed: 68 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -8,38 +8,79 @@
88

99
#include <sensor_msgs/msg/imu.hpp>
1010

11+
12+
struct video_config {
13+
bool onboard_imu = true;// 使用内部IMU
14+
bool extern_imu = false;// 使用外部IMU,TODO
15+
std::string imu_name = "imu_1";
16+
17+
// 相机名称 + 设备ID
18+
std::pair<std::string, int> CAM3_Video = {"left_cam", 0};
19+
std::pair<std::string, int> CAM4_Video = {"right_cam", -1};
20+
std::pair<std::string, int> CAM1_Video = {"front_cam", -1};
21+
std::pair<std::string, int> CAM2_Video = {"rear_cam", -1}; // -1 代表没有
22+
23+
// 通信方式
24+
int type = 0; // 0 --> 串口通信 1 --> 网口通信
25+
std::string uart_dev = "/dev/ttyACM0";
26+
std::string net_ip = "192.168.1.188";
27+
int net_port = 8888;
28+
};
29+
video_config cfg;
30+
31+
using namespace infinite_sense;
32+
1133
// 继承自 rclcpp::Nod
1234
class CamDriver final : public rclcpp::Node {
1335
public:
1436
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);
37+
: Node("ros2_video_demo"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_){
38+
39+
// 相机通信方式
40+
if (cfg.type == 0) {
41+
synchronizer_.SetUsbLink(cfg.uart_dev, 921600);
42+
} else if (cfg.type == 1) {
43+
synchronizer_.SetNetLink(cfg.net_ip, cfg.net_port);
44+
}
45+
46+
std::vector<std::pair<std::string, int>> cam_list;
47+
if (cfg.CAM1_Video.second >= 0) cam_list.push_back(cfg.CAM1_Video);
48+
if (cfg.CAM2_Video.second >= 0) cam_list.push_back(cfg.CAM2_Video);
49+
if (cfg.CAM3_Video.second >= 0) cam_list.push_back(cfg.CAM3_Video);
50+
if (cfg.CAM4_Video.second >= 0) cam_list.push_back(cfg.CAM4_Video);
51+
52+
video_cam_ = std::make_shared<VideoCam>(cam_list);
53+
synchronizer_.UseSensor(video_cam_);
54+
55+
if (cfg.onboard_imu) {
56+
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(cfg.imu_name, 1000);
57+
}
58+
59+
if (cfg.CAM1_Video.second >= 0) image_pubs_[cfg.CAM1_Video.first] = transport_.advertise(cfg.CAM1_Video.first, 30);
60+
if (cfg.CAM2_Video.second >= 0) image_pubs_[cfg.CAM2_Video.first] = transport_.advertise(cfg.CAM2_Video.first, 30);
61+
if (cfg.CAM3_Video.second >= 0) image_pubs_[cfg.CAM3_Video.first] = transport_.advertise(cfg.CAM3_Video.first, 30);
62+
if (cfg.CAM4_Video.second >= 0) image_pubs_[cfg.CAM4_Video.first] = transport_.advertise(cfg.CAM4_Video.first, 30);
63+
64+
// 设置触发参数（如果有的话）
65+
// 这里用相机名作为 key
66+
if (cfg.CAM1_Video.second >= 0) video_cam_->SetParams({{cfg.CAM1_Video.first, CAM_1}});
67+
if (cfg.CAM2_Video.second >= 0) video_cam_->SetParams({{cfg.CAM2_Video.first, CAM_2}});
68+
if (cfg.CAM3_Video.second >= 0) video_cam_->SetParams({{cfg.CAM3_Video.first, CAM_3}});
69+
if (cfg.CAM4_Video.second >= 0) video_cam_->SetParams({{cfg.CAM4_Video.first, CAM_4}});
3070

3171
// 逐个调用开始函数
3272
synchronizer_.Start();
3373

34-
// IMU 和 图像信息的发布函数
35-
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
36-
img1_pub_ = transport_.advertise(camera_name_1, 10);
74+
3775
// SDK 获取IMU和图像信息之后的回调函数
3876
{
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));
77+
Messenger::GetInstance().SubStruct(
78+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
79+
80+
if (cfg.CAM1_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
81+
if (cfg.CAM2_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
82+
if (cfg.CAM3_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM3_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
83+
if (cfg.CAM4_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM4_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
4384
}
4485
}
4586

@@ -65,7 +106,7 @@ class CamDriver final : public rclcpp::Node {
65106
}
66107

67108
// SDK 获取图像信息后的回调函数
68-
void ImageCallback1(const void *msg, size_t) const {
109+
void ImageCallback(const void *msg, size_t) {
69110
// 原始的图像数据
70111
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
71112
std_msgs::msg::Header header;
@@ -74,16 +115,17 @@ class CamDriver final : public rclcpp::Node {
74115
header.frame_id = "map";
75116
// 构造opencv图像，这里是彩色图片因此是 CV_8UC3 和 bgr8
76117
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);
118+
sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "bgr8", image_mat).toImageMsg();
119+
image_pubs_[cam_data->name].publish(image_msg);
79120
}
80121

81122

82123
private:
83124
infinite_sense::Synchronizer synchronizer_;
84125
SharedPtr node_handle_;
85126
image_transport::ImageTransport transport_;
86-
image_transport::Publisher img1_pub_,img2_pub_;
127+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
128+
std::shared_ptr<VideoCam> video_cam_;
87129
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
88130
};
89131

0 commit comments

Comments
 (0)