[Melodic][TF Tutorial] 4. Adding a frame (Python)

Env.

VMware workstation(Ubuntu 18.04 amd64, ROS Melodic)

 

1.Why adding frames

많은 경우, local frame을 사용하여 생각하는 것이 쉽다.

예를 들어, laser scan에 대해 추론하는 경우, laser scanner의 중심에 있는 프레임을 사용하는게 쉽다.

 

tf는 각각의 sensor, link에 대해 local frame을 정의할 수 있게 하며,

tf는 모든 프레임에 대한 transform을 관리해준다.

 

2. Where to add frames

tf는 tree구조를 사용한다.

그리고 closed loop를 허용하지 않는다.

이 의미는 오직 하나의 parent만을 허용하고, 다수의 children을 가질 수 있다.

 

위의 그림에서는 3 frames이 있다. (world, turtle1, turtle2)

turtle1과 turtle2는 world의 children이며, world는 turtle1과 turtle2의 parent이다.

 

만약 새로운 frame을 tf에 추가하고 싶다면,

기존 3개의 frames 중 하나가 parent frame이 되고

새로운 프레임이 child frame이 된다.

 

3. How to add a frame

$ roscd learning_tf
$ nano ./nodes/fixed_tf_broadcaster.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf

if __name__ == '__main__':
    rospy.init_node('fixed_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

$ chmod +x ./nodes/fixed_tf_broadcaster.py

 

4. Running the frame broadcaster

$ nano ./launch/start_demo.launch

  <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" />
    </node>
    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
    <node pkg="learning_tf" type="fixed_tf_broadcaster.py" name="broadcaster_fixed" />
  </launch>

# 1st terminal
$ roslaunch learning_tf start_demo.launch

이전과 같이 turtle1(중앙에 있는 거북이)를 turtle2(다른 거북이)가 따라다닌다.

4.1 rqt

# 2nd terminal
$ rqt

4.2 tf_echo

nodes/fixed_tf_broadcaster.py에서 sendTransform을 한 것과 동일한 결과를 얻을 수 있다.

$ rosrun tf tf_echo turtle1 carrot1
At time 1661267696.148
- Translation: [0.000, 2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1661267696.848
- Translation: [0.000, 2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1661267697.848
- Translation: [0.000, 2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

단, target_frame과 source_frame이 바뀌면 당연히 tf도 바뀌게 된다.

$ rosrun tf tf_echo carrot1 turtle1
At time 1661267749.948
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1661267750.549
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]
At time 1661267751.548
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

4.3 rviz

# 2nd terminal
$ rviz

 

5. Checking the results

TurtleSim에서는

이전 tutorial과 동일해 보이기 때문에

nodes/turtle_tf_listener.py를 아래와 같이 수정한다.

$ nano ./nodes/turtle_tf_listener.py

#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

이것은 아래의 부분만 변경한 것이다.

(trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))

따라서 turtle2와 carrot1의 tf를 구하여

turtle2가 carrot1으로 움직이는 명령을 보내게 된다.

# 1st terminal
$ roslaunch learning_tf start_demo.launch

이제는 turtle1(가운데 거북이)이 아닌 turtle2(다른 거북이)가 carrot1을 따라다니게 된다.

5.1 rviz

# 2nd terminal
$ rviz

rviz를 통하여 보다 분명하게 turtle2가 carrot1을 따라다니는 것을 볼 수 있다.

 

6. Broadcasting a moving frame

지금까지 carrot1이었던 extra frame은 fixed frame으로,

시간이 지나도 parent farme(turtle1)과의 상대적인 tf는 변하지 않았다.

 

만약, 시간에 따라 움직이는 moving frame은 아래와 같이 만들 수 있다.

$ nano ./nodes/dynamic_tf_broadcaster.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('dynamic_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        t = rospy.Time.now().to_sec() * math.pi
        br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

$ chmod +x ./nodes/dynamic_tf_broadcaster.py
$ nano ./launch/start_demo.launch

  <launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" />
    </node>
    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
    <node pkg="learning_tf" type="dynamic_tf_broadcaster.py" name="broadcaster_dynamic" />
  </launch>

위 start_demo.launch파일은

이전의 start_demo.launch파일의 fixed_tf_broadcaster.py부분을

아래와 같이 변경한 것이다.

<node pkg="learning_tf" type="dynamic_tf_broadcaster.py" name="broadcaster_dynamic" />

# 1st terminal
$ roslaunch learning_tf start_demo.launch

turtle2가 carrot1을 따라 빙글빙글 도는 것을 확인할 수 있다.

6.1 rviz

# 2nd terminal
$ rviz

rviz상에서 더욱 명확하게 보이는데,

carrot1이 turtle1의 주위를 빙글빙글 돌고 있으며,

turtle2가 그런 carrot1을 따라가고 있다.

 

ref.

http://wiki.ros.org/tf/Tutorials/Adding%20a%20frame%20%28Python%29

tf/Tutorials/Adding a frame (Python) - ROS Wiki

https://github.com/jstar0525/tf_tutorials

GitHub - jstar0525/tf_tutorials: tf_tutorial using ROS Melodic

$ git clone https://github.com/jstar0525/tf_tutorials.git
$ cd tf_tutorials/
$ git checkout 5e5908c6d0c4da36877fa7849c38729a8b8a5725