in simulation_ws/src/simulation_app/scripts/file_uploader.py [0:0]
def main(self):
rate = rospy.Rate(10.0)
listener = tf.TransformListener()
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('map', '/base_link', rospy.Time(0))
nav_pose = {}
nav_pose['x'] = trans[0]
nav_pose['y'] = trans[1]
if self.norm_one_distance( nav_pose, self.prev_nav_pose ) > self.NORM_ONE_DISTANCE_THRESHOLD:
self.last_robot_moving_time = rospy.rostime.time.time()
self.prev_nav_pose = nav_pose
if (rospy.rostime.time.time() - self.last_robot_moving_time) > self.ROBOT_STOP_TIMEOUT:
rospy.logwarn('[file_uploader] Robot no longer moving')
self.write_map_and_terminate = True
if (rospy.rostime.time.time() - self.start_time) > self.TOTAL_MAPPING_TIMEOUT:
rospy.logwarn('[file_uploader] Simulation time threshold reached')
self.write_map_and_terminate = True
if (self.sent_terminate_command is False) and (self.write_map_and_terminate is True):
self.write_map_to_disk(self.LOCAL_MAP_WRITE_FOLDER, self.simulation_id)
self.cancel_job()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("[file_uploader] TF exception in gathering current position")
rate.sleep()