Skip to content

Commit fe3cbfe

Browse files
lucasreljiclucasreljic
committed
pre-commit fix
1 parent b19907e commit fe3cbfe

File tree

3 files changed

+110
-48
lines changed

3 files changed

+110
-48
lines changed

‎deep_tools/camera_sync/config/multi_camera_sync_params.yaml‎

Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,10 +24,34 @@ multi_camera_sync:
2424
# Camera topics to synchronize
2525
# For raw images, use topics like: ["/camera1/image_raw", "/camera2/image_raw"]
2626
# For compressed images, use topics like: ["/camera1/image_raw/compressed", "/camera2/image_raw/compressed"]
27-
camera_topics: ["/camera_lower_sw/image_rect", "/camera_lower_ne/image_rect", "/camera_lower_se/image_rect", "/camera_lower_nw/image_rect", "/camera_pano_ww/image_rect", "/camera_pano_nn/image_rect", "/camera_pano_ee/image_rect", "/camera_pano_se/image_rect", "/camera_pano_ne/image_rect", "/camera_pano_nw/image_rect", "/camera_pano_ss/image_rect", "/camera_pano_sw/image_rect"]
27+
camera_topics:
28+
- "/camera_lower_sw/image_rect"
29+
- "/camera_lower_ne/image_rect"
30+
- "/camera_lower_se/image_rect"
31+
- "/camera_lower_nw/image_rect"
32+
- "/camera_pano_ww/image_rect"
33+
- "/camera_pano_nn/image_rect"
34+
- "/camera_pano_ee/image_rect"
35+
- "/camera_pano_se/image_rect"
36+
- "/camera_pano_ne/image_rect"
37+
- "/camera_pano_nw/image_rect"
38+
- "/camera_pano_ss/image_rect"
39+
- "/camera_pano_sw/image_rect"
2840

2941
# Optional names for the cameras (if not provided, will auto-generate camera_1, camera_2, etc.)
30-
camera_names: ["camera_lower_sw", "camera_lower_ne", "camera_lower_se", "camera_lower_nw", "camera_pano_ww", "camera_pano_nn", "camera_pano_ee", "camera_pano_se", "camera_pano_ne", "camera_pano_nw", "camera_pano_ss", "camera_pano_sw"]
42+
camera_names:
43+
- "camera_lower_sw"
44+
- "camera_lower_ne"
45+
- "camera_lower_se"
46+
- "camera_lower_nw"
47+
- "camera_pano_ww"
48+
- "camera_pano_nn"
49+
- "camera_pano_ee"
50+
- "camera_pano_se"
51+
- "camera_pano_ne"
52+
- "camera_pano_nw"
53+
- "camera_pano_ss"
54+
- "camera_pano_sw"
3155

3256
# Whether to use compressed images (sensor_msgs/CompressedImage) instead of raw (sensor_msgs/Image)
3357
# Set to true if your camera topics publish compressed images

‎deep_tools/camera_sync/include/camera_sync/multi_camera_sync_node.hpp‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -243,16 +243,24 @@ class MultiCameraSyncNode : public rclcpp::Node
243243
std::chrono::steady_clock::time_point start_time_;
244244

