def test_query_point_updates_with_base_pose(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)
to._pose.pose.position.x = 1
p2 = PoseStamped()
p2.header.frame_id = 'map'
p2.pose.position = Point(*(2, 2, 3))
p2.pose.orientation = Quaternion(*(0, 0, 0, 1))
self.assertEquals(to.get_query_point_pose("p1"), p2)
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)