How to transform a goal point from world to the robot frame
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()
Asked by pealdu on 2017-10-24 09:04:59 UTC
Comments
What have you tried so far?
Asked by jayess on 2017-10-24 11:14:25 UTC
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
Asked by pealdu on 2017-10-24 11:43:56 UTC
Could you please update your question with this info?
Asked by jayess on 2017-10-24 11:46:37 UTC
Is this then entire source? I don't see any
import
s so I'm left wondering if anything else was excluded.Asked by jayess on 2017-10-24 12:13:43 UTC
I updated, that is the complete code
Asked by pealdu on 2017-10-24 12:55:04 UTC