import argparse
import rospy
from sensor_msgs.msg import *
from nav_msgs.msg import *
from geometry_msgs.msg import *
# Parse Args
parser = argparse.ArgumentParser(description='Pose Scorer')
parser.add_argument('mapbag')
parser.add_argument('databag')
args = parser.parse_args()
# Get Data From Bag Files
the_map = get_dict( args.mapbag )['/map']
test_files = get_dict( args.databag )
scan = test_files['/base_scan']
truth = test_files['/base_pose_ground_truth']
pose = truth.pose.pose
true_pos = pose.position.x, pose.position.y, euler_from_quaternion((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))[2]
print "True Position:", true_pos
scan2 = LaserScan()
scan2.header = scan.header
scan2.angle_min = scan.angle_min
scan2.angle_max = scan.angle_max
scan2.angle_increment = scan.angle_increment
scan2.range_max = scan.range_max
import argparse
import rospy
from sensor_msgs.msg import *
from nav_msgs.msg import *
from geometry_msgs.msg import *
# Parse Args
parser = argparse.ArgumentParser(description='Pose Scorer')
parser.add_argument('mapbag')
parser.add_argument('databag')
args = parser.parse_args()
# Get Data From Bag Files
the_map = get_dict(args.mapbag)['/map']
test_files = get_dict(args.databag)
scan = test_files['/base_scan']
truth = test_files['/base_pose_ground_truth']
pose = truth.pose.pose
true_pos = pose.position.x, pose.position.y, euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))[2]
print "True Position:", true_pos
scan2 = LaserScan()
scan2.header = scan.header
scan2.angle_min = scan.angle_min
scan2.angle_max = scan.angle_max
scan2.angle_increment = scan.angle_increment