ros2: ensenso_camera: Fix ros2 python durations in compatibility layer

* 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)

Author: benthie

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)

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):
@@ -102,11 +102,26 @@ def get_response(self):
     def Duration(seconds):
         return rclpy.duration.Duration(seconds=seconds)
 
+    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, secs):
-        frequency = 1.0 / secs
+    def sleep(node, duration_or_seconds):
+        seconds = duration_to_seconds(duration_or_seconds)
+        frequency = 1.0 / seconds
         rate = node.create_rate(frequency)
         rate.sleep()
 
@@ -156,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()
@@ -288,6 +303,15 @@ def now(self):
     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)