Fix bugs in hand-eye-calibration script and ROS2 compatibility layer

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,13 +61,15 @@ 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))
-            ros2py.sleep(0.75)
-            time_until_capture -= node.get_clock().now() - start_sleep
-        ros2py.sleep(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 = 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)
 

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

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

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)