map_test.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 
3 from __future__ import print_function
4 
5 import sys
6 import threading
7 import time
8 import unittest
9 import os
10 import math
11 
12 import rospy
13 import rostest
14 
15 from fiducial_msgs.msg import FiducialMapEntryArray
16 from geometry_msgs.msg import PoseWithCovarianceStamped
17 
18 NAME='map'
19 EPSILON=0.1
20 
21 """
22 A node to verify the map created by fiducial_slam. It will verify
23 that the specified map file contains at least min_lines lines,
24 and if the expect parameter is also given, it will check that
25 the position of the specfied fiducial. In addition, the pose
26 specified in expected_pose is verified
27 """
28 
29 def rad2deg(rad):
30  return rad * 180.0 / math.pi
31 
32 class MapTest(unittest.TestCase):
33  def __init__(self, *args):
34  super(MapTest, self).__init__(*args)
35  self.mapEntries = -1
36  self.map = {}
37  self.pose = None
38 
39  def mapCallback(self, msg):
40  self.mapEntries = len(msg.fiducials)
41  for fid in msg.fiducials:
42  self.map[fid.fiducial_id] = (fid.x, fid.y, fid.z,
43  rad2deg(fid.rx), rad2deg(fid.ry), rad2deg(fid.rz))
44 
45  def poseCallback(self, msg):
46  position = msg.pose.pose.position
47  orientation = msg.pose.pose.orientation
48  self.pose = (position.x, position.y, position.z,
49  orientation.x, orientation.y, orientation.z, orientation.w)
50 
51  def test_map(self):
52  rospy.init_node('test_map')
53  minLines = rospy.get_param("~min_lines", 1)
54  expect = rospy.get_param("~expect", "")
55  expectedPose = rospy.get_param("~expected_pose", "")
56 
57  rospy.Subscriber("/fiducial_map", FiducialMapEntryArray,
58  self.mapCallback)
59  rospy.Subscriber("/fiducial_pose", PoseWithCovarianceStamped,
60  self.poseCallback)
61 
62  print("test_map");
63  t = 0
64  while True:
65  if self.mapEntries >= minLines and self.pose != None:
66  if expectedPose != "":
67  ep = expectedPose.split()
68  for i in range(len(ep)):
69  if abs(float(ep[i]) - float(self.pose[i])) > EPSILON:
70  self.fail("pose %s" % (self.pose,))
71  if expect != "":
72  lines = expect.split(',')
73  for line in lines:
74  ex = line.split()
75  fid = int(ex[0])
76  ex = ex[1:]
77  if not self.map.has_key(fid):
78  self.fail("Fiducial %d not in map" % fid)
79  fiducial = self.map[fid]
80  for i in range(len(ex)):
81  if abs(float(ex[i]) - float(fiducial[i])) > EPSILON:
82  self.fail("fiducial %d %s expected %s" % \
83  (fid, fiducial, ex,))
84  return
85  time.sleep(0.5)
86 
87 if __name__ == '__main__':
88  try:
89  rostest.run('rostest', NAME, MapTest, sys.argv)
90  except KeyboardInterrupt:
91  pass
92  print("exiting")
def mapCallback(self, msg)
Definition: map_test.py:39
def __init__(self, args)
Definition: map_test.py:33
def poseCallback(self, msg)
Definition: map_test.py:45
def rad2deg(rad)
Definition: map_test.py:29
def test_map(self)
Definition: map_test.py:51


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Sat Jun 13 2020 04:04:52