Skip to content

Commit 665f6ff

Browse files
IrvingF7IrvingF7
committed
modified JSON translation error
1 parent 58cd433 commit 665f6ff

File tree

4 files changed

+53
-23
lines changed

4 files changed

+53
-23
lines changed

‎realsense_capture/config/calibration.yaml

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,12 +6,12 @@ mount_translation_z: 41.1
66
d405_height: 23.0
77
d405_center2left: 9.0
88

9-
w: 848
10-
h: 480
11-
fl_x: 424.861
12-
fl_y: 424.861
13-
cx: 421.818
14-
cy: 241.523
9+
w: 1280
10+
h: 720
11+
fl_x: 641.299
12+
fl_y: 641.299
13+
cx: 636.707
14+
cy: 362.299
1515
k1: 0.0
1616
k2: 0.0
1717
p1: 0.0

‎realsense_capture/launch/realsense_sys_launch.py

Lines changed: 8 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -35,20 +35,21 @@
3535
{'name': 'device_type', 'default': "''", 'description': 'choose device by type'},
3636
{'name': 'config_file', 'default': "''", 'description': 'yaml config file'},
3737
{'name': 'json_file_path', 'default': "''", 'description': 'allows advanced configuration'},
38-
{'name': 'initial_reset', 'default': 'false', 'description': "''"},
38+
{'name': 'initial_reset', 'default': 'true', 'description': "''"},
3939
{'name': 'accelerate_gpu_with_glsl', 'default': "false", 'description': 'enable GPU acceleration with GLSL'},
4040
{'name': 'rosbag_filename', 'default': "''", 'description': 'A realsense bagfile to run from as a device'},
4141
{'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
4242
{'name': 'output', 'default': 'screen', 'description': 'pipe node output [screen|log]'},
4343
{'name': 'enable_color', 'default': 'true', 'description': 'enable color stream'},
44-
{'name': 'rgb_camera.color_profile', 'default': '0,0,0', 'description': 'color stream profile'},
44+
{'name': 'rgb_camera.color_profile', 'default': '1280x720x30', 'description': 'color stream profile'},
4545
{'name': 'rgb_camera.color_format', 'default': 'RGB8', 'description': 'color stream format'},
4646
{'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'},
4747
{'name': 'enable_depth', 'default': 'true', 'description': 'enable depth stream'},
4848
{'name': 'enable_infra', 'default': 'false', 'description': 'enable infra0 stream'},
4949
{'name': 'enable_infra1', 'default': 'false', 'description': 'enable infra1 stream'},
5050
{'name': 'enable_infra2', 'default': 'false', 'description': 'enable infra2 stream'},
51-
{'name': 'depth_module.depth_profile', 'default': '0,0,0', 'description': 'depth stream profile'},
51+
{'name': 'depth_module.color_profile', 'default': '1280x720x30', 'description': 'depth stream color profile'},
52+
{'name': 'depth_module.depth_profile', 'default': '1280x720x30', 'description': 'depth stream profile'},
5253
{'name': 'depth_module.depth_format', 'default': 'Z16', 'description': 'depth stream format'},
5354
{'name': 'depth_module.infra_profile', 'default': '0,0,0', 'description': 'infra streams (0/1/2) profile'},
5455
{'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'},
@@ -82,12 +83,12 @@
8283
{'name': 'pointcloud.allow_no_texture_points', 'default': 'false', 'description': "''"},
8384
{'name': 'align_depth.enable', 'default': 'true', 'description': 'enable align depth filter'},
8485
{'name': 'colorizer.enable', 'default': 'false', 'description': 'enable colorizer filter'},
85-
{'name': 'decimation_filter.enable', 'default': 'false', 'description': 'enable_decimation_filter'},
86-
{'name': 'spatial_filter.enable', 'default': 'false', 'description': 'enable_spatial_filter'},
87-
{'name': 'temporal_filter.enable', 'default': 'false', 'description': 'enable_temporal_filter'},
86+
{'name': 'decimation_filter.enable', 'default': 'true', 'description': 'enable_decimation_filter'},
87+
{'name': 'spatial_filter.enable', 'default': 'true', 'description': 'enable_spatial_filter'},
88+
{'name': 'temporal_filter.enable', 'default': 'true', 'description': 'enable_temporal_filter'},
8889
{'name': 'disparity_filter.enable', 'default': 'false', 'description': 'enable_disparity_filter'},
8990
{'name': 'hole_filling_filter.enable', 'default': 'false', 'description': 'enable_hole_filling_filter'},
90-
{'name': 'hdr_merge.enable', 'default': 'false', 'description': 'hdr_merge filter enablement flag'},
91+
{'name': 'hdr_merge.enable', 'default': 'true', 'description': 'hdr_merge filter enablement flag'},
9192
{'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
9293
{'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
9394

‎realsense_capture/realsense_capture/realsense_image_client.py

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,8 @@ def __init__(self):
4949
self.depth_save_folder = os.path.join(self.save_folder, 'depth')
5050
os.makedirs(self.depth_save_folder, exist_ok=True)
5151

52-
self.image_count = 0
52+
self.rgb_count = 0
53+
self.depth_count = 0
5354

5455
############################ JSON Setup ###############################################
5556
if self.json_path != '':
@@ -124,8 +125,12 @@ def postprocess(self, img, modality):
124125
color_reset = '\033[0m'
125126

126127
if self.save_folder != '':
127-
cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.image_count}.png'),
128-
cv2.cvtColor(encoded_img, cv2.COLOR_RGB2BGR))
128+
if modality == 'rgb':
129+
cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.rgb_count}.png'),
130+
cv2.cvtColor(encoded_img, cv2.COLOR_RGB2BGR))
131+
else:
132+
cv2.imwrite(os.path.join(self.save_folder, modality, f'{modality}_{self.depth_count}.png'),
133+
encoded_img)
129134
self.get_logger().info(f'{color_start}{modality} image saved{color_reset}')
130135
else:
131136
self.get_logger().info(f'{color_start}{modality} image captured, not saved{color_reset}')
@@ -153,10 +158,11 @@ def postprocess(self, img, modality):
153158
self.get_logger().info(f'{color_start}JSON file updated{color_reset}')
154159

