Finding real robot position using bpgt

asked 2017-11-05 16:47:11 -0600

bmancarth gravatar image

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 /base_pose_ground_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 /base_pose_ground_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()
edit retag flag offensive close merge delete