def test_add_query_point(self): to = TrackedObject("map") self.assertEquals(len(to._query_points.keys()), 0) to.add_query_point("pt1", None) self.assertEquals(len(to._query_points.keys()), 1) to.add_query_point("pt1", 3) self.assertEquals(len(to._query_points.keys()), 1) to.add_query_point("pt2", 4) self.assertEquals(len(to._query_points.keys()), 2)
def test_get_pose(self): to = TrackedObject("map") to._pose = "a" self.assertEquals(to.get_pose(), "a") to._pose = "b" self.assertEquals(to.get_pose(), "b")
def test_update_pose(self): to = TrackedObject("map") to.update_pose("a") self.assertEquals(to._pose, "a") to.update_pose("b") self.assertEquals(to._pose, "b")
def test_has_tracking_point(self): to = TrackedObject("map") to.add_tracking_point("Aruco", 0, "test") # Test Existence self.assertEquals(to.has_tracking_point("Aruco", 0), True) # Test Wrong System, Wrong ID self.assertEquals(to.has_tracking_point("Vicon", 1), False) # Test Wrong System, Correct ID self.assertEquals(to.has_tracking_point("Vicon", 0), False) # Test Correct System, Wrong ID self.assertEquals(to.has_tracking_point("Aruco", 1), False)
def test_get_query_point(self): to = TrackedObject("map") p1 = PoseStamped() p1.header.frame_id = 'map' p1.pose.position = Point(*(1, 2, 3)) p1.pose.orientation = Quaternion(*(0, 0, 0, 1)) to.add_query_point("p1", p1) self.assertEquals(to.get_query_point_pose("p1"), p1) p2 = PoseStamped() p2.header.frame_id = 'map' p2.pose.position = Point(*(4, 5, 6)) p2.pose.orientation = Quaternion(*(0, 0, 0, 1)) to.add_query_point("p2", p2) self.assertEquals(to.get_query_point_pose("p2"), p2)
def test_get_frame(self): to = TrackedObject("map") self.assertEquals(to.get_frame(), "map") to = TrackedObject("world") self.assertEquals(to.get_frame(), "world")
def test_adds_tracking_points(self): to = TrackedObject("map") # Easier to test with simple object instead of Pose to.add_tracking_point("Aruco", 0, "test") self.assertEquals(to.get_tracking_point_offset("Aruco", 0), "test") # Update; Should get replaced to.add_tracking_point("Aruco", 0, "test again") self.assertEquals(to.get_tracking_point_offset("Aruco", 0), "test again") # New one; Make sure is different to.add_tracking_point("Aruco", 1, "test third") self.assertEquals(to.get_tracking_point_offset("Aruco", 1), "test third") # Different system. Should allow id overlap to.add_tracking_point("Vicon", 0, "test vicon") self.assertEquals(to.get_tracking_point_offset("Vicon", 0), "test vicon")
def test_recognizes_tracking_systems(self): to = TrackedObject("map") self.assertEquals(to.is_tracked_by("Aruco"), False) self.assertEquals(to.is_tracked_by("Vicon"), False) to.add_tracking_point("Aruco", 0, None) self.assertEquals(to.is_tracked_by("Aruco"), True) self.assertEquals(to.is_tracked_by("Vicon"), False) to = TrackedObject("map") to.add_tracking_point("Vicon", 0, None) self.assertEquals(to.is_tracked_by("Aruco"), False) self.assertEquals(to.is_tracked_by("Vicon"), True) to = TrackedObject("map") to.add_tracking_point("Aruco", 0, None) to.add_tracking_point("Vicon", 0, None) self.assertEquals(to.is_tracked_by("Aruco"), True) self.assertEquals(to.is_tracked_by("Vicon"), True)
def test_initializes(self): to = TrackedObject("map") self.assertEquals(to._tracking_points, {})