Merge pull request #11 from Michi-Tsubaki/add-tf2

Add tf2

Author: Michi-Tsubaki

examples/example_tf2.jl

@@ -0,0 +1,94 @@
+#!/usr/bin/env julia
+# This example demonstrates minimal RViz integration with TF and visualization markers.
+# In other terminal(bash), run: `ros2 run rviz2 rviz2`
+
+using ROS2
+
+init()
+node = ROSNode("rviz_minimal")
+
+static_broadcaster = StaticTransformBroadcaster(node)
+broadcaster = TransformBroadcaster(node)
+listener = TransformListener(node)
+pose_pub = Publisher(node, "robot_pose", "geometry_msgs.msg.PoseStamped")
+marker_pub = Publisher(node, "visualization_marker", "visualization_msgs.msg.Marker")
+
+static_transform =
+    create_transform_stamped("map", "odom", (0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0))
+static_transform.header.stamp = to_msg_time(now())
+send_static_transform(static_broadcaster, static_transform)
+
+robot_x = Ref(0.0)
+robot_y = Ref(0.0)
+robot_theta = Ref(0.0)
+
+timer = ROSTimer(
+    node,
+    0.1,
+    () -> begin
+        robot_theta[] += 0.1
+        robot_x[] = 2.0 * cos(robot_theta[])
+        robot_y[] = 2.0 * sin(robot_theta[])
+
+        qz = sin(robot_theta[] / 2.0)
+        qw = cos(robot_theta[] / 2.0)
+
+        odom_transform = create_transform_stamped(
+            "odom",
+            "base_link",
+            (robot_x[], robot_y[], 0.0),
+            (0.0, 0.0, qz, qw),
+        )
+        odom_transform.header.stamp = to_msg_time(now())
+        send_transform(broadcaster, odom_transform)
+
+        pose_msg = create_msg("geometry_msgs.msg.PoseStamped")
+        pose_msg.header.stamp = to_msg_time(now())
+        pose_msg.header.frame_id = "map"
+        pose_msg.pose.position.x = robot_x[]
+        pose_msg.pose.position.y = robot_y[]
+        pose_msg.pose.position.z = 0.0
+        pose_msg.pose.orientation.x = 0.0
+        pose_msg.pose.orientation.y = 0.0
+        pose_msg.pose.orientation.z = qz
+        pose_msg.pose.orientation.w = qw
+        publish(pose_pub, pose_msg)
+
+        marker_msg = create_msg("visualization_msgs.msg.Marker")
+        marker_msg.header.stamp = to_msg_time(now())
+        marker_msg.header.frame_id = "base_link"
+        marker_msg.type = 2
+        marker_msg.action = 0
+        marker_msg.pose.position.x = 0.0
+        marker_msg.pose.position.y = 0.0
+        marker_msg.pose.position.z = 0.5
+        marker_msg.pose.orientation.w = 1.0
+        marker_msg.scale.x = 0.5
+        marker_msg.scale.y = 0.5
+        marker_msg.scale.z = 1.0
+        marker_msg.color.a = 1.0
+        marker_msg.color.r = 1.0
+        marker_msg.color.g = 0.0
+        marker_msg.color.b = 0.0
+        marker_msg.id = 0
+        publish(marker_pub, marker_msg)
+
+        try
+            transform = lookup_transform(listener, "map", "base_link")
+            println(
+                "Robot at: x=$(round(transform.transform.translation.x, digits=2)), y=$(round(transform.transform.translation.y, digits=2))",
+            )
+        catch
+        end
+    end,
+)
+
+try
+    while true
+        spin_once(node)
+        sleep(0.05)
+    end
+finally
+    timer_cancel(timer)
+    shutdown()
+end

src/ROS2.jl

@@ -12,6 +12,7 @@ include("service.jl")
 include("parameter.jl")
 include("action.jl")
 include("logging.jl")
+include("tf2.jl")
 
 function __init__()
     if contains(lowercase(get(ENV, "GITHUB_WORKFLOW", "")), "automerge")
@@ -38,6 +39,7 @@ function __init__()
         copy!(rclpy_node, pyimport("rclpy.node"))
 
         Time.__init_time__()
