Skip to content

Commit c291105

Browse files
lucasreljicEdwardius
lucasreljic
authored and
Edwardius
committed
Initial MultiCamera node
1 parent 4c6ca0c commit c291105

File tree

9 files changed

+999
-0
lines changed

9 files changed

+999
-0
lines changed

‎deep_msgs/CMakeLists.txt‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
# Copyright (c) 2025-present WATonomous. All rights reserved.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
cmake_minimum_required(VERSION 3.8)
15+
project(deep_msgs)
16+
17+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
18+
add_compile_options(-Wall -Wextra -Wpedantic)
19+
endif()
20+
21+
# find dependencies
22+
find_package(ament_cmake REQUIRED)
23+
find_package(rclcpp REQUIRED)
24+
find_package(std_msgs REQUIRED)
25+
find_package(sensor_msgs REQUIRED)
26+
find_package(rosidl_default_generators REQUIRED)
27+
28+
rosidl_generate_interfaces(${PROJECT_NAME}
29+
"msg/MultiImage.msg"
30+
"msg/MultiImageRaw.msg"
31+
DEPENDENCIES std_msgs sensor_msgs
32+
)
33+
34+
if(BUILD_TESTING)
35+
find_package(ament_lint_auto REQUIRED)
36+
ament_lint_auto_find_test_dependencies()
37+
endif()
38+
39+
ament_package()

‎deep_msgs/msg/MultiImage.msg‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# MultiImage.msg
2+
# A message that carries multiple compressed images together
3+
4+
std_msgs/Header header
5+
sensor_msgs/CompressedImage[] images

‎deep_msgs/msg/MultiImageRaw.msg‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# MultiImageRaw.msg
2+
std_msgs/Header header
3+
sensor_msgs/Image[] images

‎deep_msgs/package.xml‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>deep_msgs</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="lucas.reljic@gmail.com">lucas</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<depend>std_msgs</depend>
12+
<depend>sensor_msgs</depend>
13+
<build_depend>rosidl_default_generators</build_depend>
14+
<exec_depend>rosidl_default_runtime</exec_depend>
15+
<test_depend>ament_lint_auto</test_depend>
16+
<test_depend>ament_lint_common</test_depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
22+
<member_of_group>rosidl_interface_packages</member_of_group>
23+
</package>

