Skip to content

Commit 3afa890

Browse files
amalnanavatiamalnanavati
authored
Sync CameraInfo messages with AlignedDepth messages (#194)
* Sync CameraInfo messages with AlignedDepth messages * Fixes from in-person testing
1 parent c5cbf29 commit 3afa890

File tree

5 files changed

+210
-15
lines changed

5 files changed

+210
-15
lines changed

‎ada_feeding_perception/config/republisher.yaml

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,32 +3,27 @@ republisher:
33
ros__parameters:
44
# The name of the topics to republish from
55
from_topics:
6-
- /camera/aligned_depth_to_color/camera_info
76
- /local/camera/aligned_depth_to_color/image_raw/compressedDepth
87

98
# The types of topics to republish from
109
in_topic_types:
11-
- sensor_msgs.msg.CameraInfo
1210
- sensor_msgs.msg.CompressedImage
1311

1412
# If the republisher should convert types, specify the output type.
1513
# Currently, the republisher only supports conversions from
1614
# Image->CompressedImage and vice-versa.
1715
out_topic_types:
18-
- ""
1916
- sensor_msgs.msg.Image
2017

2118
# The name of the topics to republish to. NOTE: the `prefix` in the launchfile
2219
# must match the below pattern!
2320
to_topics:
24-
- /local/camera/aligned_depth_to_color/camera_info
2521
- /local/camera/aligned_depth_to_color/depth_octomap
2622

2723
# The target rates (Hz) for the republished topics. Rates <= 0 will publish
2824
# every message.
2925
target_rates:
3026
- 0
31-
- 0
3227

3328
# The names of a post-processing functions to apply to the message before
3429
# republishing it. Current options are:
@@ -43,7 +38,6 @@ republisher:
4338
# Any of these post-processing functions can be combined in a comma-separated list.
4439
# If an empty string, no post-processors are applied.
4540
post_processors:
46-
- ""
4741
- threshold,mask,temporal,spatial # Apply filters to the depth image for the Octomap
4842
# The binary mask used for "mask" post-processing. This mask will get scaled,
4943
# possibly disproportionately, to the same of the image.

‎ada_planning_scene/config/ada_planning_scene.yaml

Lines changed: 1 addition & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -160,12 +160,7 @@ ada_planning_scene:
160160
# from it, to get the robot URDF
161161
get_urdf_parameter_service_name: "/move_group/get_parameters"
162162
urdf_parameter_name: "robot_description"
163-
# TODO: remove!
164-
disable_workspace_wall_y_max: True # Remove the back wall, to be able to see in in RVIZ
165-
disable_workspace_wall_x_min: True # Remove the back wall, to be able to see in in RVIZ
166-
disable_workspace_wall_x_max: True # Remove the back wall, to be able to see in in RVIZ
167-
disable_workspace_wall_z_min: True # Remove the back wall, to be able to see in in RVIZ
168-
disable_workspace_wall_z_max: True # Remove the back wall, to be able to see in in RVIZ
163+
169164
# The service that needs to be called, and parameters that need to be requested,
170165
# to get the robot joint positions that must be in the workspace walls. Note that
171166
# the parameter name is made by first getting the parameter value from the

‎nano_bridge/config/nano_bridge.yaml

Lines changed: 34 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,4 +19,37 @@ sender_compressed_image:
1919

2020
receiver_compressed_image:
2121
ros__parameters:
22-
prefix: local
22+
prefix: local
23+
24+
sync_camera_info_with_topic: /camera/aligned_depth_to_color/image_raw/compressedDepth
25+
camera_info_pub_topic: /local/camera/aligned_depth_to_color/camera_info
26+
camera_info:
27+
frame_id: camera_color_optical_frame
28+
height: 480
29+
width: 640
30+
distortion_model: plumb_bob
31+
d: [0.0, 0.0, 0.0, 0.0, 0.0]
32+
k:
33+
- 614.5933227539062
34+
- 0.0
35+
- 312.1358947753906
36+
- 0.0
37+
- 614.6914672851562
38+
- 223.70831298828125
39+
- 0.0
40+
- 0.0
41+
- 1.0
42+
r: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
43+
p:
44+
- 614.5933227539062
45+
- 0.0
46+
- 312.1358947753906
47+
- 0.0
48+
- 0.0
49+
- 614.6914672851562
50+
- 223.70831298828125
51+
- 0.0
52+
- 0.0
53+
- 0.0
54+
- 1.0
55+
- 0.0

‎nano_bridge/nano_bridge/receiver_compressed_image.py

Lines changed: 174 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,10 @@
1010

1111
# Standard imports
1212
import os
13+
from typing import Optional
1314

1415
# Third-party imports
15-
from sensor_msgs.msg import CompressedImage as CompressedImageOutput
16+
from sensor_msgs.msg import CameraInfo, CompressedImage as CompressedImageOutput
1617
from rcl_interfaces.msg import ParameterDescriptor, ParameterType
1718
import rclpy
1819
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
@@ -40,10 +41,21 @@ def __init__(self) -> None:
4041

4142
# Load the parameters
4243
self.__prefix = ""
44+
self.__sync_camera_info_with_topic: Optional[str] = None
45+
self.__camera_info_pub_topic: Optional[str] = None
46+
self.__camera_info_msg = None
4347
self.__load_parameters()
4448

4549
# Create the publishers
4650
self.__pubs: dict[str, Publisher] = {}
51+
if self.__sync_camera_info_with_topic is not None:
52+
self.__pub_camera_info = self.create_publisher(
53+
msg_type=CameraInfo,
54+
topic=self.__camera_info_pub_topic,
55+
qos_profile=QoSProfile(
56+
depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
57+
),
58+
)
4759

4860
# Create the subscriber
4961
# pylint: disable=unused-private-member
@@ -71,6 +83,162 @@ def __load_parameters(self) -> None:
7183
)
7284
self.__prefix = prefix.value
7385

