move_base_db.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # This uses PostgreSQL and tf from /map_frame to /robot_frame
00004 #
00005 
00006 # parameters:
00007 #    dbname,hostname,port,username,passwd: for DB connection
00008 #    robot_frame: current robot base frame_id
00009 #    map_frame:   map origin frmae_id
00010 
00011 #                                  Table "public.tf"
00012 #         Column          |  Type   |                    Modifiers
00013 #-------------------------+---------+-------------------------------------------------
00014 # id                      | integer | not null default nextval('tf_id_seq'::regclass)
00015 # header_stamp            | bigint  |
00016 # header_frame_id         | text    |
00017 # child_frame_id          | text    |
00018 # transform_translation_x | real    |
00019 # transform_translation_y | real    |
00020 # transform_translation_z | real    |
00021 # transform_rotation_x    | real    |
00022 # transform_rotation_y    | real    |
00023 # transform_rotation_z    | real    |
00024 # transform_rotation_w    | real    |
00025 #Indexes:
00026 #    "tf_stamp_idx" btree (header_stamp)
00027 ### creating table
00028 # con = pgdb.connect(...)
00029 # cur = con.cursor()
00030 # cur.execute("CREATE TABLE tf (id int, header_stamp bigint, header_frame_id text, child_frame_id text, transform_translation_x real, transform_translation_y real, transform_translation_z real, transform_rotation_x real, transform_rotation_y real, transform_rotation_z real, transform_rotation_w real);")
00031 # cur.close()
00032 # con.commit()
00033 
00034 import roslib; roslib.load_manifest('jsk_pr2_startup')
00035 import rospy
00036 import pgdb
00037 import tf
00038 import geometry_msgs
00039 
00040 class MoveBaseDB:
00041     def __init__(self,connection=None,db_lock=None):
00042         db_name = rospy.get_param('~db_name','pr2db')
00043         host = rospy.get_param('~host_name','c1')
00044         port = rospy.get_param('~port',5432)
00045         username = rospy.get_param('~user_name','pr2admin')
00046         passwd = rospy.get_param('~passwd','')
00047         self.update_cycle = rospy.get_param('update_cycle', 1)
00048         # args dbname, host, port, opt, tty, user, passwd
00049         self.con = pgdb.connect(database=db_name, host=host, user=username, password=passwd)
00050         self.map_frame = rospy.get_param('~map_frame','/map')
00051         self.robot_frame = rospy.get_param('~robot_frame','/base_link')
00052         self.tf_listener = tf.TransformListener()
00053         self.initialpose_pub = rospy.Publisher('/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped)
00054         self.current_pose = None
00055         self.latest_pose = None
00056         self.latest_stamp = rospy.Time(0)
00057         self.load_latest_pose()
00058         self.pub_latest_pose()
00059         return
00060 
00061     # DB Insertion function
00062     def insert_pose_to_db(self, table, stamp, source, target, pose):
00063         ##rospy.loginfo("insert robot_base pose"+str(pose));
00064         (trans,rot) = pose
00065         cursor = self.con.cursor()
00066         cursor.execute("INSERT INTO %s (header_stamp,header_frame_id,child_frame_id,transform_translation_x, transform_translation_y, transform_translation_z, transform_rotation_x, transform_rotation_y, transform_rotation_z, transform_rotation_w) VALUES (%d,'%s','%s',%f,%f,%f,%f,%f,%f,%f);" % (table,stamp.to_nsec(),source,target,trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3]))
00067         cursor.close()
00068         self.con.commit()
00069 
00070     def load_latest_pose(self):
00071         cursor = self.con.cursor()
00072         pos_rot = cursor.execute("SELECT transform_translation_x, transform_translation_y, transform_translation_z, transform_rotation_x, transform_rotation_y, transform_rotation_z, transform_rotation_w FROM tf ORDER BY header_stamp DESC LIMIT 1")
00073         result = cursor.fetchall()
00074         if len(result) != 0:
00075             rospy.loginfo("latest position:  pos: %f %f %f, rot: %f %f %f %f" % (result[0][0], result[0][1], result[0][2], result[0][3], result[0][4], result[0][5], result[0][6],))
00076             self.current_pose = (result[0][0:3],result[0][3:])
00077             self.latest_pose = (result[0][0:3],result[0][3:])
00078         return
00079 
00080     def insert_current_pose(self):
00081         try:
00082             (trans,rot) = self.tf_listener.lookupTransform(self.map_frame,self.robot_frame,rospy.Time(0))
00083             stamp = self.tf_listener.getLatestCommonTime(self.map_frame,self.robot_frame)
00084             pose = (list(trans),list(rot))
00085 
00086             diffthre = 0.1 + 1.0 / (stamp - self.latest_stamp).to_sec()
00087             if (self.current_pose == None
00088                 or diffthre < (sum([(trans[i]-self.current_pose[0][i])**2 for i in [0,1,2]]) ** 0.5)
00089                 or diffthre/2 < (sum([(rot[i]-self.current_pose[1][i])**2 for i in [0,1,2,3]]) ** 0.5)):
00090                 self.insert_pose_to_db("tf",stamp,self.map_frame,self.robot_frame,pose)
00091                 self.current_pose = (trans,rot)
00092         except (tf.LookupException, tf.ConnectivityException, \
00093                 tf.ExtrapolationException):
00094             return
00095 
00096     def sleep_one_cycle(self):
00097         self.pub_latest_pose()
00098         rospy.sleep(self.update_cycle);
00099 
00100     def pub_latest_pose(self):
00101         if (self.latest_pose != None \
00102             and self.initialpose_pub.get_num_connections() > 0):
00103             try:
00104                 (trans, rot) = self.tf_listener.lookupTransform(self.map_frame, '/map', rospy.Time(0))
00105                 (ctrans, crot) = self.latest_pose
00106 
00107                 rospy.loginfo("set pos: %f %f %f, rot: %f %f %f %f" % (trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]))
00108                 rospy.loginfo("    pos: %f %f %f, rot: %f %f %f %f" % (ctrans[0], ctrans[1], ctrans[2], crot[0], crot[1], crot[2], crot[3]))
00109 
00110                 ps = geometry_msgs.msg.PoseWithCovarianceStamped()
00111                 ps.header.stamp = rospy.Time(0)
00112                 ps.header.frame_id = '/map'
00113                 ps.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
00114                 ps.pose.pose.position.x = ctrans[0] - trans[0]
00115                 ps.pose.pose.position.y = ctrans[1] - trans[1]
00116                 ps.pose.pose.position.z = ctrans[2] - trans[2]
00117                 ps.pose.pose.orientation.x = crot[0]
00118                 ps.pose.pose.orientation.y = crot[1]
00119                 ps.pose.pose.orientation.z = crot[2]
00120                 ps.pose.pose.orientation.w = crot[3]
00121                 self.initialpose_pub.publish (ps);
00122                 self.latest_pose = None
00123             except (tf.LookupException, tf.ConnectivityException, \
00124                         tf.ExtrapolationException):
00125                 return
00126 
00127 if __name__ == "__main__":
00128     rospy.init_node('move_base_db')
00129     obj = MoveBaseDB()
00130     while not rospy.is_shutdown():
00131         obj.insert_current_pose();
00132         obj.sleep_one_cycle();
00133     exit(1)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


jsk_pr2_startup
Author(s): Manabu Saito
autogenerated on Sat Mar 23 2013 17:11:33