Skip to content

Commit 091fef6

Browse files
linyicheng1linyicheng1
committed
update example demo
1 parent af45f5a commit 091fef6

File tree

6 files changed

+232
-139
lines changed

6 files changed

+232
-139
lines changed

‎example/NetCam/main_ros.cpp‎

Lines changed: 54 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,26 @@
66
#include <image_transport/image_transport.h>
77
#include <sensor_msgs/Imu.h>
88

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+
929
using namespace infinite_sense;
1030
inline ros::Time CreateRosTimestamp(const uint64_t mico_sec) {
1131
uint32_t nsec_per_second = 1e9;
@@ -47,21 +67,45 @@ class CamDriver {
4767
// mono8:灰度类型,bgr8:彩图，具体需要根据相机类型进行修改
4868
cv_bridge::CvImage(std_msgs::Header(), "mono8", image_mat).toImageMsg();
4969
image_msg->header.stamp = CreateRosTimestamp(cam_data->time_stamp_us);
50-
image_pub_.publish(image_msg);
70+
image_pubs_[cam_data->name].publish(image_msg);
5171
}
5272
void Init() {
53-
synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
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+
5479
mv_cam_ = std::make_shared<MvCam>();
55-
mv_cam_->SetParams({{"cam_1", CAM_1}});
5680
synchronizer_.UseSensor(mv_cam_);
57-
imu_pub_ = node_.advertise<sensor_msgs::Imu>(imu_name_, 1000);
58-
image_pub_ = it_.advertise(camera_name_, 30);
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+
5997
synchronizer_.Start();
6098
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
61-
Messenger::GetInstance().SubStruct(
62-
"imu_1", std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
63-
Messenger::GetInstance().SubStruct(
64-
"cam_1", std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
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));
65109
}
66110

67111
void Run() {
@@ -77,7 +121,7 @@ class CamDriver {
77121
ros::NodeHandle &node_;
78122
ros::Publisher imu_pub_;
79123
image_transport::ImageTransport it_;
80-
image_transport::Publisher image_pub_;
124+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
81125
std::shared_ptr<MvCam> mv_cam_;
82126
Synchronizer synchronizer_;
83127
std::string camera_name_;

‎example/NetCam/main_ros2.cpp‎

Lines changed: 62 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -8,31 +8,71 @@
88

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

11+
struct cam_config {
12+
bool onboard_imu = true;// 使用内部IMU
13+
bool extern_imu = false;// 使用外部IMU,TODO
14+
std::string imu_name = "onboard_imu";
15+
16+
// 相机名称 + 设备ID
17+
std::pair<std::string, int> CAM1 = {"cam_1", 1};
18+
std::pair<std::string, int> CAM2 = {"cam_2", -1};
19+
std::pair<std::string, int> CAM3 = {"cam_3", -1};
20+
std::pair<std::string, int> CAM4 = {"cam_4", -1}; // -1 代表没有
21+
22+
// 通信方式
23+
int type = 0; // 0 --> 串口通信 1 --> 网口通信
24+
std::string uart_dev = "/dev/ttyACM0";
25+
std::string net_ip = "192.168.1.188";
26+
int net_port = 8888;
27+
};
28+
cam_config cfg;
29+
30+
using namespace infinite_sense;
31+
1132
class CamDriver final : public rclcpp::Node {
1233
public:
1334
CamDriver()
1435
: Node("ros2_cam_driver"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_) {
15-
std::string camera_name_1 = "cam_1";
16-
std::string camera_name_2 = "cam_2";
17-
const std::string imu_name = "imu_1";
18-
// synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
19-
synchronizer_.SetNetLink("192.168.1.188", 8888);
20-
const auto mv_cam = std::make_shared<infinite_sense::MvCam>();
21-
mv_cam->SetParams({{camera_name_1, infinite_sense::CAM_1},
22-
{camera_name_2, infinite_sense::CAM_2},
23-
});
24-
synchronizer_.UseSensor(mv_cam);
36+
37+
if (cfg.type == 0) {
38+
synchronizer_.SetUsbLink(cfg.uart_dev, 921600);
39+
} else if (cfg.type == 1) {
40+
synchronizer_.SetNetLink(cfg.net_ip, cfg.net_port);
41+
}
42+
43+
mv_cam_ = std::make_shared<infinite_sense::MvCam>();
44+
45+
if (cfg.CAM1.second >= 0) mv_cam_->SetParams({{cfg.CAM1.first, CAM_1}});
46+
if (cfg.CAM2.second >= 0) mv_cam_->SetParams({{cfg.CAM2.first, CAM_2}});
47+
if (cfg.CAM3.second >= 0) mv_cam_->SetParams({{cfg.CAM3.first, CAM_3}});
48+
if (cfg.CAM4.second >= 0) mv_cam_->SetParams({{cfg.CAM4.first, CAM_4}});
49+
50+
51+
if (cfg.CAM1.second >= 0) image_pubs_[cfg.CAM1.first] = transport_.advertise(cfg.CAM1.first, 30);
52+
if (cfg.CAM2.second >= 0) image_pubs_[cfg.CAM2.first] = transport_.advertise(cfg.CAM2.first, 30);
53+
if (cfg.CAM3.second >= 0) image_pubs_[cfg.CAM3.first] = transport_.advertise(cfg.CAM3.first, 30);
54+
if (cfg.CAM4.second >= 0) image_pubs_[cfg.CAM4.first] = transport_.advertise(cfg.CAM4.first, 30);
55+
56+
synchronizer_.UseSensor(mv_cam_);
2557
synchronizer_.Start();
26-
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
27-
img1_pub_ = transport_.advertise(camera_name_1, 10);
28-
img2_pub_ = transport_.advertise(camera_name_2, 10);
58+
59+
if (cfg.onboard_imu) {
60+
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(cfg.imu_name, 10);
61+
Messenger::GetInstance().SubStruct(
62+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
63+
}
64+
2965
{
3066
using namespace std::placeholders;
31-
infinite_sense::Messenger::GetInstance().SubStruct(imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
32-
infinite_sense::Messenger::GetInstance().SubStruct(camera_name_1,
33-
std::bind(&CamDriver::ImageCallback1, this, _1, _2));
34-
infinite_sense::Messenger::GetInstance().SubStruct(camera_name_2,
35-
std::bind(&CamDriver::ImageCallback2, this, _1, _2));
67+
if (cfg.onboard_imu) {
68+
Messenger::GetInstance().SubStruct(
69+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
70+
}
71+
72+
if (cfg.CAM1.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
73+
if (cfg.CAM2.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
74+
if (cfg.CAM3.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM3.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
75+
if (cfg.CAM4.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM4.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
3676
}
3777
}
3878

@@ -54,32 +94,23 @@ class CamDriver final : public rclcpp::Node {
5494
imu_pub_->publish(imu_msg);
5595
}
5696

57-
void ImageCallback1(const void *msg, size_t) const {
58-
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
59-
std_msgs::msg::Header header;
60-
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
61-
header.frame_id = "map";
62-
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
63-
const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
64-
img1_pub_.publish(image_msg);
65-
}
66-
67-
void ImageCallback2(const void *msg, size_t) const {
97+
void ImageCallback(const void *msg, size_t) {
6898
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
6999
std_msgs::msg::Header header;
70100
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
71101
header.frame_id = "map";
72102
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
73103
const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
74-
img2_pub_.publish(image_msg);
104+
image_pubs_[cam_data->name].publish(image_msg);
75105
}
76106

77107

78108
private:
79109
infinite_sense::Synchronizer synchronizer_;
80110
SharedPtr node_handle_;
81111
image_transport::ImageTransport transport_;
82-
image_transport::Publisher img1_pub_,img2_pub_;
112+
std::shared_ptr<MvCam> mv_cam_;
113+
std::unordered_map<std::string, image_transport::Publisher> image_pubs_;
83114
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
84115
};
85116

‎example/VideoCam/main_ros.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -110,15 +110,15 @@ class CamDriver {
110110
synchronizer_.Start();
111111
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
112112

113-
114-
Messenger::GetInstance().SubStruct(
115-
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
116-
113+
if (cfg.onboard_imu) {
114+
Messenger::GetInstance().SubStruct(
115+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
116+
}
117117

118118
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));
119+
if (cfg.CAM2_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
120+
if (cfg.CAM3_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM3_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
121+
if (cfg.CAM4_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM4_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
122122
}
123123

124124
void Run() {

‎example/VideoCam/main_ros2.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -74,8 +74,10 @@ class CamDriver final : public rclcpp::Node {
7474

7575
// SDK 获取IMU和图像信息之后的回调函数
7676
{
77-
Messenger::GetInstance().SubStruct(
78-
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
77+
if (cfg.onboard_imu) {
78+
Messenger::GetInstance().SubStruct(
79+
cfg.imu_name, std::bind(&CamDriver::ImuCallback, this, std::placeholders::_1, std::placeholders::_2));
80+
}
7981

8082
if (cfg.CAM1_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM1_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));
8183
if (cfg.CAM2_Video.second >= 0) Messenger::GetInstance().SubStruct(cfg.CAM2_Video.first, std::bind(&CamDriver::ImageCallback, this, std::placeholders::_1, std::placeholders::_2));

‎example/VideoCam_LVI/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<?xml version="1.0"?>
22
<package format="3">
3-
<name>video_cam</name>
3+
<name>video_cam_lvi</name>
44
<version>1.0.0</version>
55
<description>The infinite sense camera package</description>
66
<maintainer email="[email protected]">infinite sense</maintainer>

0 commit comments

Comments
 (0)