How to transform a goal point from world to the robot frame

asked 2017-10-24 09:04:59 -0500

pealdu gravatar image

updated 2017-10-24 12:54:31 -0500

I created a node to control the turtlesim turtle using python, this node receives the turtle pose (/turtle1/pose) and the goal (/goal) which is a Pose2D type. Basically the node publish to '/turtle1/cmd_vel' until the turtle arrive to that position. How do I transform that goal (given in the world frame ) into the turtle frame?

I understand the theory of frames, my problem is that I dont know how to implement that transformation in python (I did the tutorials 2 times)

In the Controller node I have this:

#!/usr/bin/env python
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Pose2D, Twist, TransformStamped
from math import pow, atan2, sqrt
import angles
import tf2_ros
import tf


def callback(data):
global pose
pose = data
pose.x = round(pose.x, 4)
pose.y = round(pose.y, 4)


def callback_2(data2):
global goal
goal = data2

def controlador():
global pose
global goal
rospy.init_node('controlador', anonymous=False)
rospy.Subscriber("/turtle1/pose", Pose, callback)    
pose = Pose()    
rospy.Subscriber("goal", Pose2D, callback_2)    
goal = Pose2D()
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
rate = rospy.Rate(10) # 10hz 

tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)

while not rospy.is_shutdown():

t = PointStamped()
    t.point.x = goal.x
    t.point.y = goal.y
    t.point.z = 0.0
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "world"

try:
        target_pt = tfBuffer.transform(t, "base_link")
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        rate.sleep()
        continue

    dx = goal.x - pose.x
    dy = goal.y - pose.y
    distance = sqrt(pow(dx, 2) + pow(dy, 2))
    angulodesejado = atan2(dy, dx)
    angulo = angles.shortest_angular_distance(pose.theta, angulodesejado)


    msg = Twist()

   #linear
    msg.linear.x = 1*distance
    msg.linear.y = 0
    msg.linear.z = 0       

    #angular
    msg.angular.x = 0
    msg.angular.y = 0
    msg.angular.z = 2*angulo

    pub.publish(msg) 
    rate.sleep()



    if __name__ == '__main__':
    controlador()
edit retag flag offensive close merge delete

Comments

What have you tried so far?

jayess gravatar image jayess  ( 2017-10-24 11:14:25 -0500 )edit

In the Controller node I create a PointStamped object from the goal position, then I used: try: target_pt = tfBuffer.transform(point, "base_link") except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue

pealdu gravatar image pealdu  ( 2017-10-24 11:43:56 -0500 )edit

Could you please update your question with this info?

jayess gravatar image jayess  ( 2017-10-24 11:46:37 -0500 )edit

Is this then entire source? I don't see any imports so I'm left wondering if anything else was excluded.

jayess gravatar image jayess  ( 2017-10-24 12:13:43 -0500 )edit

I updated, that is the complete code

pealdu gravatar image pealdu  ( 2017-10-24 12:55:04 -0500 )edit