Fix some codes

 - Add tf2_ros unit tests
 - Fix tf2_ros codes to work

Signed-off-by: vinnam kim <vinnam.kim@gmail.com>

tf2_py/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(python_cmake_module REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(PythonLibs 3 REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)

tf2_ros/CMakeLists.txt

@@ -170,6 +170,10 @@ add_rostest(test/transform_listener_unittest.launch)
 add_rostest(test/transform_listener_time_reset_test.launch)
 
 endif()
+
+# Python test
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(tf2_ros_py_test test/py_test)
 endif()
 
 ament_export_include_directories(include)

tf2_ros/package.xml

@@ -48,6 +48,7 @@
   <!-- <test_depend>rostest</test_depend> -->
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

tf2_ros/src/tf2_ros/__init__.py

@@ -37,7 +37,7 @@
 from tf2_py import *
 from .buffer_interface import *
 from .buffer import *
-#from .buffer_client import *
+from .buffer_client import *
 from .transform_listener import *
 from .transform_broadcaster import *
 from .static_transform_broadcaster import *

tf2_ros/src/tf2_ros/buffer_client.py

@@ -34,7 +34,7 @@
 #* 
 #* Author: Eitan Marder-Eppstein
 #***********************************************************
-from rclpy.action.client import ActionClient, ClientGoalHandle
+from rclpy.action.client import ActionClient
 from rclpy.duration import Duration
 from rclpy.clock import Clock
 from time import sleep
@@ -82,8 +82,8 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Duration())
         goal = LookupTransform.Goal()
         goal.target_frame = target_frame
         goal.source_frame = source_frame
-        goal.source_time = time
-        goal.timeout = timeout
+        goal.source_time = time.to_msg()
+        goal.timeout = timeout.to_msg()
         goal.advanced = False
 
         return self.__process_goal(goal)
@@ -105,9 +105,9 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_
         goal = LookupTransform.Goal()
         goal.target_frame = target_frame
         goal.source_frame = source_frame
-        goal.source_time = source_time
-        goal.timeout = timeout
-        goal.target_time = target_time
+        goal.source_time = source_time.to_msg()
+        goal.timeout = timeout.to_msg()
+        goal.target_time = target_time.to_msg()
         goal.fixed_frame = fixed_frame
         goal.advanced = True
 
@@ -172,23 +172,28 @@ def __process_goal(self, goal):
         def unblock(future):
             nonlocal event
             event.set()
-
+            
         send_goal_future = self.action_client.send_goal_async(goal)
         send_goal_future.add_done_callback(unblock)
-
 
         def unblock_by_timeout():
             nonlocal send_goal_future, goal, event
             clock = Clock()
             start_time = clock.now()
-            while not :
-                if clock.now() > start_time + goal.timeout + self.timeout_padding:
+            timeout = Duration.from_msg(goal.timeout)
+            timeout_padding = Duration(seconds=self.timeout_padding)
+            while not send_goal_future.done() and not event.is_set():
+                if clock.now() > start_time + timeout + timeout_padding:
                     break
                 # TODO(vinnamkim): rclpy.Rate is not ready 
                 # See https://github.com/ros2/rclpy/issues/186
                 #r = rospy.Rate(self.check_frequency)
                 sleep(1.0 / self.check_frequency)
+
             event.set()
+
+        t = threading.Thread(target=unblock_by_timeout)
+        t.start()
 
         event.wait()
 
@@ -199,10 +204,14 @@ def unblock_by_timeout():
         if not send_goal_future.done():
             raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server")
 
-        if send_goal_future.result().status() != GoalStatus.SUCCEEDED:
-            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.")
+        goal_handle = send_goal_future.result()
 
-        return self.__process_result(send_goal_future.result())
+        if not goal_handle.accepted:
+            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server.")
+
+        response = self.action_client._get_result(goal_handle)
+
+        return self.__process_result(response.result)
 
     def __process_result(self, result):
         if result == None or result.error == None:

tf2_ros/src/tf2_ros/buffer_interface.py

@@ -46,7 +46,7 @@ def __init__(self):
         self.registration = tf2_ros.TransformRegistration()
 
     # transform, simple api
-    def transform(self, object_stamped, target_frame, timeout = Duration(), new_type = None):
+    def transform(self, object_stamped, target_frame, timeout=Duration(), new_type = None):
         """
         Transform an input into the target frame.
 
@@ -70,7 +70,7 @@ def transform(self, object_stamped, target_frame, timeout = Duration(), new_type
         return convert(res, new_type)
 
     # transform, advanced api
-    def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=Time(), new_type = None):
+    def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=Duration(), new_type = None):
         """
         Transform an input into the target frame (advanced API).
 
