ROS Learning Notes

ROS Learning Notes 10

Keypoints

• tf broadcast
• tf listener

tf broadcast

• tf broadcast is to broadcast the relationship between different coordinate frames.

    # include <ros/ros.h>
    # include <tf/transform_broadcaster.h>
    # include <turtlesim/Pose.h>
  
    std::string turtle_name;
  
    void poseCallback(const turtlesim::PoseConstPtr& msg){
  
        // create a broadcaster object
        static tf::TransformBroadcaster br;
  
        // initialize the transform object
        tf::Transform transform;
        transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
        tf::Quaternion q;
        q.setRPY(0, 0, msg->theta);
        transform.setRotation(q);
  
        // broadcast the transform between the world and the turtle
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
    }
  
    int main(int argc, char** argv)
    {
        ros::init(argc, argv, "my_tf_broadcaster");
  
        if(argc != 2){
            ROS_ERROR("need turtle name as argument");
            return -1;
        }
  
        turtle_name = argv[1];
  
        ros::NodeHandle node;
        ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  
        // loop wait for the callback
        ros::spin();
  
        return 0;
    }
  

    #!/usr/bin/env python
    # -*- coding: utf-8 -*-
  
    import rospy
    import tf
    import roslib
    roslib.load_manifest('learning_tf')
    import turtlesim.msg
  
    def handle_turtle_pose(msg, turtlename):
        br = tf.TransformBroadcaster()
        br.sendTransform((msg.x, msg.y, 0),
                         tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                         rospy.Time.now(),
                         turtlename,
                         "world")
  
    if __name__ == '__main__':
        rospy.init_node('turtle_tf_broadcaster')
        turtlename = rospy.get_param('~turtle')
        rospy.Subscriber('/%s/pose' % turtlename,
                         turtlesim.msg.Pose,
                         handle_turtle_pose,
                         turtlename)
        rospy.spin()
  

    add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
    target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
  

tf listener

• tf listener is to listen to the relationship between different coordinate frames.

    # include <ros/ros.h>
    # include <tf/transform_listener.h>
    # include <turtlesim/Spawn.h>
    # include <geometry_msgs/Twist.h>
  
    int main(int argc, char** argv)
    {
        ros::init(argc, argv, "my_tf_listener");
  
        ros::NodeHandle node;
  
        // spawn a turtle
        ros::service::waitForService("spawn");
        ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
        turtlesim::Spawn srv;
        add_turtle.call(srv);
  
        // create a velocity command publisher
        ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
  
        // create a listener object
        tf::TransformListener listener;
  
        // loop
        ros::Rate rate(10.0);
        while(node.ok()){
            tf::StampedTransform transform;
            try{
                // listen to the transform between the world and the turtle
                listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
                listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
            }
            catch(tf::TransformException &ex){
                ROS_ERROR("%s", ex.what());
                ros::Duration(1.0).sleep();
                continue;
            }
  
            geometry_msgs::Twist vel_msg;
            vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
            vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
            turtle_vel.publish(vel_msg);
  
            rate.sleep();
        }
  
        return 0;
    }
  

    #!/usr/bin/env python
    # -*- coding: utf-8 -*-
    import roslib
    roslib.load_manifest('learning_tf')
    import rospy
    import tf
    import math
    import turtlesim.srv
    import geometry_msgs.msg
  
    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', '/turtle1', 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()
  
  

    add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
    target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
  

Implementation

Below section is the same as a launch file

roscore
rosrun turtlesim turtlesim_node
## have to specify the name of the node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key 

( to kill roscore in backend, run killall -9 rosmaster)

When running python coed, also need to specify the name of the node

roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key