def __init__(self): self._initialise = False self._thread = None self._basic_move_controller = BasicMoveController() self._init_params() if self.param['simulation']: self.loginfo("Running in simulation mode.") else: self._client = dynamic_reconfigure.client.Client( rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._sub_tracked_poses = rospy.Subscriber( 'pose_tracker/poses', geometry_msgs.PoseWithCovarianceStamped, self._tracked_poses_callback) self._pub_init_pose = rospy.Publisher( 'initialpose', geometry_msgs.PoseWithCovarianceStamped, latch=True, queue_size=3) self._as_localize = actionlib.SimpleActionServer( self._localize_action_name, yocs_msgs.LocalizeAction, auto_start=False) self._as_localize.register_goal_callback(self._process_localize_goal) self._as_localize.register_preempt_callback( self._process_localize_preempt)
def __init__(self): self._thread = None self._wait_for_drink_timeout = 10.0 self._cancel_requested = False self._init_params() self._init_ros_handles() self._basic_move_controller = BasicMoveController() self._vending_machine_target_frame = self._create_target_frame() self._client = dynamic_reconfigure.client.Client(rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._as_vm_interactor = actionlib.SimpleActionServer(self._vm_interactor_action_name, vending_machine_msgs.InteractorAction, auto_start=False) self._as_vm_interactor.register_goal_callback(self._process_interactor_goal) self._as_vm_interactor.register_preempt_callback(self._process_interactor_preempt)
def __init__(self): self._initialise = False self._thread = None self._basic_move_controller = BasicMoveController() self._init_params() if self.param['simulation']: self.loginfo("Running in simulation mode.") else: self._client = dynamic_reconfigure.client.Client(rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._sub_tracked_poses = rospy.Subscriber('pose_tracker/poses', geometry_msgs.PoseWithCovarianceStamped, self._tracked_poses_callback) self._pub_init_pose = rospy.Publisher('initialpose', geometry_msgs.PoseWithCovarianceStamped, latch=True, queue_size=3) self._as_localize = actionlib.SimpleActionServer(self._localize_action_name, yocs_msgs.LocalizeAction, auto_start=False) self._as_localize.register_goal_callback(self._process_localize_goal) self._as_localize.register_preempt_callback(self._process_localize_preempt)
class LocalizationManager(object): """ Uses poses from a pose tracker to initialise the global robot pose using a pose initialisation node such as fake_localization """ _localize_action_name = 'localize' def __init__(self): self._initialise = False self._thread = None self._basic_move_controller = BasicMoveController() self._init_params() if self.param['simulation']: self.loginfo("Running in simulation mode.") else: self._client = dynamic_reconfigure.client.Client( rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._sub_tracked_poses = rospy.Subscriber( 'pose_tracker/poses', geometry_msgs.PoseWithCovarianceStamped, self._tracked_poses_callback) self._pub_init_pose = rospy.Publisher( 'initialpose', geometry_msgs.PoseWithCovarianceStamped, latch=True, queue_size=3) self._as_localize = actionlib.SimpleActionServer( self._localize_action_name, yocs_msgs.LocalizeAction, auto_start=False) self._as_localize.register_goal_callback(self._process_localize_goal) self._as_localize.register_preempt_callback( self._process_localize_preempt) def _init_params(self): param = {} param['sleeptime'] = rospy.Duration(1 / rospy.get_param('~spin_freq', 10)) param['simulation'] = rospy.get_param('~simulation', False) param['ar_pair_baseline'] = rospy.get_param('ar_pair/baseline', 0.28) param['ar_pair_target_offset'] = rospy.get_param( 'ar_pair/target_offset', 0.5) param['timeout'] = rospy.get_param('~timeout', 10.0) # configurations for simulation param['sim_global_frame'] = rospy.get_param('~simulation_global_frame', 'map') param['sim_x'] = rospy.get_param('~simulation_x', 0.0) param['sim_y'] = rospy.get_param('~simulation_y', 0.0) param['sim_a'] = rospy.get_param('~simulation_a', 0.0) self.param = param def _tracked_poses_callback(self, msg): if self._initialise: # send pose to pose initialisation node msg.header.stamp -= rospy.Duration( 0.2) # TODO: get latest common time cov = list(msg.pose.covariance) cov[6 * 0 + 0] = self._distortion * self._distortion cov[6 * 1 + 1] = self._distortion * self._distortion msg.pose.covariance = tuple(cov) self._pub_init_pose.publish(msg) self.loginfo("localization done.") self._initialise = False def _process_localize_goal(self): goal = self._as_localize.accept_new_goal() self._distortion = goal.distortion self.loginfo("Received Localize goal %s" % str(goal)) if self._initialise: message = 'robot is initialising already. Ignore the command' self._send_result(False, message) else: if self.param['simulation']: pose_msg = geometry_msgs.PoseWithCovarianceStamped() pose_msg.header.frame_id = self.param['sim_global_frame'] pose_msg.header.stamp = rospy.Time.now() - rospy.Duration( 0.2) # TODO: get latest common time pose_msg.pose.pose.position.x = self.param['sim_x'] pose_msg.pose.pose.position.y = self.param['sim_y'] pose_msg.pose.pose.position.z = 0.0 quat = tf.transformations.quaternion_from_euler( 0, 0, self.param['sim_a']) pose_msg.pose.pose.orientation = geometry_msgs.Quaternion( *quat) self._pub_init_pose.publish(pose_msg) # send success right away self._send_result(True, 'Initialisation done in simulation.') elif goal.command == goal.STAND_AND_LOCALIZE: self._thread = threading.Thread( target=self._stand_and_localize) self._thread.start() elif goal.command == goal.SPIN_AND_LOCALIZE: self._thread = threading.Thread(target=self._spin_and_localize) self._thread.start() else: message = 'Invalid command %s' % str(goal.command) self._send_result(False, message) def _process_localize_preempt(self): self.logwarn('Received Preempt Request') pass def _stand_and_localize(self): self.loginfo("Stand and Localization started.") # enable pose tracker self._update_tracker(True) self._initialise = True timeout = self.param['timeout'] sleeptime = self.param['sleeptime'] start_time = rospy.Time.now() while not rospy.is_shutdown() and self._initialise: current_time = rospy.Time.now() dif = (current_time - start_time).to_sec() if dif > timeout: break rospy.sleep(sleeptime) # disable the pose tracker self._update_tracker(False) if self._initialise: # Timeout self._initialise = False self._send_result(False, "couldn't localize in time %s" % str(dif)) else: # localized self._send_result(True, "Localized") def _spin_and_localize(self): self.loginfo("Spin and localization started") self._update_tracker(True) self._initialise = True self._basic_move_controller.spin_clockwise() self._update_tracker(False) if self._initialise: self._initialise = False self._send_result(False, "couldn't localise after full spining") else: self._send_result(True, "Localised") def _send_result(self, success, message): if success: self.loginfo(str(message)) else: self.logwarn(str(message)) r = yocs_msgs.LocalizeResult() r.success = success r.message = message self._as_localize.set_succeeded(r) def _update_tracker(self, enabled): params = {'enabled': enabled} config = self._client.update_configuration(params) def loginfo(self, msg): rospy.loginfo('Localization Manager : ' + str(msg)) def logwarn(self, msg): rospy.logwarn('Localization Manager : ' + str(msg)) def spin(self): sleeptime = self.param['sleeptime'] self._as_localize.start() while not rospy.is_shutdown(): rospy.sleep(sleeptime) if self._thread: self._thread.join()
class LocalizationManager(object): """ Uses poses from a pose tracker to initialise the global robot pose using a pose initialisation node such as fake_localization """ _localize_action_name = 'localize' def __init__(self): self._initialise = False self._thread = None self._basic_move_controller = BasicMoveController() self._init_params() if self.param['simulation']: self.loginfo("Running in simulation mode.") else: self._client = dynamic_reconfigure.client.Client(rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._sub_tracked_poses = rospy.Subscriber('pose_tracker/poses', geometry_msgs.PoseWithCovarianceStamped, self._tracked_poses_callback) self._pub_init_pose = rospy.Publisher('initialpose', geometry_msgs.PoseWithCovarianceStamped, latch=True, queue_size=3) self._as_localize = actionlib.SimpleActionServer(self._localize_action_name, yocs_msgs.LocalizeAction, auto_start=False) self._as_localize.register_goal_callback(self._process_localize_goal) self._as_localize.register_preempt_callback(self._process_localize_preempt) def _init_params(self): param = {} param['sleeptime'] = rospy.Duration(1 / rospy.get_param('~spin_freq', 10)) param['simulation'] = rospy.get_param('~simulation', False) param['ar_pair_baseline'] = rospy.get_param('ar_pair/baseline', 0.28) param['ar_pair_target_offset'] = rospy.get_param('ar_pair/target_offset', 0.5) param['timeout'] = rospy.get_param('~timeout', 10.0) # configurations for simulation param['sim_global_frame'] = rospy.get_param('~simulation_global_frame','map') param['sim_x'] = rospy.get_param('~simulation_x', 0.0) param['sim_y'] = rospy.get_param('~simulation_y', 0.0) param['sim_a'] = rospy.get_param('~simulation_a', 0.0) self.param = param def _tracked_poses_callback(self, msg): if self._initialise: # send pose to pose initialisation node msg.header.stamp -= rospy.Duration(0.2) # TODO: get latest common time cov = list(msg.pose.covariance) cov[6 * 0 + 0] = self._distortion * self._distortion cov[6 * 1 + 1] = self._distortion * self._distortion msg.pose.covariance = tuple(cov) self._pub_init_pose.publish(msg) self.loginfo("localization done.") self._initialise = False def _process_localize_goal(self): goal = self._as_localize.accept_new_goal() self._distortion = goal.distortion self.loginfo("Received Localize goal %s"%str(goal)) if self._initialise: message = 'robot is initialising already. Ignore the command' self._send_result(False, message) else: if self.param['simulation']: pose_msg = geometry_msgs.PoseWithCovarianceStamped() pose_msg.header.frame_id = self.param['sim_global_frame'] pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(0.2) # TODO: get latest common time pose_msg.pose.pose.position.x = self.param['sim_x'] pose_msg.pose.pose.position.y = self.param['sim_y'] pose_msg.pose.pose.position.z = 0.0 quat = tf.transformations.quaternion_from_euler(0, 0, self.param['sim_a']) pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(*quat) self._pub_init_pose.publish(pose_msg) # send success right away self._send_result(True,'Initialisation done in simulation.') elif goal.command == goal.STAND_AND_LOCALIZE: self._thread = threading.Thread(target=self._stand_and_localize) self._thread.start() elif goal.command == goal.SPIN_AND_LOCALIZE: self._thread = threading.Thread(target=self._spin_and_localize) self._thread.start() else: message = 'Invalid command %s'%str(goal.command) self._send_result(False, message) def _process_localize_preempt(self): self.logwarn('Received Preempt Request') pass def _stand_and_localize(self): self.loginfo("Stand and Localization started.") # enable pose tracker self._update_tracker(True) self._initialise = True timeout = self.param['timeout'] sleeptime = self.param['sleeptime'] start_time = rospy.Time.now() while not rospy.is_shutdown() and self._initialise: current_time = rospy.Time.now() dif = (current_time - start_time).to_sec() if dif > timeout: break rospy.sleep(sleeptime) # disable the pose tracker self._update_tracker(False) if self._initialise: # Timeout self._initialise = False self._send_result(False, "couldn't localize in time %s"%str(dif)) else: # localized self._send_result(True, "Localized") def _spin_and_localize(self): self.loginfo("Spin and localization started") self._update_tracker(True) self._initialise = True self._basic_move_controller.spin_clockwise() self._update_tracker(False) if self._initialise: self._initialise = False self._send_result(False, "couldn't localise after full spining") else: self._send_result(True, "Localised") def _send_result(self, success, message): if success: self.loginfo(str(message)) else: self.logwarn(str(message)) r = yocs_msgs.LocalizeResult() r.success = success r.message = message self._as_localize.set_succeeded(r) def _update_tracker(self, enabled): params = { 'enabled' : enabled} config = self._client.update_configuration(params) def loginfo(self, msg): rospy.loginfo('Localization Manager : ' + str(msg)) def logwarn(self, msg): rospy.logwarn('Localization Manager : ' + str(msg)) def spin(self): sleeptime = self.param['sleeptime'] self._as_localize.start() while not rospy.is_shutdown(): rospy.sleep(sleeptime) if self._thread: self._thread.join()
class VendingMachineInteractor(object): """ Provides simple action to interact with vending machine. """ _vm_interactor_action_name = 'vm_interactor' def __init__(self): self._thread = None self._wait_for_drink_timeout = 10.0 self._cancel_requested = False self._init_params() self._init_ros_handles() self._basic_move_controller = BasicMoveController() self._vending_machine_target_frame = self._create_target_frame() self._client = dynamic_reconfigure.client.Client(rospy.get_param('~pose_tracker', 'ar_track_alvar')) self._as_vm_interactor = actionlib.SimpleActionServer(self._vm_interactor_action_name, vending_machine_msgs.InteractorAction, auto_start=False) self._as_vm_interactor.register_goal_callback(self._process_interactor_goal) self._as_vm_interactor.register_preempt_callback(self._process_interactor_preempt) def _init_params(self): param = {} param['vending_machine_ar_id'] = rospy.get_param('~vending_machine_ar_id') param['ar_pair_global_prefix'] = rospy.get_param('ar_pair/global_prefix', 'global_marker') param['ar_pair_target_postfix'] = rospy.get_param('ar_pair/target_postfix', 'target') self._param = param def _init_ros_handles(self): self._pub = {} self._sub = {} # VM controller self._sub[VM_ORDER_RESULT] = rospy.Subscriber(VM_ORDER_RESULT, std_msgs.Int8, self._process_vm_order_result) self._pub[VM_DRINK_ORDER] = rospy.Publisher(VM_DRINK_ORDER, std_msgs.Int8, queue_size=2) # AR Pair Approach self._pub[ENABLE_AR_PAIR_APPROACH] = rospy.Publisher(ENABLE_AR_PAIR_APPROACH, std_msgs.String, queue_size=2) self._pub[DISABLE_AR_PAIR_APPROACH] = rospy.Publisher(DISABLE_AR_PAIR_APPROACH, std_msgs.String, queue_size=2) self._sub[RESULT_AR_PAIR_APPROACH] = rospy.Subscriber(RESULT_AR_PAIR_APPROACH, std_msgs.Bool, self._process_ar_pair_approach_result) def _create_target_frame(self): vm_id = self._param['vending_machine_ar_id'] prefix = self._param['ar_pair_global_prefix'] postfix = self._param['ar_pair_target_postfix'] frame = str('%s_%s_%s'%(prefix, vm_id, postfix)) return frame def _process_vm_order_result(self, msg): self._drink_received = True def _process_interactor_goal(self): goal = self._as_vm_interactor.accept_new_goal() self.loginfo("Received VM interactor goal %s"%str(goal)) if goal.command == goal.APPROACH_VM: self._thread = threading.Thread(target=self._approach_vm) self._thread.start() elif goal.command == goal.ORDER_DRINK: self.loginfo(str(goal.drinks)) self._thread = threading.Thread(target=self._order_drink, args=(goal.drinks,)) self._thread.start() elif goal.command == goal.BACK_FROM_VM: self._thread = threading.Thread(target=self._back_from_vm) self._thread.start() else: message = "Invalid Command %s"%str(goal.command) self._send_result(False, message) self._cancel_requested = False def _process_interactor_preempt(self): self.logwarn('Received Preempt Request') self._cancel_requested = True def _process_ar_pair_approach_result(self, msg): self._ar_pair_approach_result = msg def _approach_vm(self): self._update_tracker(True) self._ar_pair_approach_result = None self.loginfo("target frame : %s"%self._vending_machine_target_frame) self._pub[ENABLE_AR_PAIR_APPROACH].publish(self._vending_machine_target_frame) while not self._ar_pair_approach_result and not self._cancel_requested and not rospy.is_shutdown(): rospy.sleep(0.1) self._update_tracker(False) if self._cancel_requested == True: self._pub[ENABLE_AR_PAIR_APPROACH].publish() self._send_result(False, "Preempted!") return if self._ar_pair_approach_result.data: self._send_result(True, "Success") else: self._send_result(False, "Failed") def _back_from_vm(self): self.loginfo('Move back...') self._basic_move_controller.backward(0.4) self._send_result(True, "Success") def _order_drink(self, drinks): self.loginfo('Order Drink %s'%str(drinks)) success = True if type(drinks) is list or type(drinks) is tuple: for d in drinks: #self.loginfo("Ordering %s"%d) #rospy.sleep(2.0) self._drink_received = False self._pub[VM_DRINK_ORDER].publish(d) success = self._wait_for_drink() else: #self.loginfo("Ordering %s"%drinks) #rospy.sleep(2.0) self._drink_received = False self._pub[VM_DRINK_ORDER].publish(drinks) success = self._wait_for_drink() if success: self._send_result(True, "Success") else: self._send_result(False, "Timeout!") def _wait_for_drink(self): t1 = rospy.Time.now() while not self._drink_received and not self._cancel_requested and not rospy.is_shutdown(): t2 = rospy.Time.now() #if (t2 - t1) > rospy.Duration.from_sec(self._wait_for_drink_timeout): # self.loginfo("t2 - t1 : %s"%(t2-t1)) # self.loginfo(rospy.Duration.from_sec(self._wait_for_drink_timeout)) # return False rospy.sleep(0.1) if self._cancel_requested == True: return False return True def _send_result(self, success, message): if success: self.loginfo(message) else: self.logwarn(message) r = vending_machine_msgs.InteractorResult() r.success = success r.message = message self._as_vm_interactor.set_succeeded(r) def _update_tracker(self, enabled): params = { 'enabled' : enabled} config = self._client.update_configuration(params) def loginfo(self, msg): rospy.loginfo('VM Interactor : %s'%str(msg)) def logwarn(self, msg): rospy.logwarn('VM Interactor : %s'%str(msg)) def spin(self): self.loginfo("VM Interactor : Ready for ID[%s]"%(self._param['vending_machine_ar_id'])) self._as_vm_interactor.start() rospy.spin()