Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ros2 fixed async request #838

Closed
wants to merge 1 commit into from
Closed

ros2 fixed async request #838

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 17 additions & 4 deletions camera_calibration/src/camera_calibration/camera_calibrator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
from camera_calibration.calibrator import CAMERA_MODEL
from rclpy.qos import qos_profile_system_default
from rclpy.qos import QoSProfile
from threading import Event


class BufferQueue(Queue):
Expand Down Expand Up @@ -214,19 +215,29 @@ def do_upload(self):
print(self.c.ost())
info = self.c.as_message()

event=Event()
def done_callback(future):
nonlocal event
event.set()

req = sensor_msgs.srv.SetCameraInfo.Request()
rv = True
if self.c.is_mono:
req.camera_info = info
response = self.set_camera_info_service.call_async(req)
response.add_done_callback(done_callback)
event.wait()
rv = self.check_set_camera_info(response)
else:
req.camera_info = info[0]
response = self.set_left_camera_info_service.call_async(req)
rv = rv and self.check_set_camera_info(response)
response0 = self.set_left_camera_info_service.call_async(req)
response0.add_done_callback(done_callback)
req.camera_info = info[1]
response = self.set_right_camera_info_service.call_async(req)
rv = rv and self.check_set_camera_info(response)
response1 = self.set_right_camera_info_service.call_async(req)
response1.add_done_callback(done_callback)
event.wait()
rv = rv and self.check_set_camera_info(response0)
rv = rv and self.check_set_camera_info(response1)
return rv

def get_topic_qos(self, topic_name: str) -> QoSProfile:
Expand Down Expand Up @@ -307,6 +318,7 @@ def on_mouse(self, event, x, y, flags, param):
elif 380 <= y < 480:
# Only shut down if we set camera info correctly, #3993
if self.do_upload():
self.get_logger().info('Calibration committed')
rclpy.shutdown()
def on_model_change(self, model_select_val):
if self.c == None:
Expand Down Expand Up @@ -423,3 +435,4 @@ def redraw_stereo(self, drawable):

self._last_display = display
self.queue_display.put(display)

Loading