00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 import roslib; roslib.load_manifest('warehouse_ros')
00041 import rospy
00042 import warehouse_ros as wr
00043 import unittest
00044 import math
00045 import geometry_msgs.msg as gm
00046
00047 def make_pose(x, y, th):
00048 return gm.Pose(gm.Point(x, y, 0),
00049 gm.Quaternion(0, 0, math.sin(th/2), math.cos(th/2)))
00050
00051 def make_metadata(p, str):
00052 return {'x': p.position.x, 'y': p.position.y, 'name': str}
00053
00054 def to_tuple(p):
00055 return (p.position.x, p.position.y, p.position.z, p.orientation.x,
00056 p.orientation.y, p.orientation.z, p.orientation.w)
00057
00058 def eq_poses(p1, p2):
00059 return to_tuple(p1)==to_tuple(p2)
00060
00061
00062 class TestMongoRospy(unittest.TestCase):
00063
00064 def test_basic(self):
00065
00066
00067 coll = wr.MessageCollection("my_db", "poses", gm.Pose)
00068 p1 = make_pose(24, 42, 0)
00069 p2 = make_pose(10, 532, 3)
00070 p3 = make_pose(53, 22, 5)
00071 p4 = make_pose(22, -5, 33)
00072
00073
00074 coll.insert(p1, make_metadata(p1, "bar"))
00075 coll.insert(p2, make_metadata(p2, "baz"))
00076 coll.insert(p3, make_metadata(p3, "qux"))
00077 coll.insert(p1, make_metadata(p1, "oof"))
00078 coll.insert(p4, make_metadata(p4, "ooof"))
00079
00080
00081 results = coll.query({'x': {'$lt': 40}, 'y': {'$gt': 0}},\
00082 sort_by='name', ascending=False)
00083
00084
00085 poses, metadata = zip(*list(results))
00086
00087 self.assertEqual(len(poses), 3)
00088 self.assertTrue(eq_poses(p1, poses[0]))
00089 self.assertTrue(eq_poses(p2, poses[1]))
00090 self.assertTrue(eq_poses(p1, poses[2]))
00091
00092 self.assertEqual(metadata[0]['name'], 'oof')
00093 self.assertEqual(metadata[1]['name'], 'baz')
00094 self.assertEqual(metadata[2]['name'], 'bar')
00095
00096
00097 coll.update(metadata[0], metadata={'name': 'bat'})
00098 coll.update(metadata[2], msg=p2)
00099 res2 = coll.query({'y': {'$gt': 40}}, sort_by='name')
00100 poses, metadata = zip(*list(res2))
00101
00102 self.assertEqual(metadata[0]['name'], 'bar')
00103 self.assertEqual(metadata[1]['name'], 'bat')
00104 self.assertEqual(metadata[2]['name'], 'baz')
00105 self.assertTrue(eq_poses(p2, poses[0]))
00106 self.assertTrue(eq_poses(p1, poses[1]))
00107 self.assertTrue(eq_poses(p2, poses[2]))
00108
00109
00110
00111 self.assertEqual(5, coll.count())
00112 self.assertEqual(2, coll.remove({'y': {'$lt': 30}}))
00113 self.assertEqual(3, coll.count())
00114
00115
00116 self.assertTrue(coll.find_one({'y': {'$gt': 30}}))
00117 self.assertFalse(coll.find_one({'y': {'$lt': 30}}))
00118
00119
00120 if __name__ == "__main__":
00121 rospy.init_node('test_mongo_rospy')
00122 import rostest
00123 rostest.rosrun('mongo_ros', 'test_mongo_rospy', TestMongoRospy)
00124
00125