Finding real robot position using bpgt
I am trying to get the robot position in ROS using python, and I am struggling to get this working.
I need to subscribe to /baseposeground_truth and broadcast a transform of /map frame in order to find its location.
I have managed to find where the robot thinks it is using a transform to /odom, however for some reason or another I cannot seem to wrap my head around how to use /baseposeground_truth. Maybe I've been thinking about it too hard or long. Anyway here is my code:
#!/usr/bin/env python
import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Quaternion, Point, TransformStamped
from nav_msgs.msg import Odometry
class RealRobotPose:
def callback():
def handleBPGT(self,bpgt):
t = TransformStamped()
t.header.stamp=rospy.Time.now()
t.header.frame_id = "map"
t.child_frame_id = "odom"
t.transfrom.translation.x = bpgt.pose.pose.position.x+self.sx
t.transform.translation.y = bpgt.pose.pose.position.y+self.sy
t.transform.translation.z = 0.0
q = bpgt.pose.pose.orientation
qp = [q.x, q.y, q.z, q.w]
yaw = tf.transformations.euler_from_quaternion(qp)[2]+self.st
q = tf.transformations.quaternion_from_euler(0,0,yaw)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
self.tf_broadcaster.sendTransform(t)
def __init__(self):
rospy.init_node('real_robot_pose')
startPos=rospy.get_param("robot_start")
print(startPos)
self.sx = float(startPos[0])
self.sy = float(startPos[1])
self.st = float(startPos[2])
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
rospy.Subscriber("/odom",Odometry,self.handleBPGT)
print("Robot is Actually at:")
rospy.Subscriber("/base_pose_ground_truth",Odometry,self.callback)
if __name__ == '__main__':
d = RealRobotPose()
rospy.spin()
Asked by bmancarth on 2017-11-05 17:44:33 UTC
Comments