245245
// For 12-camera manual synchronization (message_filters max is 9)
246-
struct ImageBuffer {
246+
struct ImageBuffer
247+
{
247248
std::map<uint64_t, ImageMsg::ConstSharedPtr> buffer;
248249
std::shared_ptr<std::mutex> mutex;
249-
ImageBuffer() : mutex(std::make_shared<std::mutex>()) {}
250+
251+
ImageBuffer()
252+
: mutex(std::make_shared<std::mutex>())
253+
{}
250254
};
251255

252-
struct CompressedImageBuffer {
256+
struct CompressedImageBuffer
257+
{
253258
std::map<uint64_t, CompressedImageMsg::ConstSharedPtr> buffer;
254259
std::shared_ptr<std::mutex> mutex;
255-
CompressedImageBuffer() : mutex(std::make_shared<std::mutex>()) {}
260+
261+
CompressedImageBuffer()
262+
: mutex(std::make_shared<std::mutex>())
263+
{}
256264
};
257265

258266
std::vector<ImageBuffer> raw_image_buffers_;

‎deep_tools/camera_sync/src/multi_camera_sync_node.cpp‎

Lines changed: 72 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
#include <memory>
2121
#include <sstream>
2222
#include <string>
23+
#include <utility>
2324
#include <vector>
2425

2526
#include <rclcpp_components/register_node_macro.hpp>
@@ -200,9 +201,7 @@ void MultiCameraSyncNode::setupRawSync(size_t num_cameras)
200201
for (size_t i = 0; i < 12; ++i) {
201202
raw_image_buffers_.push_back({});
202203
image_subscribers_[i]->registerCallback(
203-
[this, i](const ImageMsg::ConstSharedPtr & msg) {
204-
this->handleRawImageCallback12(i, msg);
205-
});
204+
[this, i](const ImageMsg::ConstSharedPtr & msg) { this->handleRawImageCallback12(i, msg); });
206205
}
207206
break;
208207
}
@@ -298,9 +297,7 @@ void MultiCameraSyncNode::setupCompressedSync(size_t num_cameras)
298297
for (size_t i = 0; i < 12; ++i) {
299298
compressed_image_buffers_.push_back({});
300299
compressed_subscribers_[i]->registerCallback(
301-
[this, i](const CompressedImageMsg::ConstSharedPtr & msg) {
302-
this->handleCompressedImageCallback12(i, msg);
303-
});
300+
[this, i](const CompressedImageMsg::ConstSharedPtr & msg) { this->handleCompressedImageCallback12(i, msg); });
304301
}
305302
break;
306303
}
@@ -615,21 +612,25 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn MultiC
615612
}
616613
#endif
617614

