From 389398b4c88beddf7fcaaeb8fdc88cd46c21e51c Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 02:02:31 +0900 Subject: [PATCH 1/8] add TransformListener ans its method --- src/RobotOS.jl | 4 ++++ src/tf.jl | 17 +++++++++++++++++ 2 files changed, 21 insertions(+) create mode 100644 src/tf.jl diff --git a/src/RobotOS.jl b/src/RobotOS.jl index 09803cd..a4314c2 100644 --- a/src/RobotOS.jl +++ b/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 diff --git a/src/tf.jl b/src/tf.jl new file mode 100644 index 0000000..2fc4c26 --- /dev/null +++ b/src/tf.jl @@ -0,0 +1,17 @@ +export TransformListener, lookupTransform + +struct TransformListener + o::PyObject + function TransformListener() + new(__tf__.TransformListener()) + end +end + +function lookupTransform(tl::TransformListener, + target_frame::AbstractString, + source_frame::AbstractString, + pyt::Time) + t = convert(PyObject, pyt) + tl.o.lookupTransform(target_frame, source_frame, t) + pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, t) +end From 7202e2fd2f2df0d72cd81bbfa0fc3a38bd9e769b Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 02:14:47 +0900 Subject: [PATCH 2/8] add waitForTransform --- src/tf.jl | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/tf.jl b/src/tf.jl index 2fc4c26..fe3c680 100644 --- a/src/tf.jl +++ b/src/tf.jl @@ -1,4 +1,4 @@ -export TransformListener, lookupTransform +export TransformListener, lookupTransform, waitForTransform struct TransformListener o::PyObject @@ -10,8 +10,20 @@ end function lookupTransform(tl::TransformListener, target_frame::AbstractString, source_frame::AbstractString, - pyt::Time) - t = convert(PyObject, pyt) - tl.o.lookupTransform(target_frame, source_frame, t) - pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, t) + pytime::Time) + time = convert(PyObject, pytime) + pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time) +end + +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) + pycall(tl.o.waitForTransform, PyAny, target_frame, source_frame, + time, timeout, polling_sleep_duration) end From 94bf5dd2178543d781ed56203aba1e61f050c00a Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 02:57:18 +0900 Subject: [PATCH 3/8] add doc --- src/tf.jl | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/tf.jl b/src/tf.jl index fe3c680..32621d9 100644 --- a/src/tf.jl +++ b/src/tf.jl @@ -1,5 +1,9 @@ export TransformListener, lookupTransform, waitForTransform +""" + TransformListener() +Create a transform listener object +""" struct TransformListener o::PyObject function TransformListener() @@ -7,6 +11,10 @@ struct TransformListener end end +""" + lookupTransform(tf_listener_obj, target, source, time) +Return tuple of (position, quaternion) +""" function lookupTransform(tl::TransformListener, target_frame::AbstractString, source_frame::AbstractString, @@ -15,6 +23,10 @@ function lookupTransform(tl::TransformListener, pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time) end +""" + waitForTransform(tf_listener_obj, target, source, time, timeout, pypolling_sleep_duration) +Waits for the given transformation to become available. +""" function waitForTransform(tl::TransformListener, target_frame::AbstractString, source_frame::AbstractString, From 32aaa9e39b13ce7b843372e450e74ae42a18c405 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 03:12:53 +0900 Subject: [PATCH 4/8] catch exception --- src/tf.jl | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/tf.jl b/src/tf.jl index 32621d9..058cea8 100644 --- a/src/tf.jl +++ b/src/tf.jl @@ -20,7 +20,15 @@ function lookupTransform(tl::TransformListener, source_frame::AbstractString, pytime::Time) time = convert(PyObject, pytime) - pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time) + transform = try + pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time) + catch err + if isa(err, PyCall.PyError) + error("LookupException: $(err.val.args[1])") + else + rethrow(err) + end + end end """ @@ -36,6 +44,14 @@ function waitForTransform(tl::TransformListener, time = convert(PyObject, pytime) timeout = convert(PyObject, pytimeout) polling_sleep_duration = convert(PyObject, pypolling_sleep_duration) - pycall(tl.o.waitForTransform, PyAny, target_frame, source_frame, - time, timeout, polling_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("TransformException: $(err.val.args[1])") + else + rethrow(err) + end + end end From 91f509ff8f925ee3e48586f6b6a6554abf229aec Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 03:33:50 +0900 Subject: [PATCH 5/8] show error massage include exception type information --- src/tf.jl | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/tf.jl b/src/tf.jl index 058cea8..32d2894 100644 --- a/src/tf.jl +++ b/src/tf.jl @@ -11,6 +11,15 @@ struct TransformListener end end +""" + generate_error_message(err) +Retrun error message string which include noth 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) @@ -24,7 +33,8 @@ function lookupTransform(tl::TransformListener, pycall(tl.o.lookupTransform, PyAny, target_frame, source_frame, time) catch err if isa(err, PyCall.PyError) - error("LookupException: $(err.val.args[1])") + error_massage = generate_error_message(err) + error(error_massage) else rethrow(err) end @@ -33,7 +43,7 @@ end """ waitForTransform(tf_listener_obj, target, source, time, timeout, pypolling_sleep_duration) -Waits for the given transformation to become available. +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, @@ -49,7 +59,8 @@ function waitForTransform(tl::TransformListener, time, timeout, polling_sleep_duration) catch err if isa(err, PyCall.PyError) - error("TransformException: $(err.val.args[1])") + error_massage = generate_error_message(err) + error(error_massage) else rethrow(err) end From 8994ca81a7a582f03485e54d5707f5536b29f4e5 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 05:08:30 +0900 Subject: [PATCH 6/8] added test and check passed --- .travis.yml | 1 + test/runtests.jl | 1 + test/tf.jl | 7 +++++++ test/tfpubnode.py | 24 ++++++++++++++++++++++++ 4 files changed, 33 insertions(+) create mode 100644 test/tf.jl create mode 100755 test/tfpubnode.py diff --git a/.travis.yml b/.travis.yml index d8615c1..9b90fba 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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())' diff --git a/test/runtests.jl b/test/runtests.jl index a1fa188..7da6fd6 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -9,3 +9,4 @@ include("time.jl") include("typegeneration.jl") include("pubsub.jl") include("services.jl") +include("tf.jl") diff --git a/test/tf.jl b/test/tf.jl new file mode 100644 index 0000000..dad4880 --- /dev/null +++ b/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.]) + diff --git a/test/tfpubnode.py b/test/tfpubnode.py new file mode 100755 index 0000000..bf76307 --- /dev/null +++ b/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() From b95c2cea454ae4aaf456cac65c241e6884610707 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Mon, 6 Apr 2020 05:18:33 +0900 Subject: [PATCH 7/8] fix typos --- src/tf.jl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/tf.jl b/src/tf.jl index 32d2894..a80b634 100644 --- a/src/tf.jl +++ b/src/tf.jl @@ -2,7 +2,7 @@ export TransformListener, lookupTransform, waitForTransform """ TransformListener() -Create a transform listener object +Create a transform listener object. """ struct TransformListener o::PyObject @@ -13,7 +13,7 @@ end """ generate_error_message(err) -Retrun error message string which include noth exception type and error massage information +Retrun error message string which includes both exception type and error massage information. """ function generate_error_message(err) exception_type = err.T.__name__ @@ -22,7 +22,7 @@ end """ lookupTransform(tf_listener_obj, target, source, time) -Return tuple of (position, quaternion) +Return tuple of (position, quaternion). """ function lookupTransform(tl::TransformListener, target_frame::AbstractString, From 54123c66b3920f8a32c45c0e1054c5436f3b6b05 Mon Sep 17 00:00:00 2001 From: HiroIshida Date: Wed, 8 Apr 2020 23:55:29 +0900 Subject: [PATCH 8/8] add apt install ros-kinetic-tf2-ros and ros-kinetic-tf in travis tmp --- .travis.yml | 2 +- src/dev.jl | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100644 src/dev.jl diff --git a/.travis.yml b/.travis.yml index 9b90fba..c228d8f 100644 --- a/.travis.yml +++ b/.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: diff --git a/src/dev.jl b/src/dev.jl new file mode 100644 index 0000000..8cb56d0 --- /dev/null +++ b/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))