Go to the documentation of this file.00001
00002
00003
00004
00005
00006 import threading
00007 import actionlib
00008 import dynamic_reconfigure.client
00009 import rospy
00010 import tf
00011 from yocs_navigator import BasicMoveController
00012 import std_msgs.msg as std_msgs
00013 import yocs_msgs.msg as yocs_msgs
00014 import geometry_msgs.msg as geometry_msgs
00015
00016 class LocalizationManager(object):
00017 """
00018 Uses poses from a pose tracker to initialise the global robot pose
00019 using a pose initialisation node such as fake_localization
00020 """
00021
00022 _localize_action_name = 'localize'
00023
00024 def __init__(self):
00025 self._initialise = False
00026 self._thread = None
00027 self._basic_move_controller = BasicMoveController()
00028 self._init_params()
00029
00030 if self.param['simulation']:
00031 self.loginfo("Running in simulation mode.")
00032 else:
00033 self._client = dynamic_reconfigure.client.Client(rospy.get_param('~pose_tracker', 'ar_track_alvar'))
00034 self._sub_tracked_poses = rospy.Subscriber('pose_tracker/poses', geometry_msgs.PoseWithCovarianceStamped, self._tracked_poses_callback)
00035
00036 self._pub_init_pose = rospy.Publisher('initialpose', geometry_msgs.PoseWithCovarianceStamped, latch=True, queue_size=3)
00037
00038 self._as_localize = actionlib.SimpleActionServer(self._localize_action_name, yocs_msgs.LocalizeAction, auto_start=False)
00039 self._as_localize.register_goal_callback(self._process_localize_goal)
00040 self._as_localize.register_preempt_callback(self._process_localize_preempt)
00041
00042 def _init_params(self):
00043 param = {}
00044 param['sleeptime'] = rospy.Duration(1 / rospy.get_param('~spin_freq', 10))
00045 param['simulation'] = rospy.get_param('~simulation', False)
00046 param['ar_pair_baseline'] = rospy.get_param('ar_pair/baseline', 0.28)
00047 param['ar_pair_target_offset'] = rospy.get_param('ar_pair/target_offset', 0.5)
00048 param['timeout'] = rospy.get_param('~timeout', 10.0)
00049
00050
00051 param['sim_global_frame'] = rospy.get_param('~simulation_global_frame','map')
00052 param['sim_x'] = rospy.get_param('~simulation_x', 0.0)
00053 param['sim_y'] = rospy.get_param('~simulation_y', 0.0)
00054 param['sim_a'] = rospy.get_param('~simulation_a', 0.0)
00055
00056 self.param = param
00057
00058 def _tracked_poses_callback(self, msg):
00059 if self._initialise:
00060
00061 msg.header.stamp -= rospy.Duration(0.2)
00062 cov = list(msg.pose.covariance)
00063 cov[6 * 0 + 0] = self._distortion * self._distortion
00064 cov[6 * 1 + 1] = self._distortion * self._distortion
00065 msg.pose.covariance = tuple(cov)
00066 self._pub_init_pose.publish(msg)
00067 self.loginfo("localization done.")
00068 self._initialise = False
00069
00070 def _process_localize_goal(self):
00071 goal = self._as_localize.accept_new_goal()
00072 self._distortion = goal.distortion
00073 self.loginfo("Received Localize goal %s"%str(goal))
00074
00075 if self._initialise:
00076 message = 'robot is initialising already. Ignore the command'
00077 self._send_result(False, message)
00078 else:
00079 if self.param['simulation']:
00080 pose_msg = geometry_msgs.PoseWithCovarianceStamped()
00081 pose_msg.header.frame_id = self.param['sim_global_frame']
00082 pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(0.2)
00083 pose_msg.pose.pose.position.x = self.param['sim_x']
00084 pose_msg.pose.pose.position.y = self.param['sim_y']
00085 pose_msg.pose.pose.position.z = 0.0
00086 quat = tf.transformations.quaternion_from_euler(0, 0, self.param['sim_a'])
00087 pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(*quat)
00088 self._pub_init_pose.publish(pose_msg)
00089
00090 self._send_result(True,'Initialisation done in simulation.')
00091 elif goal.command == goal.STAND_AND_LOCALIZE:
00092 self._thread = threading.Thread(target=self._stand_and_localize)
00093 self._thread.start()
00094 elif goal.command == goal.SPIN_AND_LOCALIZE:
00095 self._thread = threading.Thread(target=self._spin_and_localize)
00096 self._thread.start()
00097 else:
00098 message = 'Invalid command %s'%str(goal.command)
00099 self._send_result(False, message)
00100
00101 def _process_localize_preempt(self):
00102 self.logwarn('Received Preempt Request')
00103 pass
00104
00105 def _stand_and_localize(self):
00106 self.loginfo("Stand and Localization started.")
00107
00108
00109 self._update_tracker(True)
00110 self._initialise = True
00111
00112 timeout = self.param['timeout']
00113 sleeptime = self.param['sleeptime']
00114 start_time = rospy.Time.now()
00115 while not rospy.is_shutdown() and self._initialise:
00116 current_time = rospy.Time.now()
00117 dif = (current_time - start_time).to_sec()
00118 if dif > timeout:
00119 break
00120 rospy.sleep(sleeptime)
00121
00122
00123 self._update_tracker(False)
00124
00125
00126 if self._initialise:
00127 self._initialise = False
00128 self._send_result(False, "couldn't localize in time %s"%str(dif))
00129 else:
00130 self._send_result(True, "Localized")
00131
00132 def _spin_and_localize(self):
00133 self.loginfo("Spin and localization started")
00134
00135 self._update_tracker(True)
00136 self._initialise = True
00137
00138 self._basic_move_controller.spin_clockwise()
00139
00140 self._update_tracker(False)
00141 if self._initialise:
00142 self._initialise = False
00143 self._send_result(False, "couldn't localise after full spining")
00144 else:
00145 self._send_result(True, "Localised")
00146
00147 def _send_result(self, success, message):
00148 if success:
00149 self.loginfo(str(message))
00150 else:
00151 self.logwarn(str(message))
00152 r = yocs_msgs.LocalizeResult()
00153 r.success = success
00154 r.message = message
00155 self._as_localize.set_succeeded(r)
00156
00157 def _update_tracker(self, enabled):
00158 params = { 'enabled' : enabled}
00159 config = self._client.update_configuration(params)
00160
00161 def loginfo(self, msg):
00162 rospy.loginfo('Localization Manager : ' + str(msg))
00163
00164 def logwarn(self, msg):
00165 rospy.logwarn('Localization Manager : ' + str(msg))
00166
00167 def spin(self):
00168 sleeptime = self.param['sleeptime']
00169 self._as_localize.start()
00170 while not rospy.is_shutdown():
00171 rospy.sleep(sleeptime)
00172 if self._thread:
00173 self._thread.join()