move_base_db.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import rospy
00004 import tf2_ros
00005 
00006 try:
00007     import tf2 # groovy
00008 except:
00009     import tf2_py as tf2
00010 import geometry_msgs
00011 from mongodb_store.message_store import MessageStoreProxy
00012 
00013 class MoveBaseDB(object):
00014     def __init__(self):
00015         self.db_name = rospy.get_param('robot/database','jsk_robot_lifelog')
00016         try:
00017             self.col_name = rospy.get_param('robot/name')
00018         except KeyError as e:
00019             rospy.logerr("please specify param \"robot/name\" (e.g. pr1012, olive)")
00020             exit(1)
00021         self.update_cycle = rospy.get_param('~update_cycle', 1.0)
00022         self.map_frame = rospy.get_param('~map_frame','map')
00023         self.robot_frame = rospy.get_param('~robot_frame','base_link')
00024         self.tf_listener = tf2_ros.BufferClient("tf2_buffer_server")
00025         self.initialpose_pub = rospy.Publisher('/initialpose', geometry_msgs.msg.PoseWithCovarianceStamped)
00026         self.msg_store = MessageStoreProxy(database=self.db_name, collection=self.col_name)
00027         rospy.loginfo("connected to %s.%s" % (self.db_name, self.col_name))
00028         rospy.loginfo("map->robot: %s -> %s" % (self.map_frame, self.robot_frame))
00029 
00030         self.current_pose = None
00031         self.latest_pose = None
00032         self.latest_stamp = rospy.Time(0)
00033         self._load_latest_pose()
00034         self.pub_latest_pose()
00035         self.latest_exception = None
00036 
00037     def _insert_pose_to_db(self, map_to_robot):
00038         try:
00039             res = self.msg_store.insert(map_to_robot)
00040             rospy.loginfo("inserted %s : %s" % (res, map_to_robot))
00041         except Exception as e:
00042             rospy.logerr('failed to insert current robot pose to db: %s', e)
00043 
00044     def _load_latest_pose(self):
00045         updated = None
00046         try:
00047             updated = self.msg_store.query('geometry_msgs/TransformStamped', single=True, sort_query=[("$natural", -1)])
00048         except Exception as e:
00049             rospy.logerr('failed to load latest pose from db: %s' % e)
00050             if 'master has changed' in str(e):
00051                 self._load_latest_pose()
00052             else:
00053                 return
00054         if updated is not None:
00055             rospy.loginfo('found latest pose %s' % updated)
00056             self.current_pose = updated[0]
00057             self.latest_pose = updated[0]
00058 
00059     def _need_update_db(self, t):
00060         if self.current_pose is None:
00061             if self.latest_pose is None:
00062                 return True
00063             else:
00064                 return False
00065         diffthre = 0.1 + 1.0 / (rospy.Time.now() - self.latest_stamp).to_sec()
00066         diffpos = sum([(t.transform.translation.x - self.current_pose.transform.translation.x) ** 2,
00067                        (t.transform.translation.y - self.current_pose.transform.translation.y) ** 2,
00068                        (t.transform.translation.z - self.current_pose.transform.translation.z) ** 2]) ** 0.5
00069         diffrot = sum([(t.transform.rotation.x - self.current_pose.transform.rotation.x) ** 2,
00070                        (t.transform.rotation.y - self.current_pose.transform.rotation.y) ** 2,
00071                        (t.transform.rotation.z - self.current_pose.transform.rotation.z) ** 2,
00072                        (t.transform.rotation.w - self.current_pose.transform.rotation.w) ** 2]) ** 0.5
00073         rospy.loginfo("diffthre: %f, diffpos: %f, diffrot: %f" % (diffthre, diffpos, diffrot))
00074         if diffthre < diffpos or diffthre / 2.0 < diffrot:
00075             return True
00076         else:
00077             return False
00078 
00079     def insert_current_pose(self):
00080         try:
00081             transform = self.tf_listener.lookup_transform(self.map_frame,self.robot_frame,rospy.Time(0))
00082 #            rospy.loginfo("current pr2 location: %s" % transform)
00083             if self._need_update_db(transform):
00084                 self._insert_pose_to_db(transform)
00085                 self.current_pose = transform
00086         except Exception as e:
00087             if not self.latest_exception or str(e) is self.latest_exception:
00088                 rospy.logwarn('failed to get current robot pose from tf: %s' % e)
00089                 self.latest_exception = str(e)
00090 
00091     def sleep_one_cycle(self):
00092         rospy.sleep(self.update_cycle)
00093 
00094     def pub_latest_pose(self):
00095         if(self.latest_pose is not None and self.initialpose_pub.get_num_connections() > 0):
00096             pub_msg = geometry_msgs.msg.PoseWithCovarianceStamped()
00097             pub_msg.header.stamp = rospy.Time(0)
00098             pub_msg.header.frame_id = '/map'
00099             pub_msg.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]
00100             pub_msg.pose.pose.position.x = self.latest_pose.transform.translation.x
00101             pub_msg.pose.pose.position.y = self.latest_pose.transform.translation.y
00102             pub_msg.pose.pose.position.z = self.latest_pose.transform.translation.z
00103             pub_msg.pose.pose.orientation = self.latest_pose.transform.rotation
00104             try:
00105                 self.initialpose_pub.publish(pub_msg)
00106                 self.latest_pose = None
00107                 rospy.loginfo('published latest pose %s' % pub_msg)
00108             except Exception as e:
00109                 rospy.logerr('failed to publish initial robot pose: %s' % e)
00110 
00111 if __name__ == "__main__":
00112     rospy.init_node('move_base_db')
00113     obj = MoveBaseDB()
00114     while not rospy.is_shutdown():
00115         obj.insert_current_pose()
00116         obj.pub_latest_pose()
00117         obj.sleep_one_cycle()


jsk_pr2_startup
Author(s):
autogenerated on Wed Sep 16 2015 10:32:17