Finding real robot position using bpgt

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

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.frame_id = "map"
    t.child_frame_id = "odom"
    t.transfrom.translation.x =
    t.transform.translation.y =
    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]
    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]

def __init__(self):
    print(startPos) = float(startPos[0]) = float(startPos[1]) = float(startPos[2])

    self.tf_broadcaster = tf2_ros.TransformBroadcaster()
    print("Robot is Actually at:")

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