86+
# Camera Info
87+
sync_camera_info_with_topic = self.declare_parameter(
88+
"sync_camera_info_with_topic",
89+
None,
90+
descriptor=ParameterDescriptor(
91+
name="sync_camera_info_with_topic",
92+
type=ParameterType.PARAMETER_STRING,
93+
description=("Whether to sync camera info with topic."),
94+
read_only=True,
95+
),
96+
)
97+
self.__sync_camera_info_with_topic = sync_camera_info_with_topic.value
98+
99+
camera_info_pub_topic = self.declare_parameter(
100+
"camera_info_pub_topic",
101+
None,
102+
descriptor=ParameterDescriptor(
103+
name="camera_info_pub_topic",
104+
type=ParameterType.PARAMETER_STRING,
105+
description=("The topic to publish camera info."),
106+
read_only=True,
107+
),
108+
)
109+
self.__camera_info_pub_topic = camera_info_pub_topic.value
110+
if (
111+
self.__sync_camera_info_with_topic is not None
112+
and self.__camera_info_pub_topic is None
113+
):
114+
raise ValueError(
115+
"If sync_camera_info_with_topic is set, camera_info_pub_topic must be set."
116+
)
117+
118+
if self.__sync_camera_info_with_topic is not None:
119+
self.__camera_info_msg = CameraInfo()
120+
frame_id = self.declare_parameter(
121+
"camera_info.frame_id",
122+
"camera_color_optical_frame",
123+
descriptor=ParameterDescriptor(
124+
name="camera_info.frame_id",
125+
type=ParameterType.PARAMETER_STRING,
126+
description=("The frame ID of the camera."),
127+
read_only=True,
128+
),
129+
)
130+
self.__camera_info_msg.header.frame_id = frame_id.value
131+
height = self.declare_parameter(
132+
"camera_info.height",
133+
480,
134+
descriptor=ParameterDescriptor(
135+
name="camera_info.height",
136+
type=ParameterType.PARAMETER_INTEGER,
137+
description=("The height of the image."),
138+
read_only=True,
139+
),
140+
)
141+
self.__camera_info_msg.height = height.value
142+
width = self.declare_parameter(
143+
"camera_info.width",
144+
640,
145+
descriptor=ParameterDescriptor(
146+
name="camera_info.width",
147+
type=ParameterType.PARAMETER_INTEGER,
148+
description=("The width of the image."),
149+
read_only=True,
150+
),
151+
)
152+
self.__camera_info_msg.width = width.value
153+
distortion_model = self.declare_parameter(
154+
"camera_info.distortion_model",
155+
"plumb_bob",
156+
descriptor=ParameterDescriptor(
157+
name="camera_info.distortion_model",
158+
type=ParameterType.PARAMETER_STRING,
159+
description=("The distortion model of the camera."),
160+
read_only=True,
161+
),
162+
)
163+
self.__camera_info_msg.distortion_model = distortion_model.value
164+
d = self.declare_parameter(
165+
"camera_info.d",
166+
[0.0, 0.0, 0.0, 0.0, 0.0],
167+
descriptor=ParameterDescriptor(
168+
name="camera_info.d",
169+
type=ParameterType.PARAMETER_DOUBLE_ARRAY,
170+
description=("The distortion coefficients."),
171+
read_only=True,
172+
),
173+
)
174+
self.__camera_info_msg.d = d.value
175+
k = self.declare_parameter(
176+
"camera_info.k",
177+
[
178+
614.5933227539062,
179+
0.0,
180+
312.1358947753906,
181+
0.0,
182+
614.6914672851562,
183+
223.70831298828125,
184+
0.0,
185+
0.0,
186+
1.0,
187+
],
188+
descriptor=ParameterDescriptor(
189+
name="camera_info.k",
190+
type=ParameterType.PARAMETER_DOUBLE_ARRAY,
191+
description=("The camera matrix."),
192+
read_only=True,
193+
),
194+
)
195+
self.__camera_info_msg.k = k.value
196+
r = self.declare_parameter(
197+
"camera_info.r",
198+
[
199+
1.0,
200+
0.0,
201+
0.0,
202+
0.0,
203+
1.0,
204+
0.0,
205+
0.0,
206+
0.0,
207+
1.0,
208+
],
209+
descriptor=ParameterDescriptor(
210+
name="camera_info.r",
211+
type=ParameterType.PARAMETER_DOUBLE_ARRAY,
212+
description=("The rectification matrix."),
213+
read_only=True,
214+
),
215+
)
216+
self.__camera_info_msg.r = r.value
217+
p = self.declare_parameter(
218+
"camera_info.p",
219+
[
220+
614.5933227539062,
221+
0.0,
222+
312.1358947753906,
223+
0.0,
224+
0.0,
225+
614.6914672851562,
226+
223.70831298828125,
227+
0.0,
228+
0.0,
229+
0.0,
230+
1.0,
231+
0.0,
232+
],
233+
descriptor=ParameterDescriptor(
234+
name="camera_info.p",
235+
type=ParameterType.PARAMETER_DOUBLE_ARRAY,
236+
description=("The projection matrix."),
237+
read_only=True,
238+
),
239+
)
240+
self.__camera_info_msg.p = p.value
241+
74242
def __callback(self, msg: CompressedImageInput) -> None:
75243
"""
76244
Callback function for the subscriber.
@@ -94,6 +262,11 @@ def __callback(self, msg: CompressedImageInput) -> None:
94262
)
95263
self.get_logger().info(f"Created publisher for {repub_topic_name}.")
96264

265+
# Create the camera info message
266+
if self.__sync_camera_info_with_topic is not None:
267+
self.__camera_info_msg.header.stamp = msg.data.header.stamp
268+
self.__pub_camera_info.publish(self.__camera_info_msg)
269+
97270
# Publish the message
98271
self.__pubs[topic_name].publish(msg.data)
99272

‎nano_bridge/nano_bridge/sender_compressed_image.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ def __callback(
9494
self.__msg_recv_time[topic_name] = start_time
9595
self.__msg_count[topic_name] += 1
9696

97-
# Create the ByteMultiArray message
97+
# Create the message
9898
msg_out = CompressedImageOutput(
9999
topic=topic_name,
100100
data=msg_in,

0 commit comments

Comments
 (0)