+        TF2.__init_tf2__()
 
     catch e
         @warn "ROS2 initialization deferred: $e"
@@ -52,6 +54,7 @@ using .Service
 using .Parameter
 using .Action
 using .Logging
+using .TF2
 
 export ROSNode,
     init,
@@ -113,11 +116,19 @@ export ROSNode,
     warn,
     log_error,
     fatal,
-    set_level,  # from Logging
+    set_level,
     DEBUG,
     INFO,
     WARN,
     ERROR,
-    FATAL
+    FATAL,
+    TransformListener,
+    TransformBroadcaster,
+    StaticTransformBroadcaster,
+    lookup_transform,
+    can_transform,
+    send_transform,
+    send_static_transform,
+    create_transform_stamped # from TF2
 
 end

src/tf2.jl

@@ -0,0 +1,102 @@
+module TF2
+
+using PyCall
+using ..Core: ROSNode
+
+const tf2_ros = PyNULL()
+const tf2_geometry_msgs = PyNULL()
+const geometry_msgs = PyNULL()
+
+function __init_tf2__()
+    copy!(tf2_ros, pyimport("tf2_ros"))
+    copy!(tf2_geometry_msgs, pyimport("tf2_geometry_msgs"))
+    copy!(geometry_msgs, pyimport("geometry_msgs.msg"))
+end
+
+mutable struct TransformListener
+    _py_listener::PyObject
+    _buffer::PyObject
+
+    function TransformListener(node::ROSNode)
+        buffer = tf2_ros.Buffer()
+        listener = tf2_ros.TransformListener(buffer, node.pynode)
+        new(listener, buffer)
+    end
+end
+
+mutable struct TransformBroadcaster
+    _py_broadcaster::PyObject
+
+    function TransformBroadcaster(node::ROSNode)
+        broadcaster = tf2_ros.TransformBroadcaster(node.pynode)
+        new(broadcaster)
+    end
+end
+
+mutable struct StaticTransformBroadcaster
+    _py_broadcaster::PyObject
+
+    function StaticTransformBroadcaster(node::ROSNode)
+        broadcaster = tf2_ros.StaticTransformBroadcaster(node.pynode)
+        new(broadcaster)
+    end
+end
+
+function lookup_transform(
+    listener::TransformListener,
+    target_frame::String,
+    source_frame::String,
+    time = nothing,
+)
+    if time === nothing
+        return listener._buffer.lookup_transform(target_frame, source_frame, tf2_ros.Time())
+    else
+        return listener._buffer.lookup_transform(target_frame, source_frame, time)
+    end
+end
+
+function can_transform(
+    listener::TransformListener,
+    target_frame::String,
+    source_frame::String,
+    time = nothing,
+)
+    if time === nothing
+        return listener._buffer.can_transform(target_frame, source_frame, tf2_ros.Time())
+    else
+        return listener._buffer.can_transform(target_frame, source_frame, time)
+    end
+end
+
+function send_transform(broadcaster::TransformBroadcaster, transform)
+    broadcaster._py_broadcaster.sendTransform(transform)
+end
+
+function send_static_transform(broadcaster::StaticTransformBroadcaster, transform)
+    broadcaster._py_broadcaster.sendTransform(transform)
+end
+
+function create_transform_stamped(
+    parent_frame::String,
+    child_frame::String,
+    translation::Tuple{Float64,Float64,Float64},
+    rotation::Tuple{Float64,Float64,Float64,Float64},
+)
+    transform = geometry_msgs.TransformStamped()
+    transform.header.frame_id = parent_frame
+    transform.child_frame_id = child_frame
+    transform.transform.translation.x = translation[1]
+    transform.transform.translation.y = translation[2]
+    transform.transform.translation.z = translation[3]
+    transform.transform.rotation.x = rotation[1]
+    transform.transform.rotation.y = rotation[2]
+    transform.transform.rotation.z = rotation[3]
+    transform.transform.rotation.w = rotation[4]
+    return transform
+end
+
+export TransformListener, TransformBroadcaster, StaticTransformBroadcaster
+export lookup_transform, can_transform, send_transform, send_static_transform
+export create_transform_stamped
+
+end