From cd4f8e5afbde856e3354684a53efeb9cfbec949d Mon Sep 17 00:00:00 2001 From: Benjamin Thiemann Date: Thu, 29 Jan 2026 15:50:26 +0100 Subject: [PATCH 1/2] ros2: ensenso_camera: Fix sleep calls in calibrate_handeye script --- ensenso_camera/scripts/calibrate_handeye | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ensenso_camera/scripts/calibrate_handeye b/ensenso_camera/scripts/calibrate_handeye index 8d94a0f4..62e7e8a1 100755 --- a/ensenso_camera/scripts/calibrate_handeye +++ b/ensenso_camera/scripts/calibrate_handeye @@ -65,9 +65,9 @@ def _main(node_name): while time_until_capture > 0.8: start_sleep = node.get_clock().now() node.get_logger().info("Capturing next pattern in {}s.".format(time_until_capture)) - ros2py.sleep(0.75) + ros2py.sleep(node, 0.75) time_until_capture -= node.get_clock().now() - start_sleep - ros2py.sleep(time_until_capture) + ros2py.sleep(node, time_until_capture) calibrate_hand_eye(node, calibrate_hand_eye_client) From dc39960ece39bd7cf1cb5395013420149403aec9 Mon Sep 17 00:00:00 2001 From: Benjamin Thiemann Date: Fri, 30 Jan 2026 08:06:12 +0100 Subject: [PATCH 2/2] ros2: ensenso_camera: Fix ros2 python durations in compatibility layer Replace the original compatibility function with duration_from_seconds. Reminder: This function returns a numeric value for ROS2, because there the Duration type is not (yet) supported in messages. The Duration keyword is now again available in the namespace. * Implement add and sub functions for durations, because ROS2 does not support them yet. * Implement helper function duration_to_seconds becuase ROS2 durations only return nanoseconds. ROS1: * Sleep accepts both numeric values and Duration objects * Arithmetic on Time objects results in Duration objects * Arithmetic on Duration objects results in Duration objects ROS2: * Sleep not existing, implemented in our ros2.py * Arithmetic on Time objects results in Duration objects * Arithmetic on Duration objects not implemented yet * Overloads implemented in rclpy #1387 (next release) --- ensenso_camera/scripts/calibrate_handeye | 12 +++--- ensenso_camera/scripts/fit_primitive | 2 +- ensenso_camera/scripts/pattern_marker | 2 +- ensenso_camera/src/ensenso_camera/ros2.py | 46 +++++++++++++++++++---- 4 files changed, 47 insertions(+), 15 deletions(-) diff --git a/ensenso_camera/scripts/calibrate_handeye b/ensenso_camera/scripts/calibrate_handeye index 62e7e8a1..2be0550f 100755 --- a/ensenso_camera/scripts/calibrate_handeye +++ b/ensenso_camera/scripts/calibrate_handeye @@ -41,7 +41,7 @@ def calibrate_hand_eye(node, client): def _main(node_name): node = ros2py.create_node(node_name, args=sys.argv) - capture_wait = ros2py.get_param(node, "capture_wait", 10.0) + capture_wait = ros2py.Duration(ros2py.get_param(node, "capture_wait", 10.0)) count_poses = max(ros2py.get_param(node, "count_poses", 5), 6) timeout = ros2py.get_param(node, "timeout", 60) @@ -61,12 +61,14 @@ def _main(node_name): break # ---- Move the robot to the next position. - time_until_capture = capture_wait - (node.get_clock().now() - start_capture) - while time_until_capture > 0.8: + time_until_capture = ros2py.sub_durations(capture_wait, (node.get_clock().now() - start_capture)) + while time_until_capture > ros2py.Duration(0.8): start_sleep = node.get_clock().now() - node.get_logger().info("Capturing next pattern in {}s.".format(time_until_capture)) + node.get_logger().info( + "Capturing next pattern in {}s".format(ros2py.duration_to_seconds(time_until_capture)) + ) ros2py.sleep(node, 0.75) - time_until_capture -= node.get_clock().now() - start_sleep + time_until_capture = ros2py.sub_durations(time_until_capture, (node.get_clock().now() - start_sleep)) ros2py.sleep(node, time_until_capture) calibrate_hand_eye(node, calibrate_hand_eye_client) diff --git a/ensenso_camera/scripts/fit_primitive b/ensenso_camera/scripts/fit_primitive index 140b508d..d2dd2b57 100755 --- a/ensenso_camera/scripts/fit_primitive +++ b/ensenso_camera/scripts/fit_primitive @@ -97,7 +97,7 @@ def _main(node_name): marker.id = marker_id marker.type = Marker.CUBE marker.action = Marker.ADD - marker.lifetime = ros2py.Duration(1) + marker.lifetime = ros2py.duration_from_seconds(1) marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 diff --git a/ensenso_camera/scripts/pattern_marker b/ensenso_camera/scripts/pattern_marker index 24c99c6a..54752a86 100755 --- a/ensenso_camera/scripts/pattern_marker +++ b/ensenso_camera/scripts/pattern_marker @@ -59,7 +59,7 @@ def _main(node_name): marker.id = i marker.type = Marker.CUBE marker.action = Marker.ADD - marker.lifetime = ros2py.Duration(2) + marker.lifetime = ros2py.duration_from_seconds(2) marker.color.a = 1.0 marker.color.r = 1.0 diff --git a/ensenso_camera/src/ensenso_camera/ros2.py b/ensenso_camera/src/ensenso_camera/ros2.py index 0b1bccbc..47175473 100644 --- a/ensenso_camera/src/ensenso_camera/ros2.py +++ b/ensenso_camera/src/ensenso_camera/ros2.py @@ -79,7 +79,7 @@ def send_goal(self): def wait_for_response(self, timeout_sec=None): # Separate sending goal and waiting for response so that we can handle several clients at once by sending # all the goals first and then waiting until all clients have received the response. - self._event.wait(timeout_sec) + self._event.wait(duration_to_seconds(timeout_sec)) self._response = ActionResponse(self._result.status, self._result.result) def response_callback(self, future): @@ -99,11 +99,29 @@ def result_callback(self, future): def get_response(self): return self._response - def Duration(sec): - return sec + def Duration(seconds): + return rclpy.duration.Duration(seconds=seconds) - def sleep(node, secs): - frequency = 1.0 / secs + def add_durations(a, b): + return rclpy.duration.Duration(nanoseconds=a.nanoseconds + b.nanoseconds) + + def sub_durations(a, b): + return rclpy.duration.Duration(nanoseconds=a.nanoseconds - b.nanoseconds) + + def duration_to_seconds(duration_or_seconds): + if duration_or_seconds is None: + return None + elif isinstance(duration_or_seconds, rclpy.duration.Duration): + return duration_or_seconds.nanoseconds / 1e9 + else: + return duration_or_seconds + + def duration_from_seconds(seconds): + return seconds + + def sleep(node, duration_or_seconds): + seconds = duration_to_seconds(duration_or_seconds) + frequency = 1.0 / seconds rate = node.create_rate(frequency) rate.sleep() @@ -153,7 +171,7 @@ def wait_for_server(node, client, timeout_sec=None, exit=False): Returns True if the client established a server connection in the given amount of time, False otherwise. """ node.get_logger().info("Connecting to action server {} ...".format(client._action_name)) - if not client.wait_for_server(timeout_sec): + if not client.wait_for_server(duration_to_seconds(timeout_sec)): node.get_logger().error(SERVER_TIMEOUT_ERROR_MESSAGE) if exit: sys.exit() @@ -282,8 +300,20 @@ class Clock: def now(self): return rospy.Time.now() - def Duration(sec): - return rospy.Duration(sec) + def Duration(seconds): + return rospy.Duration(seconds) + + def add_durations(a, b): + return a + b + + def sub_durations(a, b): + return a - b + + def duration_to_seconds(d): + return d.to_sec() + + def duration_from_seconds(sec): + return Duration(sec) def sleep(node, secs): rospy.sleep(secs)