155160
# only increase the count with RGB so we don't double count
156-
self.image_count += 1
161+
self.rgb_count += 1
157162

158163
elif modality == 'depth':
159164
self.depth_publisher.publish(img)
165+
self.depth_count += 1
160166

161167
return encoded_img
162168

@@ -228,10 +234,10 @@ def _json_update(self):
228234
transformation_matrix = self._process_tf(transformstamp)
229235

230236

231-
update_dict['file_path'] = os.path.join('rgb', f'rgb_{self.image_count}.png')
237+
update_dict['file_path'] = os.path.join('rgb', f'rgb_{self.rgb_count}.png')
232238
update_dict['transformation_matrix'] = transformation_matrix.tolist()
233-
update_dict['colmap_im_id'] = self.image_count
234-
update_dict['depth_file_path'] = os.path.join('depth', f'depth_{self.image_count}.png')
239+
update_dict['colmap_im_id'] = self.rgb_count
240+
update_dict['depth_file_path'] = os.path.join('depth', f'depth_{self.rgb_count}.png')
235241

236242
self.json_dict['frames'].append(update_dict)
237243

‎realsense_capture/realsense_capture/realsense_static_tf_publisher.py

Lines changed: 26 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
from rclpy.node import Node
77

88
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
9+
from tf2_ros.buffer import Buffer
10+
from tf2_ros.transform_listener import TransformListener
911

1012
class RealSenseStaticTFPublisher(Node):
1113
"""
@@ -31,16 +33,37 @@ def __init__(self):
3133
# Publish static transforms once at startup
3234
self.make_transforms()
3335

36+
# ############################ TF Setup ########################################
37+
# # buffer to hold the transform in a cache
38+
# self.tf_buffer = Buffer()
39+
40+
# # listener. Important to spin a thread, otherwise the listen will block and no TF can be updated
41+
# self.tf_listener = TransformListener(buffer=self.tf_buffer, node=self, spin_thread=True)
42+
43+
# self.pose_publisher = self.create_publisher(
44+
# msg_type=TransformStamped,
45+
# topic='/realsense_capture/realsense_pose',
46+
# qos_profile=10)
47+
48+
# self.create_timer(0.5, self.publish_pose)
49+
50+
def publish_pose(self):
51+
try:
52+
t = self.tf_buffer.lookup_transform('link_base', 'link_realsense', self.get_clock().now())
53+
self.pose_publisher.publish(t)
54+
except Exception as e:
55+
self.get_logger().info(f"Failed to publish pose: {e}")
56+
3457
def make_transforms(self):
3558
t = TransformStamped()
3659

3760
t.header.stamp = self.get_clock().now().to_msg()
3861
t.header.frame_id = 'link_eef'
3962
t.child_frame_id = 'link_realsense'
4063

41-
t.transform.translation.x = float(-self.d405_center2left)
42-
t.transform.translation.y = float(-self.mount_translation_y)
43-
t.transform.translation.z = float(self.mount_translation_z+self.d405_height)
64+
t.transform.translation.x = float(-self.d405_center2left)*0.001
65+
t.transform.translation.y = float(-self.mount_translation_y)*0.001
66+
t.transform.translation.z = float(self.mount_translation_z+self.d405_height)*0.001
4467

4568
t.transform.rotation.x = 0.0
4669
t.transform.rotation.y = 0.0

0 commit comments

Comments
 (0)