‎deep_tools/camera_sync/CMakeLists.txt‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
# Copyright (c) 2025-present WATonomous. All rights reserved.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
cmake_minimum_required(VERSION 3.8)
15+
project(camera_sync)
16+
17+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
18+
add_compile_options(-Wall -Wextra -Wpedantic)
19+
endif()
20+
21+
# find dependencies
22+
find_package(ament_cmake REQUIRED)
23+
find_package(rclcpp REQUIRED)
24+
find_package(rclcpp_lifecycle REQUIRED)
25+
find_package(sensor_msgs REQUIRED)
26+
find_package(cv_bridge REQUIRED)
27+
find_package(message_filters REQUIRED)
28+
find_package(image_transport REQUIRED)
29+
find_package(deep_msgs REQUIRED)
30+
31+
# Create include directory
32+
include_directories(include)
33+
34+
# Add the multi-camera sync node
35+
add_executable(multi_camera_sync_node src/multi_camera_sync_node.cpp)
36+
ament_target_dependencies(multi_camera_sync_node
37+
rclcpp
38+
rclcpp_lifecycle
39+
sensor_msgs
40+
cv_bridge
41+
message_filters
42+
image_transport
43+
deep_msgs
44+
)
45+
46+
# Install executables
47+
install(TARGETS
48+
multi_camera_sync_node
49+
DESTINATION lib/${PROJECT_NAME}
50+
)
51+
52+
# Install include directory
53+
install(DIRECTORY include/
54+
DESTINATION include
55+
)
56+
57+
# Install launch files if any
58+
install(DIRECTORY launch/
59+
DESTINATION share/${PROJECT_NAME}/launch
60+
FILES_MATCHING PATTERN "*.py"
61+
)
62+
63+
if(BUILD_TESTING)
64+
find_package(ament_lint_auto REQUIRED)
65+
ament_lint_auto_find_test_dependencies()
66+
endif()
67+
68+
ament_package()

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

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
// Copyright (c) 2025-present WATonomous. All rights reserved.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CAMERA_SYNC__MULTI_CAMERA_SYNC_NODE_HPP_
16+
#define CAMERA_SYNC__MULTI_CAMERA_SYNC_NODE_HPP_
17+
18+
#include <cv_bridge/cv_bridge.h>
19+
#include <message_filters/subscriber.h>
20+
#include <message_filters/sync_policies/approximate_time.h>
21+
#include <message_filters/synchronizer.h>
22+
23+
#include <chrono>
24+
#include <memory>
25+
#include <string>
26+
#include <vector>
27+
28+
#include <image_transport/image_transport.hpp>
29+
#include <rclcpp/rclcpp.hpp>
30+
#include <sensor_msgs/msg/compressed_image.hpp>
31+
#include <sensor_msgs/msg/image.hpp>
32+
33+
#include "deep_msgs/msg/multi_image.hpp"
34+
#include "deep_msgs/msg/multi_image_raw.hpp"
35+
36+
namespace camera_sync
37+
{
38+
39+
/**
40+
* @brief Node that synchronizes N camera image messages using message filters
41+
*
42+
* This node can handle both raw images (sensor_msgs/Image) and compressed images
43+
* (sensor_msgs/CompressedImage) and synchronize them based on their timestamps.
44+
* Supports 2-6 cameras with a compact implementation.
45+
*/
46+
class MultiCameraSyncNode : public rclcpp::Node
47+
{
48+
public:
49+
explicit MultiCameraSyncNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
50+
~MultiCameraSyncNode() = default;
51+
52+
private:
53+
// Message types
54+
using ImageMsg = sensor_msgs::msg::Image;
55+
using CompressedImageMsg = sensor_msgs::msg::CompressedImage;
56+
57+
// Subscriber types
58+
using ImageSubscriber = message_filters::Subscriber<ImageMsg>;
59+
using CompressedImageSubscriber = message_filters::Subscriber<CompressedImageMsg>;
60+
61+
// Sync policies for raw images
62+
using ImageSyncPolicy2 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg>;
63+
using ImageSyncPolicy3 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg>;
64+
using ImageSyncPolicy4 = message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
65+
using ImageSyncPolicy5 =
66+
message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
67+
using ImageSyncPolicy6 =
68+
message_filters::sync_policies::ApproximateTime<ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg>;
69+
70+
// Sync policies for compressed images
71+
using CompressedSyncPolicy2 = message_filters::sync_policies::ApproximateTime<CompressedImageMsg, CompressedImageMsg>;
72+
using CompressedSyncPolicy3 =
73+
message_filters::sync_policies::ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
74+
using CompressedSyncPolicy4 = message_filters::sync_policies::
75+
ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
76+
using CompressedSyncPolicy5 = message_filters::sync_policies::
77+
ApproximateTime<CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg, CompressedImageMsg>;
78+
using CompressedSyncPolicy6 = message_filters::sync_policies::ApproximateTime<
79+
CompressedImageMsg,
80+
CompressedImageMsg,
81+
CompressedImageMsg,
82+
CompressedImageMsg,
83+
CompressedImageMsg,
84+
CompressedImageMsg>;
85+
86+
/**
87+
* @brief Initialize the node parameters
88+
*/
89+
void initializeParameters();
90+
91+
/**
92+
* @brief Setup subscribers and synchronizers
93+
*/
94+
void setupSynchronization();
95+
96+
/**
97+
* @brief Setup raw image synchronization
98+
*/
99+
void setupRawSync(size_t num_cameras);
100+
101+
/**
102+
* @brief Setup compressed image synchronization
103+
*/
104+
void setupCompressedSync(size_t num_cameras);
105+
106+
/**
107+
* @brief Callback functions for synchronized raw images
108+
*/
109+
void syncCallback2Raw(const ImageMsg::ConstSharedPtr & img1, const ImageMsg::ConstSharedPtr & img2);
110+
void syncCallback3Raw(
111+
const ImageMsg::ConstSharedPtr & img1,
112+
const ImageMsg::ConstSharedPtr & img2,
113+
const ImageMsg::ConstSharedPtr & img3);
114+
void syncCallback4Raw(
115+
const ImageMsg::ConstSharedPtr & img1,
116+
const ImageMsg::ConstSharedPtr & img2,
117+
const ImageMsg::ConstSharedPtr & img3,
118+
const ImageMsg::ConstSharedPtr & img4);
119+
void syncCallback5Raw(
120+
const ImageMsg::ConstSharedPtr & img1,
121+
const ImageMsg::ConstSharedPtr & img2,
122+
const ImageMsg::ConstSharedPtr & img3,
123+
const ImageMsg::ConstSharedPtr & img4,
124+
const ImageMsg::ConstSharedPtr & img5);
125+
void syncCallback6Raw(
126+
const ImageMsg::ConstSharedPtr & img1,
127+
const ImageMsg::ConstSharedPtr & img2,
128+
const ImageMsg::ConstSharedPtr & img3,
129+
const ImageMsg::ConstSharedPtr & img4,
130+
const ImageMsg::ConstSharedPtr & img5,
131+
const ImageMsg::ConstSharedPtr & img6);
132+
133+
/**
134+
* @brief Callback functions for synchronized compressed images
135+
*/
136+
void syncCallback2Compressed(
137+
const CompressedImageMsg::ConstSharedPtr & img1, const CompressedImageMsg::ConstSharedPtr & img2);
138+
void syncCallback3Compressed(
139+
const CompressedImageMsg::ConstSharedPtr & img1,
140+
const CompressedImageMsg::ConstSharedPtr & img2,
141+
const CompressedImageMsg::ConstSharedPtr & img3);
142+
void syncCallback4Compressed(
143+
const CompressedImageMsg::ConstSharedPtr & img1,
144+
const CompressedImageMsg::ConstSharedPtr & img2,
145+
const CompressedImageMsg::ConstSharedPtr & img3,
146+
const CompressedImageMsg::ConstSharedPtr & img4);
147+
void syncCallback5Compressed(
148+
const CompressedImageMsg::ConstSharedPtr & img1,
149+
const CompressedImageMsg::ConstSharedPtr & img2,
150+
const CompressedImageMsg::ConstSharedPtr & img3,
151+
const CompressedImageMsg::ConstSharedPtr & img4,
152+
const CompressedImageMsg::ConstSharedPtr & img5);
153+
void syncCallback6Compressed(
154+
const CompressedImageMsg::ConstSharedPtr & img1,
155+
const CompressedImageMsg::ConstSharedPtr & img2,
156+
const CompressedImageMsg::ConstSharedPtr & img3,
157+
const CompressedImageMsg::ConstSharedPtr & img4,
158+
const CompressedImageMsg::ConstSharedPtr & img5,
159+
const CompressedImageMsg::ConstSharedPtr & img6);
160+
161+
/**
162+
* @brief Process synchronized images (statistics and custom logic)
163+
* @param timestamps Vector of synchronized timestamps from all cameras
164+
*/
165+
void processSynchronizedImages(const std::vector<rclcpp::Time> & timestamps);
166+
167+
// Parameters
168+
std::vector<std::string> camera_topics_;
169+
std::vector<std::string> camera_names_;
170+
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> image_array;
171+
bool use_compressed_;
172+
double sync_tolerance_ms_;
173+
int queue_size_;
174+
bool publish_sync_info_;
175+
176+
// Subscribers
177+
std::vector<std::unique_ptr<ImageSubscriber>> image_subscribers_;
178+
std::vector<std::unique_ptr<CompressedImageSubscriber>> compressed_subscribers_;
179+
180+
// Synchronizers for raw images
181+
std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy2>> sync2_raw_;
182+
std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy3>> sync3_raw_;
183+
std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy4>> sync4_raw_;
184+
std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy5>> sync5_raw_;
185+
std::unique_ptr<message_filters::Synchronizer<ImageSyncPolicy6>> sync6_raw_;
186+
187+
// Synchronizers for compressed images
188+
std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy2>> sync2_compressed_;
189+
std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy3>> sync3_compressed_;
190+
std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy4>> sync4_compressed_;
191+
std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy5>> sync5_compressed_;
192+
std::unique_ptr<message_filters::Synchronizer<CompressedSyncPolicy6>> sync6_compressed_;
193+
194+
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr sync_info_pub_;
195+
196+
// Publishers for multi-image messages
197+
rclcpp::Publisher<deep_msgs::msg::MultiImageRaw>::SharedPtr multi_image_raw_pub_;
198+
rclcpp::Publisher<deep_msgs::msg::MultiImage>::SharedPtr multi_image_compressed_pub_;
199+
200+
// Statistics
201+
int64_t sync_count_;
202+
rclcpp::Time last_sync_time_;
203+
std::chrono::steady_clock::time_point start_time_;
204+
};
205+
206+
} // namespace camera_sync
207+
208+
#endif // camera_sync__MULTI_CAMERA_SYNC_NODE_HPP_

0 commit comments

Comments
 (0)