Skip to content

Commit 09722cd

Browse files
KingRichordKingRichord
committed
closes #3 finished ros2 demo
1 parent df57683 commit 09722cd

File tree

8 files changed

+106
-111
lines changed

8 files changed

+106
-111
lines changed

‎example/CustomCam/cus_cam.cpp‎

Lines changed: 16 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -2,13 +2,9 @@
22
#include "infinite_sense.h"
33
namespace infinite_sense {
44

5-
CustomCam::~CustomCam() {
6-
Stop();
7-
}
5+
CustomCam::~CustomCam() { Stop(); }
86

9-
bool CustomCam::Initialization() {
10-
return true;
11-
}
7+
bool CustomCam::Initialization() { return true; }
128

139
void CustomCam::Stop() {
1410
Disable();
@@ -21,24 +17,24 @@ void CustomCam::Stop() {
2117
cam_threads.clear();
2218
cam_threads.shrink_to_fit();
2319
}
24-
void CustomCam::Receive(void *handle, const std::string &name) {
20+
void CustomCam::Receive(void *handle, const std::string &name) {
2521
Messenger &messenger = Messenger::GetInstance();
2622
while (is_running) {
27-
// 1. 初始化相机数据
28-
CamData cam_data;
29-
// 2. 读取相机同步时间戳
30-
if (params.find(name) != params.end()) {
31-
if (uint64_t time; GET_LAST_TRIGGER_STATUS(params[name], time)) {
32-
cam_data.time_stamp_us = time;
33-
}
23+
// 1. 初始化相机数据
24+
CamData cam_data;
25+
// 2. 读取相机同步时间戳
26+
if (params.find(name) != params.end()) {
27+
if (uint64_t time; GET_LAST_TRIGGER_STATUS(params[name], time)) {
28+
cam_data.time_stamp_us = time;
3429
}
35-
// 3. 读取相机图像
36-
cam_data.name = name;
37-
cam_data.image = GMat();
38-
// 4. 发布相机数据
39-
messenger.PubStruct(name,&cam_data,sizeof(cam_data));
40-
std::this_thread::sleep_for(std::chrono::milliseconds{1});
4130
}
31+
// 3. 读取相机图像
32+
cam_data.name = name;
33+
cam_data.image = GMat();
34+
// 4. 发布相机数据
35+
messenger.PubStruct(name, &cam_data, sizeof(cam_data));
36+
std::this_thread::sleep_for(std::chrono::milliseconds{1});
37+
}
4238
}
4339
void CustomCam::Start() {
4440
std::string name = "cus_camera";

‎example/CustomCam/main.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,8 @@ void ImageCallback(const void* msg, size_t) {
1212
int main() {
1313
// 1.创建同步器
1414
Synchronizer synchronizer;
15-
synchronizer.SetUsbLink("/dev/ttyACM0", 460800);
15+
// synchronizer.SetUsbLink("/dev/ttyACM0", 460800);
16+
synchronizer.SetNetLink("192.168.1.188", 8888);
1617
// 2.配置同步接口
1718
auto mv_cam = std::make_shared<CustomCam>();
1819
mv_cam->SetParams({

‎example/GigeCam/main.cpp‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -4,21 +4,25 @@
44
#include "mv_cam.h"
55
using namespace infinite_sense;
66
// 自定义回调函数
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] << " " << imu_data->a[2] << " Gyro: " << imu_data->g[0] << " " << imu_data->g[1] << " " << imu_data->g[2] << " Temp: " << imu_data->temperature;
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;
1012
// 处理IMU数据
1113
}
14+
1215
// 自定义回调函数
13-
void ImageCallback(const void* msg, size_t) {
14-
const auto* cam_data = static_cast<const CamData*>(msg);
16+
void ImageCallback(const void *msg, size_t) {
17+
const auto *cam_data = static_cast<const CamData *>(msg);
1518
// 处理图像数据
1619
}
20+
1721
int main() {
1822
// 1.创建同步器
1923
Synchronizer synchronizer;
2024
// synchronizer.SetUsbLink("/dev/ttyACM0", 921600);
21-
synchronizer.SetNetLink("192.168.1.188",8888);
25+
synchronizer.SetNetLink("192.168.1.188", 8888);
2226
// 2.配置同步接口
2327
auto mv_cam = std::make_shared<MvCam>();
2428
mv_cam->SetParams({{"cam_1", CAM_1}});
@@ -37,4 +41,4 @@ int main() {
3741
// 5.停止同步
3842
synchronizer.Stop();
3943
return 0;
40-
}
44+
}

‎example/GigeCam/main_ros.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,15 +4,17 @@
44
#include "mv_cam.h"
55
using namespace infinite_sense;
66
// 自定义回调函数
7-
void ImuCallback(const void* msg, size_t) {
8-
const auto* imu_data = static_cast<const ImuData*>(msg);
7+
void ImuCallback(const void *msg, size_t) {
8+
const auto *imu_data = static_cast<const ImuData *>(msg);
99
// 处理IMU数据
1010
}
11+
1112
// 自定义回调函数
12-
void ImageCallback(const void* msg, size_t) {
13-
const auto* cam_data = static_cast<const CamData*>(msg);
13+
void ImageCallback(const void *msg, size_t) {
14+
const auto *cam_data = static_cast<const CamData *>(msg);
1415
// 处理图像数据
1516
}
17+
1618
int main() {
1719
// 1.创建同步器
1820
Synchronizer synchronizer;
@@ -36,4 +38,4 @@ int main() {
3638
// 5.停止同步
3739
synchronizer.Stop();
3840
return 0;
39-
}
41+
}

‎example/GigeCam/main_ros2.cpp‎

Lines changed: 54 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -9,69 +9,65 @@
99
#include <sensor_msgs/msg/imu.hpp>
1010

1111
class CamDriver final : public rclcpp::Node {
12-
public:
13-
CamDriver()
14-
: Node("gige_driver"),
15-
node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {
16-
})),
17-
transport_(node_handle_) {
18-
std::string camera_name = "cam_1";
19-
const std::string imu_name = "imu_1";
20-
// synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
21-
synchronizer_.SetNetLink("192.168.1.188",8888);
22-
const auto mv_cam = std::make_shared<infinite_sense::MvCam>();
23-
mv_cam->SetParams({{camera_name, infinite_sense::CAM_1}});
24-
synchronizer_.UseSensor(mv_cam);
25-
synchronizer_.Start();
26-
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
27-
img_pub_ = transport_.advertise(camera_name, 10); {
28-
using namespace std::placeholders;
29-
infinite_sense::Messenger::GetInstance().SubStruct(
30-
imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
31-
infinite_sense::Messenger::GetInstance().SubStruct(camera_name,
32-
std::bind(&CamDriver::ImageCallback, this, _1, _2));
33-
}
12+
public:
13+
CamDriver()
14+
: Node("gige_driver"), node_handle_(std::shared_ptr<CamDriver>(this, [](auto *) {})), transport_(node_handle_) {
15+
std::string camera_name = "cam_1";
16+
const std::string imu_name = "imu_1";
17+
// synchronizer_.SetUsbLink("/dev/ttyACM0", 921600);
18+
synchronizer_.SetNetLink("192.168.1.188", 8888);
19+
const auto mv_cam = std::make_shared<infinite_sense::MvCam>();
20+
mv_cam->SetParams({{camera_name, infinite_sense::CAM_1}});
21+
synchronizer_.UseSensor(mv_cam);
22+
synchronizer_.Start();
23+
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_name, 10);
24+
img_pub_ = transport_.advertise(camera_name, 10);
25+
{
26+
using namespace std::placeholders;
27+
infinite_sense::Messenger::GetInstance().SubStruct(imu_name, std::bind(&CamDriver::ImuCallback, this, _1, _2));
28+
infinite_sense::Messenger::GetInstance().SubStruct(camera_name,
29+
std::bind(&CamDriver::ImageCallback, this, _1, _2));
3430
}
31+
}
3532

36-
void ImuCallback(const void *msg, size_t) const {
37-
const auto *imu_data = static_cast<const infinite_sense::ImuData *>(msg);
38-
sensor_msgs::msg::Imu imu_msg;
39-
imu_msg.header.stamp = rclcpp::Time(imu_data->time_stamp_us * 1000);
40-
imu_msg.header.frame_id = "map";
41-
imu_msg.linear_acceleration.x = imu_data->a[0];
42-
imu_msg.linear_acceleration.y = imu_data->a[1];
43-
imu_msg.linear_acceleration.z = imu_data->a[2];
44-
imu_msg.angular_velocity.x = imu_data->g[0];
45-
imu_msg.angular_velocity.y = imu_data->g[1];
46-
imu_msg.angular_velocity.z = imu_data->g[2];
47-
imu_msg.orientation.w = imu_data->q[0];
48-
imu_msg.orientation.x = imu_data->q[1];
49-
imu_msg.orientation.y = imu_data->q[2];
50-
imu_msg.orientation.z = imu_data->q[3];
51-
imu_pub_->publish(imu_msg);
52-
}
33+
void ImuCallback(const void *msg, size_t) const {
34+
const auto *imu_data = static_cast<const infinite_sense::ImuData *>(msg);
35+
sensor_msgs::msg::Imu imu_msg;
36+
imu_msg.header.stamp = rclcpp::Time(imu_data->time_stamp_us * 1000);
37+
imu_msg.header.frame_id = "map";
38+
imu_msg.linear_acceleration.x = imu_data->a[0];
39+
imu_msg.linear_acceleration.y = imu_data->a[1];
40+
imu_msg.linear_acceleration.z = imu_data->a[2];
41+
imu_msg.angular_velocity.x = imu_data->g[0];
42+
imu_msg.angular_velocity.y = imu_data->g[1];
43+
imu_msg.angular_velocity.z = imu_data->g[2];
44+
imu_msg.orientation.w = imu_data->q[0];
45+
imu_msg.orientation.x = imu_data->q[1];
46+
imu_msg.orientation.y = imu_data->q[2];
47+
imu_msg.orientation.z = imu_data->q[3];
48+
imu_pub_->publish(imu_msg);
49+
}
5350

54-
void ImageCallback(const void *msg, size_t) const {
55-
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
56-
std_msgs::msg::Header header;
57-
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
58-
header.frame_id = "map";
59-
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
60-
const sensor_msgs::msg::Image::SharedPtr image_msg =
61-
cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
62-
img_pub_.publish(image_msg);
63-
}
51+
void ImageCallback(const void *msg, size_t) const {
52+
const auto *cam_data = static_cast<const infinite_sense::CamData *>(msg);
53+
std_msgs::msg::Header header;
54+
header.stamp = rclcpp::Time(cam_data->time_stamp_us * 1000);
55+
header.frame_id = "map";
56+
const cv::Mat image_mat(cam_data->image.rows, cam_data->image.cols, CV_8UC1, cam_data->image.data);
57+
const sensor_msgs::msg::Image::SharedPtr image_msg = cv_bridge::CvImage(header, "mono8", image_mat).toImageMsg();
58+
img_pub_.publish(image_msg);
59+
}
6460

65-
private:
66-
infinite_sense::Synchronizer synchronizer_;
67-
rclcpp::Node::SharedPtr node_handle_;
68-
image_transport::ImageTransport transport_;
69-
image_transport::Publisher img_pub_;
70-
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
61+
private:
62+
infinite_sense::Synchronizer synchronizer_;
63+
SharedPtr node_handle_;
64+
image_transport::ImageTransport transport_;
65+
image_transport::Publisher img_pub_;
66+
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
7167
};
7268

7369
int main(const int argc, char *argv[]) {
74-
rclcpp::init(argc, argv);
75-
rclcpp::spin(std::make_shared<CamDriver>());
76-
return 0;
70+
rclcpp::init(argc, argv);
71+
rclcpp::spin(std::make_shared<CamDriver>());
72+
return 0;
7773
}

‎example/GigeCam/mv_cam.cpp‎

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -60,12 +60,10 @@ bool PrintDeviceInfo(const MV_CC_DEVICE_INFO *info) {
6060
LOG(INFO) << " Device Model Name : " << info->SpecialInfo.stGigEInfo.chModelName;
6161
LOG(INFO) << " Current IP Address : " << n_ip1 << "." << n_ip2 << "." << n_ip3 << "." << n_ip4;
6262
LOG(INFO) << " User Defined Name : " << info->SpecialInfo.stGigEInfo.chUserDefinedName;
63-
}
64-
else if (info->nTLayerType == MV_USB_DEVICE) {
63+
} else if (info->nTLayerType == MV_USB_DEVICE) {
6564
LOG(INFO) << " Device Model Name : " << info->SpecialInfo.stUsb3VInfo.chModelName;
6665
LOG(INFO) << " User Defined Name : " << info->SpecialInfo.stUsb3VInfo.chUserDefinedName;
67-
}
68-
else {
66+
} else {
6967
LOG(ERROR) << "[ERROR] Unsupported camera type!";
7068
return false;
7169
}
@@ -178,7 +176,7 @@ void MvCam::Stop() {
178176
LOG(INFO) << "Exit " << i << " cam ";
179177
}
180178
}
181-
void MvCam::Receive(void *handle, const std::string &name) {
179+
void MvCam::Receive(void *handle, const std::string &name) {
182180
unsigned int last_count = 0;
183181
MV_FRAME_OUT st_out_frame;
184182
CamData cam_data;
@@ -195,12 +193,10 @@ void MvCam::Receive(void *handle, const std::string &name) {
195193
// 这里的time_stamp_us是相机触发时间，需要加上曝光时间的一半，以获得相机拍摄的时间
196194
if (params.find(name) == params.end()) {
197195
LOG(ERROR) << "cam " << name << " not found!";
198-
}
199-
else {
196+
} else {
200197
if (uint64_t time; GET_LAST_TRIGGER_STATUS(params[name], time)) {
201198
cam_data.time_stamp_us = time + static_cast<uint64_t>(expose_time.fCurValue / 2.);
202-
}
203-
else {
199+
} else {
204200
LOG(ERROR) << "cam " << name << " not found!";
205201
}
206202
}
@@ -226,7 +222,7 @@ void MvCam::Receive(void *handle, const std::string &name) {
226222
cam_data.image = GMat(st_out_frame.stFrameInfo.nHeight, st_out_frame.stFrameInfo.nWidth,
227223
GMatType<uint8_t, 3>::Type, st_out_frame.pBufAddr);
228224
}
229-
messenger.PubStruct(name,&cam_data,sizeof(cam_data));
225+
messenger.PubStruct(name, &cam_data, sizeof(cam_data));
230226
if (last_count == 0) {
231227
last_count = st_out_frame.stFrameInfo.nFrameNum;
232228
} else {

‎infinite_sense_core/include/config.h‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,18 @@
44
#include "image.h"
55
namespace infinite_sense {
66
struct ImuData {
7-
uint64_t time_stamp_us; // microseconds since start of recording
8-
float temperature; // temperature in Celsius
9-
std::string name; // imu name
10-
float a[3]; // accelerometer
11-
float g[3]; // gyroscope
12-
float q[4]; // quaternion
7+
uint64_t time_stamp_us; // microseconds since start of recording
8+
float temperature; // temperature in Celsius
9+
std::string name; // imu name
10+
float a[3]; // accelerometer
11+
float g[3]; // gyroscope
12+
float q[4]; // quaternion
1313
};
1414

1515
struct CamData {
16-
uint64_t time_stamp_us; // microseconds since start of recording
17-
std::string name; // camera name
18-
GMat image; // image data
16+
uint64_t time_stamp_us; // microseconds since start of recording
17+
std::string name; // camera name
18+
GMat image; // image data
1919
};
2020

2121
struct LaserData {

‎infinite_sense_core/include/messenger.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ class Messenger {
2525
void Sub(const std::string& topic, const std::function<void(const std::string&)>& callback);
2626
void SubStruct(const std::string& topic, const std::function<void(const void*, size_t)>& callback);
2727

28-
private:
28+
private:
2929
Messenger();
3030
~Messenger();
3131
void CleanUp();

0 commit comments

Comments
 (0)