Skip to content

Commit 47a7ad9

Browse files
KingRichordKingRichord
committed
add some commit
1 parent a99cac4 commit 47a7ad9

5 files changed

Lines changed: 50 additions & 53 deletions

File tree

‎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;
8-
float temperature;
9-
std::string name;
10-
float a[3];
11-
float g[3];
12-
float q[4];
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;
17-
std::string name;
18-
GMat image;
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/data.h‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,15 +10,14 @@ inline void ProcessTriggerData(const nlohmann::json &data) {
1010
const uint64_t time_stamp = data["t"];
1111
const uint16_t status = data["s"];
1212
SET_LAST_TRIGGER_STATUS(time_stamp, status);
13-
};
13+
}
1414

1515
inline void ProcessIMUData(const nlohmann::json &data) {
1616
if (data["f"] != "imu") {
1717
return;
1818
}
1919
ImuData imu{};
20-
const uint64_t time_stamp = data["t"];
21-
imu.time_stamp_us = time_stamp;
20+
imu.time_stamp_us = data["t"];
2221
imu.a[0] = data["d"][0];
2322
imu.a[1] = data["d"][1];
2423
imu.a[2] = data["d"][2];
@@ -44,13 +43,13 @@ inline void ProcessGPSData(const nlohmann::json &data) {
4443
gps.gps_stamp_us_trigger = data["d"][3];
4544
gps.time_stamp_us = data["t"];
4645
Messenger::GetInstance().PubStruct("gps", &gps, sizeof(gps));
47-
};
46+
}
4847

4948
inline void ProcessLOGData(const nlohmann::json &data) {
5049
if (data["f"] != "log") {
5150
return;
5251
}
5352
LOG(data["l"]) << data["msg"];
54-
};
53+
}
5554

5655
} // namespace infinite_sense

‎infinite_sense_core/src/messenger.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ void Messenger::CleanUp() {
3333
}
3434
}
3535
} catch (...) {
36-
// 忽略析构期异常
36+
LOG(ERROR) << "Messenger clean up error";
3737
}
3838
}
3939