618-
void MultiCameraSyncNode::handleRawImageCallback12(
619-
size_t camera_idx, const ImageMsg::ConstSharedPtr & msg)
615+
void MultiCameraSyncNode::handleRawImageCallback12(size_t camera_idx, const ImageMsg::ConstSharedPtr & msg)
620616
{
621617
if (camera_idx >= raw_image_buffers_.size()) {
622-
RCLCPP_WARN(this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, raw_image_buffers_.size());
618+
RCLCPP_WARN(
619+
this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, raw_image_buffers_.size());
623620
return;
624621
}
625622

626623
{
627624
std::lock_guard<std::mutex> lock(*raw_image_buffers_[camera_idx].mutex);
628625
uint64_t msg_time_ns = msg->header.stamp.sec * 1000000000ULL + msg->header.stamp.nanosec;
629626
raw_image_buffers_[camera_idx].buffer[msg_time_ns] = msg;
630-
631-
RCLCPP_DEBUG(this->get_logger(), "Camera %zu: received image at time %lu (buffer size: %zu)",
632-
camera_idx, msg_time_ns, raw_image_buffers_[camera_idx].buffer.size());
627+
628+
RCLCPP_DEBUG(
629+
this->get_logger(),
630+
"Camera %zu: received image at time %lu (buffer size: %zu)",
631+
camera_idx,
632+
msg_time_ns,
633+
raw_image_buffers_[camera_idx].buffer.size());
633634

634635
// Clean old messages outside tolerance window
635636
auto it = raw_image_buffers_[camera_idx].buffer.begin();
@@ -650,17 +651,22 @@ void MultiCameraSyncNode::handleCompressedImageCallback12(
650651
size_t camera_idx, const CompressedImageMsg::ConstSharedPtr & msg)
651652
{
652653
if (camera_idx >= compressed_image_buffers_.size()) {
653-
RCLCPP_WARN(this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, compressed_image_buffers_.size());
654+
RCLCPP_WARN(
655+
this->get_logger(), "Camera index %zu out of bounds (size: %zu)", camera_idx, compressed_image_buffers_.size());
654656
return;
655657
}
656658

657659
{
658660
std::lock_guard<std::mutex> lock(*compressed_image_buffers_[camera_idx].mutex);
659661
uint64_t msg_time_ns = msg->header.stamp.sec * 1000000000ULL + msg->header.stamp.nanosec;
660662
compressed_image_buffers_[camera_idx].buffer[msg_time_ns] = msg;
661-
662-
RCLCPP_DEBUG(this->get_logger(), "Camera %zu: received compressed image at time %lu (buffer size: %zu)",
663-
camera_idx, msg_time_ns, compressed_image_buffers_[camera_idx].buffer.size());
663+
664+
RCLCPP_DEBUG(
665+
this->get_logger(),
666+
"Camera %zu: received compressed image at time %lu (buffer size: %zu)",
667+
camera_idx,
668+
msg_time_ns,
669+
compressed_image_buffers_[camera_idx].buffer.size());
664670

665671
// Clean old messages outside tolerance window
666672
auto it = compressed_image_buffers_[camera_idx].buffer.begin();
@@ -698,9 +704,9 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Raw()
698704

699705
// Use the most recent timestamp from camera 0 as reference
700706
sync_time_ns = raw_image_buffers_[0].buffer.rbegin()->first;
701-
707+
702708
RCLCPP_DEBUG(this->get_logger(), "Trying to sync with reference time %lu", sync_time_ns);
703-
709+
704710
// Log buffer sizes
705711
for (size_t i = 0; i < raw_image_buffers_.size(); ++i) {
706712
RCLCPP_DEBUG(this->get_logger(), " Buffer %zu size: %zu", i, raw_image_buffers_[i].buffer.size());
@@ -712,13 +718,13 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Raw()
712718
int64_t tolerance_ns = static_cast<int64_t>(sync_tolerance_ms_ * 1e6);
713719
int64_t lower_bound_ns = static_cast<int64_t>(sync_time_ns) - tolerance_ns;
714720
int64_t upper_bound_ns = static_cast<int64_t>(sync_time_ns) + tolerance_ns;
715-
721+
716722
// Find the closest timestamp within tolerance window
717723
auto it = buffer.lower_bound(lower_bound_ns >= 0 ? static_cast<uint64_t>(lower_bound_ns) : 0);
718-
724+
719725
ImageMsg::ConstSharedPtr best_match = nullptr;
720726
int64_t best_time_diff = INT64_MAX;
721-
727+
722728
// Search for the closest match within the tolerance window
723729
while (it != buffer.end() && static_cast<int64_t>(it->first) <= upper_bound_ns) {
724730
int64_t time_diff_ns = std::abs(static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(it->first));
@@ -728,7 +734,7 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Raw()
728734
}
729735
++it;
730736
}
731-
737+
732738
if (best_match != nullptr && best_time_diff <= tolerance_ns) {
733739
synced_images.push_back(best_match);
734740
// Find the timestamp key for logging
@@ -737,14 +743,21 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Raw()
737743
uint32_t sec = pair.first / 1000000000ULL;
738744
uint32_t nanosec = pair.first % 1000000000ULL;
739745
timestamps.push_back(rclcpp::Time(sec, nanosec));
740-
RCLCPP_DEBUG(this->get_logger(), " Camera %zu: found match (time diff: %ld ns)", i,
741-
static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(pair.first));
746+
RCLCPP_DEBUG(
747+
this->get_logger(),
748+
" Camera %zu: found match (time diff: %ld ns)",
749+
i,
750+
static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(pair.first));
742751
break;
743752
}
744753
}
745754
} else {
746-
RCLCPP_DEBUG(this->get_logger(), " Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)",
747-
i, best_time_diff, tolerance_ns);
755+
RCLCPP_DEBUG(
756+
this->get_logger(),
757+
" Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)",
758+
i,
759+
best_time_diff,
760+
tolerance_ns);
748761
all_found = false;
749762
break;
750763
}
@@ -756,8 +769,12 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Raw()
756769
}
757770

758771
if (!all_found || synced_images.size() != raw_image_buffers_.size()) {
759-
RCLCPP_DEBUG(this->get_logger(), "Failed to sync: all_found=%d, synced_size=%zu, total_cameras=%zu",
760-
all_found, synced_images.size(), raw_image_buffers_.size());
772+
RCLCPP_DEBUG(
773+
this->get_logger(),
774+
"Failed to sync: all_found=%d, synced_size=%zu, total_cameras=%zu",
775+
all_found,
776+
synced_images.size(),
777+
raw_image_buffers_.size());
761778
return;
762779
}
763780

@@ -789,12 +806,13 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Compressed()
789806

790807
// Use the most recent timestamp from camera 0 as reference
791808
sync_time_ns = compressed_image_buffers_[0].buffer.rbegin()->first;
792-
809+
793810
RCLCPP_DEBUG(this->get_logger(), "Trying to sync compressed with reference time %lu", sync_time_ns);
794-
811+
795812
// Log buffer sizes
796813
for (size_t i = 0; i < compressed_image_buffers_.size(); ++i) {
797-
RCLCPP_DEBUG(this->get_logger(), " Compressed buffer %zu size: %zu", i, compressed_image_buffers_[i].buffer.size());
814+
RCLCPP_DEBUG(
815+
this->get_logger(), " Compressed buffer %zu size: %zu", i, compressed_image_buffers_[i].buffer.size());
798816
}
799817

800818
bool all_found = true;
@@ -803,13 +821,13 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Compressed()
803821
int64_t tolerance_ns = static_cast<int64_t>(sync_tolerance_ms_ * 1e6);
804822
int64_t lower_bound_ns = static_cast<int64_t>(sync_time_ns) - tolerance_ns;
805823
int64_t upper_bound_ns = static_cast<int64_t>(sync_time_ns) + tolerance_ns;
806-
824+
807825
// Find the closest timestamp within tolerance window
808826
auto it = buffer.lower_bound(lower_bound_ns >= 0 ? static_cast<uint64_t>(lower_bound_ns) : 0);
809-
827+
810828
CompressedImageMsg::ConstSharedPtr best_match = nullptr;
811829
int64_t best_time_diff = INT64_MAX;
812-
830+
813831
// Search for the closest match within the tolerance window
814832
while (it != buffer.end() && static_cast<int64_t>(it->first) <= upper_bound_ns) {
815833
int64_t time_diff_ns = std::abs(static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(it->first));
@@ -819,7 +837,7 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Compressed()
819837
}
820838
++it;
821839
}
822-
840+
823841
if (best_match != nullptr && best_time_diff <= tolerance_ns) {
824842
synced_images.push_back(best_match);
825843
// Find the timestamp key for logging
@@ -828,14 +846,21 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Compressed()
828846
uint32_t sec = pair.first / 1000000000ULL;
829847
uint32_t nanosec = pair.first % 1000000000ULL;
830848
timestamps.push_back(rclcpp::Time(sec, nanosec));
831-
RCLCPP_DEBUG(this->get_logger(), " Camera %zu: found match (time diff: %ld ns)", i,
832-
static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(pair.first));
849+
RCLCPP_DEBUG(
850+
this->get_logger(),
851+
" Camera %zu: found match (time diff: %ld ns)",
852+
i,
853+
static_cast<int64_t>(sync_time_ns) - static_cast<int64_t>(pair.first));
833854
break;
834855
}
835856
}
836857
} else {
837-
RCLCPP_DEBUG(this->get_logger(), " Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)",
838-
i, best_time_diff, tolerance_ns);
858+
RCLCPP_DEBUG(
859+
this->get_logger(),
860+
" Camera %zu: no match in tolerance window (best diff: %ld ns, tolerance: %ld ns)",
861+
i,
862+
best_time_diff,
863+
tolerance_ns);
839864
all_found = false;
840865
break;
841866
}
@@ -847,15 +872,20 @@ void MultiCameraSyncNode::tryPublishSyncedImages12Compressed()
847872
}
848873

849874
if (!all_found || synced_images.size() != compressed_image_buffers_.size()) {
850-
RCLCPP_DEBUG(this->get_logger(), "Failed to sync compressed: all_found=%d, synced_size=%zu, total_cameras=%zu",
851-
all_found, synced_images.size(), compressed_image_buffers_.size());
875+
RCLCPP_DEBUG(
876+
this->get_logger(),
877+
"Failed to sync compressed: all_found=%d, synced_size=%zu, total_cameras=%zu",
878+
all_found,
879+
synced_images.size(),
880+
compressed_image_buffers_.size());
852881
return;
853882
}
854883

855884
// Publish synced images
856885
RCLCPP_DEBUG(this->get_logger(), "Publishing synced compressed images (sync count: %ld)", ++sync_count_);
857886
processSynchronizedImages(timestamps);
858-
auto compressed_msg = createMultiImageMessage<sensor_msgs::msg::CompressedImage, deep_msgs::msg::MultiImage>(synced_images);
887+
auto compressed_msg =
888+
createMultiImageMessage<sensor_msgs::msg::CompressedImage, deep_msgs::msg::MultiImage>(synced_images);
859889
multi_image_compressed_pub_->publish(compressed_msg);
860890
}
861891

0 commit comments

Comments
 (0)