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()