@@ -73,8 +73,9 @@ void Messenger::Sub(const std::string& topic, const std::function<void(const std
7373
}
7474

7575
std::string received_topic(static_cast<char*>(topic_msg.data()), topic_msg.size());
76-
if (received_topic != topic) continue;
77-
76+
if (received_topic != topic) {
77+
continue;
78+
}
7879
std::string data(static_cast<char*>(data_msg.data()), data_msg.size());
7980
callback(data);
8081
}
@@ -100,8 +101,9 @@ void Messenger::SubStruct(const std::string& topic, const std::function<void(con
100101
}
101102

102103
std::string received_topic(static_cast<char*>(topic_msg.data()), topic_msg.size());
103-
if (received_topic != topic) continue;
104-
104+
if (received_topic != topic) {
105+
continue;
106+
}
105107
callback(data_msg.data(), data_msg.size());
106108
}
107109
} catch (const zmq::error_t& e) {

‎infinite_sense_core/src/net.cpp‎

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,10 @@ namespace infinite_sense {
77

88
NetManager::NetManager(std::string target_ip, unsigned short port) : port_(port), target_ip_(std::move(target_ip)) {
99
net_ptr_ = std::make_shared<UDPSocket>();
10-
1110
const uint64_t curr_time = static_cast<uint64_t>(
1211
std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now().time_since_epoch())
1312
.count());
14-
1513
net_ptr_->sendTo(reinterpret_cast<const uint8_t*>(&curr_time), sizeof(curr_time), target_ip_, port_);
16-
1714
ptp_ = std::make_unique<Ptp>();
1815
ptp_->SetNetPtr(net_ptr_, target_ip_, port_);
1916
}
@@ -27,22 +24,26 @@ NetManager::~NetManager() {
2724
}
2825

2926
void NetManager::Start() {
30-
if (started_) return;
27+
if (started_) {
28+
return;
29+
}
3130
started_ = true;
32-
3331
rx_thread_ = std::thread(&NetManager::Receive, this);
3432
tx_thread_ = std::thread(&NetManager::TimeStampSynchronization, this);
35-
3633
LOG(INFO) << "Net manager started";
3734
}
3835

3936
void NetManager::Stop() {
40-
if (!started_) return;
37+
if (!started_) {
38+
return;
39+
}
4140
started_ = false;
42-
43-
if (rx_thread_.joinable()) rx_thread_.join();
44-
if (tx_thread_.joinable()) tx_thread_.join();
45-
41+
if (rx_thread_.joinable()) {
42+
rx_thread_.join();
43+
}
44+
if (tx_thread_.joinable()) {
45+
tx_thread_.join();
46+
}
4647
LOG(INFO) << "Net manager stopped";
4748
}
4849

@@ -61,14 +62,14 @@ void NetManager::Receive() const {
6162

6263
try {
6364
std::string recv_data(reinterpret_cast<char*>(buffer.data()), size);
64-
if (recv_data.empty()) continue;
65-
65+
if (recv_data.empty()) {
66+
continue;
67+
}
6668
auto json_data = nlohmann::json::parse(recv_data, nullptr, false);
6769
if (json_data.is_discarded()) {
6870
LOG(WARNING) << "Received malformed JSON";
6971
continue;
7072
}
71-
7273
ptp_->ReceivePtpData(json_data);
7374
ProcessTriggerData(json_data);
7475
ProcessIMUData(json_data);

‎infinite_sense_core/src/usb.cpp‎

Lines changed: 15 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -8,22 +8,19 @@ namespace infinite_sense {
88
UsbManager::UsbManager(std::string port, int baud_rate)
99
: port_(std::move(port)), started_(false) {
1010
serial_ptr_ = std::make_unique<serial::Serial>();
11-
1211
try {
1312
serial_ptr_->setPort(port_);
1413
serial_ptr_->setBaudrate(baud_rate);
1514
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
1615
serial_ptr_->setTimeout(to);
1716
serial_ptr_->open();
18-
1917
if (serial_ptr_->isOpen()) {
2018
serial_ptr_->flush();
2119
LOG(INFO) << "Serial port " << port_ << " opened successfully.";
2220
} else {
2321
LOG(ERROR) << "Failed to open serial port: " << port_;
2422
return;
2523
}
26-
2724
ptp_ = std::make_unique<Ptp>();
2825
ptp_->SetUsbPtr(serial_ptr_);
2926
} catch (const serial::IOException& e) {
@@ -43,55 +40,53 @@ void UsbManager::Start() {
4340
LOG(ERROR) << "Cannot start USB manager: serial port not open.";
4441
return;
4542
}
46-
4743
started_ = true;
4844
rx_thread_ = std::thread(&UsbManager::Receive, this);
4945
tx_thread_ = std::thread(&UsbManager::TimeStampSynchronization, this);
50-
5146
LOG(INFO) << "USB manager started";
5247
}
5348

5449
void UsbManager::Stop() {
55-
if (!started_) return;
56-
50+
if (!started_) {
51+
return;
52+
}
5753
started_ = false;
58-
59-
if (rx_thread_.joinable()) rx_thread_.join();
60-
if (tx_thread_.joinable()) tx_thread_.join();
61-
54+
if (rx_thread_.joinable()) {
55+
rx_thread_.join();
56+
}
57+
if (tx_thread_.joinable()) {
58+
tx_thread_.join();
59+
}
6260
if (serial_ptr_ && serial_ptr_->isOpen()) {
6361
serial_ptr_->close();
6462
LOG(INFO) << "Serial port " << port_ << " closed.";
6563
}
66-
6764
LOG(INFO) << "USB manager stopped";
6865
}
6966

7067
void UsbManager::Receive() const {
7168
while (started_) {
7269
try {
73-
if (!serial_ptr_ || !serial_ptr_->isOpen()) break;
74-
70+
if (!serial_ptr_ || !serial_ptr_->isOpen()) {
71+
break;
72+
}
7573
if (serial_ptr_->available()) {
7674
const std::string serial_recv = serial_ptr_->readline();
77-
78-
if (serial_recv.empty()) continue;
79-
75+
if (serial_recv.empty()) {
76+
continue;
77+
}
8078
auto json_data = nlohmann::json::parse(serial_recv, nullptr, false);
8179
if (json_data.is_discarded()) {
8280
LOG(WARNING) << "Received malformed JSON: " << serial_recv;
8381
continue;
8482
}
85-
8683
ptp_->ReceivePtpData(json_data);
8784
ProcessTriggerData(json_data);
8885
ProcessIMUData(json_data);
8986
ProcessGPSData(json_data);
9087
ProcessLOGData(json_data);
9188
}
92-
9389
std::this_thread::sleep_for(std::chrono::milliseconds(1));
94-
9590
} catch (const std::exception& e) {
9691
LOG(ERROR) << "Receive thread exception: " << e.what();
9792
}

0 commit comments

Comments
 (0)