Merge 54123c6 into 73e79aa

.travis.yml

@@ -15,7 +15,7 @@ before_install:
   - sudo apt-add-repository -y "deb http://packages.ros.org/ros/ubuntu xenial main"
   - wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add -
   - sudo apt-get update
-  - sudo apt-get -y install ros-kinetic-ros-base ros-kinetic-common-msgs
+  - sudo apt-get -y install ros-kinetic-ros-base ros-kinetic-common-msgs ros-kinetic-tf2-ros ros-kinetic-tf
   - sudo rosdep init
   - rosdep update
 before_script:
@@ -24,6 +24,7 @@ before_script:
   - roscore &
   - sleep 5
   - python test/echonode.py &
+  - python test/tfpubnode.py &
   - sleep 5
 after_success:
   - julia --project -e 'import Pkg; Pkg.add("Coverage"); using Coverage; Coveralls.submit(Coveralls.process_folder())'

src/RobotOS.jl

@@ -6,6 +6,7 @@ using PyCall
 const _py_sys = PyCall.PyNULL()
 const _py_ros_callbacks = PyCall.PyNULL()
 const __rospy__ = PyCall.PyNULL()
+const __tf__ = PyCall.PyNULL()
 
 include("debug.jl")
 include("time.jl")
@@ -14,6 +15,7 @@ include("rospy.jl")
 include("pubsub.jl")
 include("services.jl")
 include("callbacks.jl")
+include("tf.jl")
 
 function __init__()
     #Put julia's ARGS into python's so remappings will work
@@ -28,6 +30,8 @@ function __init__()
     end
     copy!(_py_ros_callbacks, pyimport("ros_callbacks"))
 
+    copy!(__tf__, pyimport("tf"))
+
     try
         copy!(__rospy__, pyimport("rospy"))
     catch ex

src/dev.jl

@@ -0,0 +1,7 @@
+using RobotOS
+using PyCall
+
+init_node("test")
+tl = TransformListener()
+a = lookupTransform(tl, "base_link", "hoge", Time())
+waitForTransform(tl, "base_link", "hoge", Time(), Duration(2.0))

src/tf.jl

@@ -0,0 +1,68 @@
+export TransformListener, lookupTransform, waitForTransform
+
+"""
+    TransformListener()
+Create a transform listener object.
+"""
+struct TransformListener
+    o::PyObject
+    function TransformListener()
+        new(__tf__.TransformListener())
+    end
+end
+
+"""
+    generate_error_message(err) 
+Retrun error message string which includes both exception type and error massage information.
+"""
+function generate_error_message(err)
+    exception_type = err.T.__name__
+    error_massage = exception_type * ": $(err.val.args[1])"
+end
+
+"""
+    lookupTransform(tf_listener_obj, target, source, time) 
+Return tuple of (position, quaternion).
+"""
+function lookupTransform(tl::TransformListener,
+                         target_frame::AbstractString,
+                         source_frame::AbstractString,
+                         pytime::Time)
+    time = convert(PyObject, pytime)
+    transform = try
+        pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time)
+    catch err
+        if isa(err, PyCall.PyError)
+            error_massage = generate_error_message(err)
+            error(error_massage)
+        else
+            rethrow(err)
+        end
+    end
+end
+
+"""
+    waitForTransform(tf_listener_obj, target, source, time, timeout, pypolling_sleep_duration) 
+Waits for the given transformation to become available. If the timeout occurs before the transformation becomes available, raises an exception.
+"""
+function waitForTransform(tl::TransformListener,
+                          target_frame::AbstractString, 
+                          source_frame::AbstractString,
+                          pytime::Time,
+                          pytimeout::Duration;
+                          pypolling_sleep_duration = Duration(0.01))
+    time = convert(PyObject, pytime)
+    timeout = convert(PyObject, pytimeout)
+    polling_sleep_duration = convert(PyObject, pypolling_sleep_duration)
+    try
+        pycall(tl.o.waitForTransform, PyAny, target_frame, source_frame,
+               time, timeout, polling_sleep_duration)
+    catch err
+        if isa(err, PyCall.PyError)
+            error_massage = generate_error_message(err)
+            error(error_massage)
+        else
+            rethrow(err)
+        end
+    end
+end

test/runtests.jl

@@ -9,3 +9,4 @@ include("time.jl")
 include("typegeneration.jl")
 include("pubsub.jl")
 include("services.jl")
+include("tf.jl")

test/tf.jl

@@ -0,0 +1,7 @@
+#Test if tf listner works correctly
+
+tl = TransformListener()
+waitForTransform(tl, "parent_link", "child_link", Time(), Duration(1.0))
+transform = lookupTransform(tl, "parent_link", "child_link", Time())
+@test transform == ([0, 0, 0.], [0, 0, 0, 1.])
+

test/tfpubnode.py

@@ -0,0 +1,24 @@
+# A simple ROS node in python that publishes tf topic
+
+import rospy
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+from std_msgs.msg import Header
+
+def main():
+    rospy.init_node("tf_pub", anonymous=True)
+    pub = rospy.Publisher("/tf", TFMessage, queue_size=10)
+
+    while not rospy.is_shutdown():
+        tfstamped = TransformStamped()
+        tfstamped.header = Header(0, rospy.get_rostime(), "parent_link")
+        tfstamped.child_frame_id = "child_link"
+        tfstamped.transform.rotation.w = 1.0
+
+        tf_msg = TFMessage()
+        tf_msg.transforms.append(tfstamped)
+
+        pub.publish(tf_msg)
+
+if __name__ == "__main__":
+    main()