@@ -99,7 +99,7 @@ def transform_full(self, object_stamped, target_frame, target_time, fixed_frame,
 
         return convert(res, new_type)
 
-    def lookup_transform(self, target_frame, source_frame, time, timeout=Time()):
+    def lookup_transform(self, target_frame, source_frame, time, timeout=Duration()):
         """
         Get the transform from the source frame to the target frame.
 
@@ -114,7 +114,7 @@ def lookup_transform(self, target_frame, source_frame, time, timeout=Time()):
         """
         raise NotImplementedException()
 
-    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Time()):
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()):
         """
         Get the transform from the source frame to the target frame using the advanced API.
 
@@ -132,7 +132,7 @@ def lookup_transform_full(self, target_frame, target_time, source_frame, source_
         raise NotImplementedException()        
 
     # can, simple api
-    def can_transform(self, target_frame, source_frame, time, timeout=Time()):
+    def can_transform(self, target_frame, source_frame, time, timeout=Duration()):
         """
         Check if a transform from the source frame to the target frame is possible.
 
@@ -148,7 +148,7 @@ def can_transform(self, target_frame, source_frame, time, timeout=Time()):
         raise NotImplementedException()        
 
     # can, advanced api
-    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Time()):
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=Duration()):
         """
         Check if a transform from the source frame to the target frame is possible (advanced API).
 

tf2_ros/test/py_test/test_buffer.py

@@ -1,65 +1,59 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
+# ***********************************************************
+# * Software License Agreement (BSD License)
+# *
+# *  Copyright (c) 2009, Willow Garage, Inc.
+# *  All rights reserved.
+# *
+# *  Redistribution and use in source and binary forms, with or without
+# *  modification, are permitted provided that the following conditions
+# *  are met:
+# *
+# *   * Redistributions of source code must retain the above copyright
+# *     notice, this list of conditions and the following disclaimer.
+# *   * Redistributions in binary form must reproduce the above
+# *     copyright notice, this list of conditions and the following
+# *     disclaimer in the documentation and/or other materials provided
+# *     with the distribution.
+# *   * Neither the name of Willow Garage, Inc. nor the names of its
+# *     contributors may be used to endorse or promote products derived
+# *     from this software without specific prior written permission.
+# *
+# *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# *  POSSIBILITY OF SUCH DAMAGE.
+# *
+# * Author: Vinnam Kim vinnam.kim@gmail.com
+# ***********************************************************
 
-import time
 import unittest
 import rclpy
 
-from rclpy.action import ActionServer
 from tf2_ros.buffer import Buffer
-from tf2_msgs.action import LookupTransform
-from geometry_msgs.msg import TransformStamped
-from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
+from geometry_msgs.msg import TransformStamped, PointStamped
 
-TIME_FUDGE = 0.3
-
-class TestBufferClient(unittest.TestCase):
+class TestBuffer(unittest.TestCase):
     @classmethod
     def setUpClass(cls):
-        cls.context = rclpy.context.Context()
-        rclpy.init(context=cls.context)
-        cls.executor = SingleThreadedExecutor(context=cls.context)
-        cls.node = rclpy.create_node('TestBuffer', context=cls.context)
+        pass
 
     @classmethod
     def tearDownClass(cls):
-        cls.node.destroy_node()
-        rclpy.shutdown(context=cls.context)
-
-    def setUp(self):
-        self.feedback = None
-
-    def feedback_callback(self, feedback):
-        self.feedback = feedback
-
-    def timed_spin(self, duration):
-        start_time = time.time()
-        while (time.time() - start_time) < duration:
-            rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1)
-
-    def execute_goal_callback(self, goal_handle):
-        print('execute_goal_callback')
-        goal_handle.set_succeeded()
-        return LookupTransform.Result()
+        pass
 
     def test_can_transform_valid_transform(self):
         buffer = Buffer()
         clock = rclpy.clock.Clock()
         rclpy_time = clock.now()
 
-        from builtin_interfaces.msg import Time
-
         transform = TransformStamped()
         transform.header.frame_id = "foo"
         transform.header.stamp = rclpy_time.to_msg()
@@ -71,10 +65,12 @@ def test_can_transform_valid_transform(self):
         transform.transform.rotation.x = 0.0
         transform.transform.rotation.y = 0.0
         transform.transform.rotation.z = 0.0
-        print(dir(buffer))
-        self.assertTrue(buffer.set_transform(transform, "unittest"))
-        self.assertTrue(buffer.can_transform("bar", "foo", rclpy_time))
-        output = buffer.lookupTransform("foo", "bar", rclpy_time)
+
+        self.assertEqual(buffer.set_transform(transform, "unittest"), None)
+
+        self.assertEqual(buffer.can_transform("foo", "bar", rclpy_time), True)
+
+        output = buffer.lookup_transform("foo", "bar", rclpy_time)
         self.assertEqual(transform.child_frame_id, output.child_frame_id)
         self.assertEqual(transform.transform.translation.x, output.transform.translation.x)
         self.assertEqual(transform.transform.translation.y, output.transform.translation.y)