def __init__(self):
self.tools = Tools()
self.move_base_client = SimpleActionClient('/move_base',
MoveBaseAction)
self.approach_client = SimpleActionClient('/approach_action',
MoveBaseAction)
self.retreat_client = SimpleActionClient('/retreat_action',
MoveBaseAction)
self.camera_ptz_client = SimpleActionClient('/axis/axis_control',
CameraAction)
self.led_publishers = [
rospy.Publisher('/led0', Led),
rospy.Publisher('/led1', Led),
rospy.Publisher('/led2', Led)
]
self.verification_timer = None
self.cube_timer = None
self.standstill_timer = None
self._action = None
for client in [
self.move_base_client, self.approach_client,
self.retreat_client, self.camera_ptz_client
]:
if client:
rospy.loginfo('waiting for {} action server...'.format(
client.action_client.ns))
client.wait_for_server()
rospy.loginfo('connected to {} action server'.format(
client.action_client.ns))
rospy.loginfo('actions initialized')
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
EstopGuard.add_callback(self.estop_changed_cb)
# align camera
self.actions.look_to(self.look_done_cb)
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
self.at_approach = False
self.at_ring = False
self.loading_cube = False
self.unloading_cube = False
self.cube_operation_failure = False
self.number_approach_failure = False
self.standstill_failure = False
self.scan_scheduled = False
self.camera_aligned = False
self.cube_status_string = ''
self.status_string = ''
EstopGuard.add_callback(self.estop_changed_cb)
def __init__(self): self.actions = Actions() self.percepts = Percepts() self.tools = Tools() self.previous_scan_pose = None self.decision_required = True EstopGuard.add_callback(self.estop_changed_cb) # align camera self.actions.look_to(self.look_done_cb)
def __init__(self):
self.tools = Tools()
self.move_base_client = SimpleActionClient('/move_base', MoveBaseAction)
self.approach_client = SimpleActionClient('/approach_action', MoveBaseAction)
self.retreat_client = SimpleActionClient('/retreat_action', MoveBaseAction)
self.camera_ptz_client = SimpleActionClient('/axis/axis_control', CameraAction)
self.led_publishers = [rospy.Publisher('/led0', Led), rospy.Publisher('/led1', Led), rospy.Publisher('/led2', Led)]
self.verification_timer = None
self.cube_timer = None
self.standstill_timer = None
self._action = None
for client in [self.move_base_client, self.approach_client, self.retreat_client, self.camera_ptz_client]:
if client:
rospy.loginfo('waiting for {} action server...'.format(client.action_client.ns))
client.wait_for_server()
rospy.loginfo('connected to {} action server'.format(client.action_client.ns))
rospy.loginfo('actions initialized')
def __init__(self): self.actions = Actions() self.percepts = Percepts() self.tools = Tools() self.previous_scan_pose = None self.decision_required = True self.at_approach = False self.at_ring = False self.loading_cube = False self.unloading_cube = False self.cube_operation_failure = False self.number_approach_failure = False self.standstill_failure = False self.scan_scheduled = False self.camera_aligned = False self.cube_status_string = '' self.status_string = '' EstopGuard.add_callback(self.estop_changed_cb)
def __init__(self):
self.tools = Tools()
self.model = None
self.worldmodel_subscriber = rospy.Subscriber('/worldmodel/objects', ObjectModel, self.worldmodel_cb)
self.estimated_map_center = None
self.map = None
self.map_center_need_update = False
self.map_subscriber = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
self.cube = None
self.cube_subscriber = rospy.Subscriber('/cube_sensor', Bool, self.cube_sensor_cb)
self.barcode = None
self.cube_number = -1
self.detection_enabled = False
self.barcode_subscriber = rospy.Subscriber('/barcode', Int32, self.barcode_cb)
self.barcode_enable_service = rospy.ServiceProxy('/barcode_detection/enable_detection', Empty)
self.barcode_disable_service = rospy.ServiceProxy('/barcode_detection/disable_detection', Empty)
self.check_path_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
for service in [self.barcode_enable_service, self.barcode_disable_service, self.check_path_service]:
rospy.loginfo('waiting for service {}...'.format(service.resolved_name))
service.wait_for_service()
rospy.loginfo('service {} is now available.'.format(service.resolved_name))
rospy.loginfo('percepts initialized.')
class Actions(object):
def __init__(self):
self.tools = Tools()
self.move_base_client = SimpleActionClient('/move_base', MoveBaseAction)
self.approach_client = SimpleActionClient('/approach_action', MoveBaseAction)
self.retreat_client = SimpleActionClient('/retreat_action', MoveBaseAction)
self.camera_ptz_client = SimpleActionClient('/axis/axis_control', CameraAction)
self.led_publishers = [rospy.Publisher('/led0', Led), rospy.Publisher('/led1', Led), rospy.Publisher('/led2', Led)]
self.verification_timer = None
self.cube_timer = None
self.standstill_timer = None
self._action = None
for client in [self.move_base_client, self.approach_client, self.retreat_client, self.camera_ptz_client]:
if client:
rospy.loginfo('waiting for {} action server...'.format(client.action_client.ns))
client.wait_for_server()
rospy.loginfo('connected to {} action server'.format(client.action_client.ns))
rospy.loginfo('actions initialized')
def cancel_all_actions(self):
#rospy.loginfo('canceling all actions...')
if self._action:
if self._action.is_active():
self._action.cancel()
self.cancel_cube_timer()
self.cancel_verification_timer()
self.disable_LEDs()
def random_move(self, done_cb, timeout_cb):
stamped = PoseStamped()
range = self.tools.rnd.uniform(1.0, 2.5)
yaw_offset = self.tools.rnd.uniform(-0.15*math.pi, 0.35*math.pi)
stamped.pose.position.x = range * math.cos(yaw_offset)
stamped.pose.position.y = range * math.sin(yaw_offset)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw_offset)
stamped.pose.orientation.x = quat[0]
stamped.pose.orientation.y = quat[1]
stamped.pose.orientation.z = quat[2]
stamped.pose.orientation.w = quat[3]
stamped.header.frame_id = 'base_footprint'
stamped.header.stamp = rospy.Time.now()
self.cancel_standstill_timer()
self.move_to(stamped, done_cb, timeout_cb)
def move_to(self, stamped, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose = stamped
goal.target_pose.header.stamp = rospy.Time(0)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(stamped))
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().move_base_timeout, action_client=self.move_base_client, goal=goal)
def approach(self, stamped, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose = stamped
goal.target_pose.header.stamp = rospy.Time(0)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.5
msg.markers[-1].color.g = 0.8
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().approach_timeout, action_client=self.approach_client, goal=goal)
def retreat(self, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_footprint'
goal.target_pose.pose.position.x = -(Params().approach_distance - Params().ring_distance)
goal.target_pose.header.stamp = rospy.Time(0)
goal.target_pose.pose.orientation.w = 1
goal.target_pose = self.tools.transform_pose('map', goal.target_pose)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.8
msg.markers[-1].color.g = 0.5
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().approach_timeout, action_client=self.retreat_client, goal=goal)
def camera_sweep(self, done_cb, delta_yaw=None, duration=None, zoom=1):
if not delta_yaw:
delta_yaw = Params().axis_sweep_angle
if not duration:
duration = Params().axis_sweep_duration
goal = CameraGoal()
goal.command = 2 #sweep
goal.tilt = Params().axis_tilt
goal.sweep_step = delta_yaw
goal.sweep_hold_time = duration
self.cancel_all_actions()
self._action = ActionWrapper(action_client=self.camera_ptz_client, goal=goal, done_cb=done_cb)
def look_at(self, stamped, done_cb):
stamped.header.stamp = rospy.Time(0)
ps = Tools.tf_listener.transformPose('axis_link', stamped)
yaw = math.atan2(ps.pose.position.y, ps.pose.position.x)
pitch = Params().axis_tilt #math.atan2(ps.pose.position.z, ps.pose.position.x)
self.look_to(done_cb, yaw, pitch)
def look_to(self, done_cb, yaw=None, pitch=None, zoom=1):
if not yaw:
yaw = Params().axis_pan
if not pitch:
pitch = Params().axis_tilt
goal = CameraGoal()
goal.command = 1
goal.pan = yaw
goal.tilt = pitch
self.cancel_all_actions()
self._action = ActionWrapper(action_client=self.camera_ptz_client, goal=goal, done_cb=done_cb)
def start_verification_timer(self, timeout_cb):
self.verification_timer = rospy.Timer(rospy.Duration(Params().verification_duration), timeout_cb, oneshot=True)
def cancel_verification_timer(self):
if not self.cube_timer:
return
if self.cube_timer.is_alive():
self.cube_timer.shutdown()
def start_cube_operation_timer(self, timeout_cb):
if self.cube_timer:
if self.cube_timer.is_alive():
return
self.cube_timer = rospy.Timer(rospy.Duration(Params().cube_timeout), timeout_cb, oneshot=True)
def start_standstill_timer(self, timeout_cb):
if self.standstill_timer:
if self.standstill_timer.is_alive():
return
self.standstill_timer = rospy.Timer(rospy.Duration(Params().standstill_timeout), timeout_cb, oneshot=True)
def cancel_standstill_timer(self):
if not self.standstill_timer:
return
if self.standstill_timer.is_alive():
self.standstill_timer.shutdown()
def cancel_cube_timer(self):
if not self.cube_timer:
return
if self.cube_timer.is_alive():
self.cube_timer.shutdown()
def enable_LEDs(self):
msg = Led()
msg.value = Led.GREEN
for pub in self.led_publishers:
pub.publish(msg)
def disable_LEDs(self):
msg = Led()
msg.value = Led.BLACK
for pub in self.led_publishers:
pub.publish(msg)
class Actions(object):
def __init__(self):
self.tools = Tools()
self.move_base_client = SimpleActionClient('/move_base',
MoveBaseAction)
self.approach_client = SimpleActionClient('/approach_action',
MoveBaseAction)
self.retreat_client = SimpleActionClient('/retreat_action',
MoveBaseAction)
self.camera_ptz_client = SimpleActionClient('/axis/axis_control',
CameraAction)
self.led_publishers = [
rospy.Publisher('/led0', Led),
rospy.Publisher('/led1', Led),
rospy.Publisher('/led2', Led)
]
self.verification_timer = None
self.cube_timer = None
self.standstill_timer = None
self._action = None
for client in [
self.move_base_client, self.approach_client,
self.retreat_client, self.camera_ptz_client
]:
if client:
rospy.loginfo('waiting for {} action server...'.format(
client.action_client.ns))
client.wait_for_server()
rospy.loginfo('connected to {} action server'.format(
client.action_client.ns))
rospy.loginfo('actions initialized')
def cancel_all_actions(self):
#rospy.loginfo('canceling all actions...')
if self._action:
if self._action.is_active():
self._action.cancel()
self.cancel_cube_timer()
self.cancel_verification_timer()
self.disable_LEDs()
def random_move(self, done_cb, timeout_cb):
stamped = PoseStamped()
range = self.tools.rnd.uniform(1.0, 2.5)
yaw_offset = self.tools.rnd.uniform(-0.15 * math.pi, 0.35 * math.pi)
stamped.pose.position.x = range * math.cos(yaw_offset)
stamped.pose.position.y = range * math.sin(yaw_offset)
quat = tf.transformations.quaternion_from_euler(0, 0, yaw_offset)
stamped.pose.orientation.x = quat[0]
stamped.pose.orientation.y = quat[1]
stamped.pose.orientation.z = quat[2]
stamped.pose.orientation.w = quat[3]
stamped.header.frame_id = 'base_footprint'
stamped.header.stamp = rospy.Time.now()
self.cancel_standstill_timer()
self.move_to(stamped, done_cb, timeout_cb)
def move_to(self, stamped, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose = stamped
goal.target_pose.header.stamp = rospy.Time(0)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(stamped))
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb,
timeout_cb=timeout_cb,
timeout=Params().move_base_timeout,
action_client=self.move_base_client,
goal=goal)
def approach(self, stamped, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose = stamped
goal.target_pose.header.stamp = rospy.Time(0)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.5
msg.markers[-1].color.g = 0.8
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb,
timeout_cb=timeout_cb,
timeout=Params().approach_timeout,
action_client=self.approach_client,
goal=goal)
def retreat(self, done_cb, timeout_cb):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_footprint'
goal.target_pose.pose.position.x = -(Params().approach_distance -
Params().ring_distance)
goal.target_pose.header.stamp = rospy.Time(0)
goal.target_pose.pose.orientation.w = 1
goal.target_pose = self.tools.transform_pose('map', goal.target_pose)
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(goal.target_pose))
msg.markers[-1].color.r = 0.8
msg.markers[-1].color.g = 0.5
self.tools.visualization_publisher.publish(msg)
self.cancel_all_actions()
self.cancel_standstill_timer()
self._action = ActionWrapper(done_cb=done_cb,
timeout_cb=timeout_cb,
timeout=Params().approach_timeout,
action_client=self.retreat_client,
goal=goal)
def camera_sweep(self, done_cb, delta_yaw=None, duration=None, zoom=1):
if not delta_yaw:
delta_yaw = Params().axis_sweep_angle
if not duration:
duration = Params().axis_sweep_duration
goal = CameraGoal()
goal.command = 2 #sweep
goal.tilt = Params().axis_tilt
goal.sweep_step = delta_yaw
goal.sweep_hold_time = duration
self.cancel_all_actions()
self._action = ActionWrapper(action_client=self.camera_ptz_client,
goal=goal,
done_cb=done_cb)
def look_at(self, stamped, done_cb):
stamped.header.stamp = rospy.Time(0)
ps = Tools.tf_listener.transformPose('axis_link', stamped)
yaw = math.atan2(ps.pose.position.y, ps.pose.position.x)
pitch = Params(
).axis_tilt #math.atan2(ps.pose.position.z, ps.pose.position.x)
self.look_to(done_cb, yaw, pitch)
def look_to(self, done_cb, yaw=None, pitch=None, zoom=1):
if not yaw:
yaw = Params().axis_pan
if not pitch:
pitch = Params().axis_tilt
goal = CameraGoal()
goal.command = 1
goal.pan = yaw
goal.tilt = pitch
self.cancel_all_actions()
self._action = ActionWrapper(action_client=self.camera_ptz_client,
goal=goal,
done_cb=done_cb)
def start_verification_timer(self, timeout_cb):
self.verification_timer = rospy.Timer(rospy.Duration(
Params().verification_duration),
timeout_cb,
oneshot=True)
def cancel_verification_timer(self):
if not self.cube_timer:
return
if self.cube_timer.is_alive():
self.cube_timer.shutdown()
def start_cube_operation_timer(self, timeout_cb):
if self.cube_timer:
if self.cube_timer.is_alive():
return
self.cube_timer = rospy.Timer(rospy.Duration(Params().cube_timeout),
timeout_cb,
oneshot=True)
def start_standstill_timer(self, timeout_cb):
if self.standstill_timer:
if self.standstill_timer.is_alive():
return
self.standstill_timer = rospy.Timer(rospy.Duration(
Params().standstill_timeout),
timeout_cb,
oneshot=True)
def cancel_standstill_timer(self):
if not self.standstill_timer:
return
if self.standstill_timer.is_alive():
self.standstill_timer.shutdown()
def cancel_cube_timer(self):
if not self.cube_timer:
return
if self.cube_timer.is_alive():
self.cube_timer.shutdown()
def enable_LEDs(self):
msg = Led()
msg.value = Led.GREEN
for pub in self.led_publishers:
pub.publish(msg)
def disable_LEDs(self):
msg = Led()
msg.value = Led.BLACK
for pub in self.led_publishers:
pub.publish(msg)
class DeliverCubesStrategy(object):
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
self.at_approach = False
self.at_ring = False
self.loading_cube = False
self.unloading_cube = False
self.cube_operation_failure = False
self.number_approach_failure = False
self.standstill_failure = False
self.scan_scheduled = False
self.camera_aligned = False
self.cube_status_string = ''
self.status_string = ''
EstopGuard.add_callback(self.estop_changed_cb)
def print_state(self):
rospy.loginfo('***')
if self.cube_operation_failure:
self.cube_status_string = 'recovery'
elif self.percepts.cube_loaded():
self.cube_status_string = 'cube: {}'.format(
self.percepts.current_number())
else:
self.cube_status_string = 'no cube'
rospy.loginfo(self.cube_status_string)
rospy.loginfo(self.status_string)
def decide(self, event):
try:
self.print_state()
try:
self.percepts.visualize_worldmodel('{}\n{}'.format(
self.cube_status_string, self.status_string))
except NotEnoughDataException:
pass
if not self.decision_required:
return
Params().update()
self.percepts.estimate_center_from_map()
# cube operation failure recovery
if self.cube_operation_failure:
if self.at_ring:
self.retreat()
return
else:
self.cube_operation_failure = False
self.previous_scan_pose = None
self.explore()
return
# number operation failure recovery
if self.number_approach_failure:
self.number_approach_failure = False
self.previous_scan_pose = self.tools.get_current_pose()
self.explore()
return
# no move failure recovery
if self.standstill_failure:
self.standstill_failure = False
self.previous_scan_pose = self.tools.get_current_pose()
self.explore()
return
# cube operations
if self.at_ring:
if self.loading_cube:
self.load_cube()
return
elif self.unloading_cube:
self.unload_cube()
return
else:
# cube operation complete
self.retreat()
return
if not self.camera_aligned:
self.actions.look_to(self.look_ahead_done_cb)
self.actions.start_standstill_timer(self.standstill_timeout_cb)
# drive and camera operations
if not self.percepts.cube_loaded():
try:
# approach loading station
if Params().hack_octa_loading_station:
self.approach_average_of_two_loading_station()
else:
self.approach_loading_station()
return
except NotEnoughDataException as e:
# not enough data to find loading station
self.explore()
return
else:
try:
# approach number
self.approach_number()
return
except NotEnoughDataException as e:
# not enough data to find number
self.explore()
return
except NotEnoughDataException as e:
rospy.logwarn('exception {}: {}'.format(type(e), e.message))
rospy.sleep(1.0)
except Exception as e:
rospy.logerr(traceback.format_exc())
def load_cube(self):
self.actions.enable_LEDs()
if not self.percepts.cube_loaded():
self.actions.start_cube_operation_timer(
self.cube_operation_timeout_cb)
self.status_string = 'waiting for cube...'
rospy.loginfo(self.status_string)
return # wait
number = self.percepts.current_number()
self.status_string = 'cube received, number {}.'.format(number)
rospy.loginfo(self.status_string)
self.loading_cube = False
self.actions.cancel_cube_timeout()
def unload_cube(self):
self.actions.enable_LEDs()
if self.percepts.cube_loaded():
self.actions.start_cube_operation_timer(
self.cube_operation_timeout_cb)
self.status_string = 'waiting for cube removal...'
rospy.loginfo(self.status_string)
return # wait
self.status_string = 'cube removed.'
rospy.loginfo(self.status_string)
self.unloading_cube = False
self.actions.cancel_cube_timeout()
def approach_loading_station(self):
current = self.tools.get_current_pose()
loading_station = self.percepts.get_best_loading_station(current)
if self.tools.is_good_approach_pose(current, loading_station):
self.status_string = 'approaching loading station...'
rospy.loginfo(self.status_string)
stamped = PoseStamped()
stamped.header = loading_station.header
stamped.pose = self.tools.project_pose(loading_station.pose.pose)
stamped.pose.position.z = -0.01
self.actions.approach(stamped,
self.approach_loading_station_done_cb,
self.approach_timeout_cb)
else:
self.status_string = 'moving to loading station...'
rospy.loginfo(self.status_string)
approach = self.percepts.sample_approach_pose(
loading_station.pose.pose, loading_station.header.frame_id)
self.actions.move_to(approach, self.preapproach_done_cb,
self.move_timeout_cb)
self.decision_required = False
def approach_average_of_two_loading_station(self):
current = self.tools.get_current_pose()
approach = self.percepts.get_best_average_approach_from_two_line_markers(
current)
if self.tools.xy_distance(current, approach) < 0.1:
self.status_string = 'approaching loading station...'
rospy.loginfo(self.status_string)
stamped = PoseStamped(header=approach.header)
offset = Pose()
offset.position.x = Params().approach_distance
offset.orientation.w = 1
stamped.pose = self.tools.add_poses(approach.pose, offset)
stamped.pose.position.z = -0.01
self.actions.approach(stamped,
self.approach_loading_station_done_cb,
self.approach_timeout_cb)
else:
self.status_string = 'moving to loading station...'
rospy.loginfo(self.status_string)
self.actions.move_to(approach, self.preapproach_done_cb,
self.move_timeout_cb)
self.decision_required = False
def approach_number(self):
number = self.percepts.current_number()
banner = self.percepts.get_number_banner(number)
current = self.tools.get_current_pose()
if banner.info.support < Params().min_trusted_support:
# go to some good pose
# look at the number for x seconds
if self.tools.is_good_verification_pose(current, banner):
# look at number
self.status_string = 'looking at {}'.format(number)
self.actions.look_at(PoseStamped(header=banner.header,
pose=banner.pose.pose),
done_cb=self.verification_lookat_done_cb)
else:
for i in range(20):
stamped = self.tools.sample_verification_pose(
banner, self.percepts.estimate_center_from_map())
sampled_poses = []
if self.percepts.check_path(current, stamped):
self.status_string = 'moving to look at {}'.format(
number)
rospy.loginfo(self.status_string)
self.actions.move_to(
stamped,
done_cb=self.move_to_verify_done_cb,
timeout_cb=self.move_timeout_cb)
self.decision_required = False
return
sampled_poses.append(stamped)
self.tools.visualize_poses(sampled_poses,
ns='failed_verification_poses')
self.status_string = 'failure at {}'.format(number)
self.number_approach_failure = True
elif self.tools.is_good_approach_pose(current, banner):
self.status_string = 'approaching number {}...'.format(number)
rospy.loginfo(self.status_string)
stamped = PoseStamped()
stamped.header = banner.header
stamped.pose = self.tools.project_pose(banner.pose.pose)
#stamped.pose = self.tools.set_orientation_from_yaw(banner.pose.pose, self.tools.get_yaw(banner.pose.pose) + math.pi)
stamped.pose.position.z = 0.01
self.actions.approach(stamped, self.approach_number_done_cb,
self.approach_timeout_cb)
else:
self.status_string = 'moving to number {}...'.format(number)
rospy.loginfo(self.status_string)
approach = self.percepts.sample_approach_pose(
banner.pose.pose, banner.header.frame_id)
self.actions.move_to(approach, self.preapproach_done_cb,
self.move_timeout_cb)
self.decision_required = False
def retreat(self):
self.status_string = 'retreating...'
rospy.loginfo(self.status_string)
self.actions.disable_LEDs()
self.actions.retreat(self.retreat_done_cb, self.retreat_timeout_cb)
self.decision_required = False
def explore(self):
# scan
if not self.previous_scan_pose:
self.previous_scan_pose = self.tools.get_current_pose()
current_pose = self.tools.get_current_pose()
if self.tools.xy_distance(
self.previous_scan_pose,
current_pose) > Params().min_travel_distance_for_rescan:
self.status_string = 'sweeping...'
rospy.loginfo(self.status_string)
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
self.status_string = 'exploring...'
rospy.loginfo(self.status_string)
exploration_pose = self.percepts.sample_scan_pose()
self.actions.move_to(exploration_pose, self.move_to_scan_done_cb,
self.move_to_scan_timeout_cb)
self.decision_required = False
def sweep_done_cb(self, status, result):
self.status_string = 'camera_sweep: done'
rospy.loginfo(self.status_string)
self.previous_scan_pose = self.tools.get_current_pose()
self.camera_aligned = False
self.decision_required = True
def look_ahead_done_cb(self, status, result):
self.status_string = 'camera_look_ahead: done'
rospy.loginfo(self.status_string)
self.camera_aligned = True
self.decision_required = True
def approach_loading_station_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'approach_loading_station: done'
rospy.loginfo(self.status_string)
self.loading_cube = True
self.actions.enable_LEDs()
#self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.at_ring = True
self.decision_required = True
def approach_number_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'approach_number: done'
rospy.loginfo(self.status_string)
self.unloading_cube = True
#self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.at_ring = True
self.decision_required = True
def retreat_done_cb(self, status, result):
self.status_string = 'retreat: done'
rospy.loginfo(self.status_string)
self.at_ring = False
self.decision_required = True
def preapproach_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'pre-approach: done'.format(status, result)
rospy.loginfo(self.status_string)
self.decision_required = True
def move_to_scan_done_cb(self, status, result):
self.status_string = 'move to scan: done'
rospy.loginfo(self.status_string)
self.scan_scheduled = True
self.decision_required = True
def move_to_scan_timeout_cb(self, event):
self.status_string = 'move to scan: timeout'
rospy.loginfo(self.status_string)
self.decision_required = True
def move_to_verify_done_cb(self, status, result):
self.status_string = 'move to verify: done'
rospy.loginfo(self.status_string)
self.decision_required = True
def move_timeout_cb(self, event):
self.status_string = 'move to verify: timeout'
rospy.loginfo(self.status_string)
self.decision_required = True
def approach_timeout_cb(self, event):
self.status_string = 'approach: timeout'
rospy.loginfo(self.status_string)
self.at_ring = True
self.decision_required = True
def retreat_timeout_cb(self, event):
self.status_string = 'retreat: timeout'
rospy.loginfo(self.status_string)
# TODO: do something here... retreat?
self.decision_required = True
def cube_operation_timeout_cb(self, event):
self.status_string = 'cube_operation: timeout'
rospy.loginfo(self.status_string)
self.cube_operation_failure = True
self.decision_required = True
def verification_lookat_done_cb(self, status, result):
self.actions.start_verification_timer(self.verification_timeout_cb)
def verification_timeout_cb(self, event):
self.status_string = 'verification: done'
rospy.loginfo(self.status_string)
self.decision_required = True
def standstill_timeout_cb(self, event):
self.status_string = 'no_move: timeout'
rospy.loginfo(self.status_string)
self.standstill_failure = True
self.decision_required = True
def estop_changed_cb(self, stop):
if stop:
self.status_string = 'estop triggered.'
rospy.loginfo(self.status_string)
self.decision_required = False
rospy.Rate(4).sleep() # let current decisions finish...
self.actions.cancel_all_actions() # ... and then cancel them.
self.actions.cancel_standstill_timer()
else:
self.status_string = 'estop released.'
rospy.loginfo(self.status_string)
self.decision_required = True
class Percepts(object):
def __init__(self):
self.tools = Tools()
self.model = None
self.worldmodel_subscriber = rospy.Subscriber('/worldmodel/objects', ObjectModel, self.worldmodel_cb)
self.estimated_map_center = None
self.map = None
self.map_center_need_update = False
self.map_subscriber = rospy.Subscriber('/map', OccupancyGrid, self.map_cb)
self.cube = None
self.cube_subscriber = rospy.Subscriber('/cube_sensor', Bool, self.cube_sensor_cb)
self.barcode = None
self.cube_number = -1
self.detection_enabled = False
self.barcode_subscriber = rospy.Subscriber('/barcode', Int32, self.barcode_cb)
self.barcode_enable_service = rospy.ServiceProxy('/barcode_detection/enable_detection', Empty)
self.barcode_disable_service = rospy.ServiceProxy('/barcode_detection/disable_detection', Empty)
self.check_path_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
for service in [self.barcode_enable_service, self.barcode_disable_service, self.check_path_service]:
rospy.loginfo('waiting for service {}...'.format(service.resolved_name))
service.wait_for_service()
rospy.loginfo('service {} is now available.'.format(service.resolved_name))
rospy.loginfo('percepts initialized.')
def map_to_world(self, x, y):
if not self.map:
raise NotEnoughDataException('no map message received.')
stamped = PoseStamped()
stamped.header.frame_id = self.map.header.frame_id
stamped.pose.position.x = x * self.map.info.resolution
stamped.pose.position.y = y * self.map.info.resolution
stamped.pose.orientation.w = 1
origin_transform = pm.fromMsg(self.map.info.origin)
center_transform = pm.fromMsg(stamped.pose)
stamped.pose = pm.toMsg(origin_transform * center_transform)
return stamped
def check_path(self, start, goal):
response = self.check_path_service.call(start=start, goal=goal)
return len(response.plan.poses) > 0
def cube_loaded(self):
if not self.cube:
raise NotEnoughDataException('no cube sensor message received.')
return self.cube.data
def current_number(self):
if self.cube_number == -1:
raise NotEnoughDataException('no barcode detected.')
return self.cube_number
def estimate_center_from_map(self):
if not self.map:
raise NotEnoughDataException('no map message received.')
if self.estimated_map_center and not self.map_center_need_update:
return self.estimated_map_center
map = self.map
# estimate center cell based on free cells
mean_x = 0
mean_y = 0
counter = 0
for x in range(0, map.info.width, 2):
for y in range(0, map.info.height, 2):
index = y * map.info.width + x
if map.data[index] == -1: #unknown
continue
if map.data[index] < 30: # free
mean_x += x
mean_y += y
counter += 1
self.estimated_map_center = self.map_to_world(mean_x / float(counter), mean_y / float(counter))
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(self.estimated_map_center, ns='worldmodel/map_center', z_offset=0.2))
msg.markers[-1].color.b = 0.8
msg.markers[-1].type = Marker.CYLINDER
self.tools.visualization_publisher.publish(msg)
self.map_center_need_update = False
return self.estimated_map_center
def estimate_center_from_worldmodel(self):
if not self.model:
raise NotEnoughDataException('no worldmodel message received.')
min_x = 1000
max_x = -1000
min_y = 1000
max_y = -1000
for object in self.model.objects:
min_x = min(object.pose.pose.position.x, min_x)
max_x = max(object.pose.pose.position.x, max_x)
min_y = min(object.pose.pose.position.y, min_y)
max_y = max(object.pose.pose.position.y, max_y)
stamped = PoseStamped()
stamped.header.frame_id = self.map.header.frame_id
stamped.pose.position.x = (max_x + min_x) / 2.0
stamped.pose.position.y = (max_y + min_y) / 2.0
stamped.pose.orientation.w = 1
msg = MarkerArray()
msg.markers.append(self.tools.create_pose_marker(stamped, ns='worldmodel/model_center', z_offset=0.2))
msg.markers[-1].color.b = 0.8
msg.markers[-1].color.g = 0.8
msg.markers[-1].type = Marker.CYLINDER
self.tools.visualization_publisher.publish(msg)
return stamped
def visualize_worldmodel(self, status_string=''):
if not self.model:
raise NotEnoughDataException('no worldmodel message received.')
msg = MarkerArray()
# loading stations
id = 0
try:
for id, station in enumerate(self.get_loading_stations()):
approach_pose = self.sample_approach_pose(station.pose.pose, station.header.frame_id)
msg.markers.extend(self.tools.create_loading_station_markers(station, approach_pose, id))
except NotEnoughDataException:
rospy.loginfo('no loading stations known.')
finally:
for id in range(id+1, 10):
msg.markers.extend(self.tools.create_loading_station_markers(id=id))
# banners
for number in range(10):
try:
banner = self.get_number_banner(number)
approach = self.sample_approach_pose(banner.pose.pose, banner.header.frame_id)
msg.markers.extend(self.tools.create_banner_markers(banner, approach, id=number))
except NotEnoughDataException:
#rospy.loginfo('number {} unknown.'.format(number))
msg.markers.extend(self.tools.create_banner_markers(id=number))
msg.markers.append(self.tools.create_status_marker(text=status_string))
self.tools.visualization_publisher.publish(msg)
def get_best_loading_station(self, current):
all = self.get_loading_stations()
good = []
if Params().smart_loading_station_selection:
for loading_station in all:
approach_pose = self.sample_approach_pose(loading_station.pose.pose, loading_station.header.frame_id)
valid = self.check_path(current, approach_pose)
if valid:
offset = Pose()
offset.position.x = Params().approach_test_distance
offset.orientation.w = 1
goal = PoseStamped(header=current.header)
goal.pose = self.tools.add_poses(approach_pose.pose, offset)
valid = self.check_path(approach_pose, goal)
if valid:
good.append(loading_station)
else:
good = all
if Params().random_loading_stations:
return self.tools.rnd.choice(good)
else:
good.sort(key=lambda object: self.tools.xy_point_distance(object.pose.pose.position, current.pose.position))
return good[0]
def get_best_average_approach_from_two_line_markers(self, current):
center = self.estimate_center_from_map()
all = self.get_loading_stations()
all.sort(key=lambda object: math.pi + self.tools.get_map_yaw(center.pose, object.pose.pose))
inbetween_approach_poses = []
approaches = []
for i in range(len(all)):
first = all[i]
second = all[(i+1)%len(all)]
if self.tools.xy_point_distance(first.pose.pose.position, second.pose.pose.position) < Params().max_merge_distance:
stamped = PoseStamped(header=center.header)
stamped.pose = self.tools.sample_approach_from_averaged(first.pose.pose, second.pose.pose)
approaches.append(stamped)
if Params().smart_loading_station_selection:
good = []
for approach_pose in approaches:
valid = self.check_path(current, approach_pose)
if valid:
good.append(approach_pose)
else:
good = approaches
good.sort(key=lambda stamped: self.tools.xy_point_distance(stamped.pose.position, current.pose.position))
return good[0]
def get_loading_stations(self):
center = self.estimate_center_from_map()
if not self.model:
raise NotEnoughDataException('no worldmodel message received.')
model = self.model
lines = [object for object in model.objects if object.info.class_id == 'isolated_lines' and self.tools.xy_point_distance(object.pose.pose.position, center.pose.position) < Params().max_loading_station_distance_from_center]
if len(lines) == 0:
raise NotEnoughDataException('no known loading stations.')
return lines
def get_closest_wall(self, point):
if not self.model:
raise NotEnoughDataException('no worldmodel message received.')
model = self.model
lines = [object for object in model.objects if object.info.class_id == 'connected_lines' and self.tools.xy_point_distance(object.pose.pose.position, point) < Params().approach_distance]
if len(lines) == 0:
raise NotEnoughDataException('no known walls.')
lines.sort(key=lambda object: self.tools.xy_point_distance(object.pose.pose.position, point))
return lines[0]
def _fix_banner_orientation(self, banner):
try:
wall = self.get_closest_wall(banner.pose.pose.position)
yaw = self.tools.get_yaw(wall.pose.pose)
banner.pose.pose = self.tools.set_orientation_from_yaw(banner.pose.pose, yaw)
return
except NotEnoughDataException:
pass
center = self.estimate_center_from_map()
angle = self.tools.get_facing_angle(center.pose, banner.pose.pose)
if abs(angle) < math.pi/2.0:
yaw = self.tools.get_yaw(banner.pose.pose)
banner.pose.pose = self.tools.set_orientation_from_yaw(banner.pose.pose, yaw+math.pi)
def get_number_banner(self, number):
banners = self.get_number_banner_data(number)
self._fix_banner_orientation(banners[0])
return banners[0]
def filter_conflicting_banners(self, number_objects):
""" Adjust the support for all number objects that are at the same pose.
If there is a dominating one, reduce the support for all others to be non-trustworthy.
If there are multiple dominating ones, reduce all to avoid confusions."""
# FIXME The second part might destroy some high support if the vision detects two
# numbers consistently. Shouldn't happen?!?
# For now we do this locally without clustering.
# If we reduce the support of one, it can't dominate its neighbors any more,
# so the order matters unless clustered beforehand.
def get_neighbors(number_obj, objects):
""" return all objects around number_obj (including that)
that are in number_conflict_distance and
also have at least min_trusted_support"""
return [obj for obj in objects if
self.tools.xy_point_distance(
number_obj.pose.pose.position, obj.pose.pose.position) <= Params().number_conflict_distance
and obj.info.support >= Params().min_trusted_support]
for no in number_objects:
if no.info.support < Params().min_trusted_support: # this one is bad anyways
continue
neigh = get_neighbors(no, number_objects)
if len(neigh) <= 1: # only we are in here
continue
# OK, we are trusted AND we have trusted neighbors
# That is bad.
neigh.sort(key = lambda obj: -obj.info.support)
# If there is a single dominator, bring all others down
if(neigh[0].info.support >= Params().domination_factor * neigh[1].info.support):
for nn in neigh[1:]:
rospy.loginfo("Reducing support below trusted for dominated by %s pose for %s, at %f %f. Had: %f support." % (neigh[0].info.class_id, nn.info.class_id, nn.pose.pose.position.x, nn.pose.pose.position.y, nn.info.support))
nn.info.support = Params().min_trusted_support - 1
else:
# If the best one isn't a single dominator, bring all down - this position is confusing
for nn in neigh:
rospy.loginfo("Reducing support below trusted for conflicting poses for %s, at %f %f. Had: %f support." % (nn.info.class_id, nn.pose.pose.position.x, nn.pose.pose.position.y, nn.info.support))
nn.info.support = Params().min_trusted_support - 1
def get_number_banner_data(self, number):
if not self.model:
raise NotEnoughDataException('no worldmodel message received.')
number_class = 'number_banner_{}'.format(number)
model = self.model
banners = [object for object in model.objects
if object.info.class_id == number_class and object.info.support >= Params().min_valid_support]
if len(banners) == 0:
raise NotEnoughDataException('number not found: {}'.format(number))
self.filter_conflicting_banners([object for object in model.objects
if object.info.class_id.startswith("number_banner_")])
banners.sort(key=lambda object: -object.info.support)
return banners
def sample_approach_pose(self, pose, frame_id):
stamped = PoseStamped()
stamped.header.frame_id = frame_id
stamped.pose = pose
return self.sample_approach_stamped(stamped)
def sample_approach_stamped(self, stamped):
center = self.estimate_center_from_map()
if stamped.header.frame_id != center.header.frame_id:
approach = self.tools.transform_pose(center.header.frame_id, stamped)
else:
approach = copy.deepcopy(stamped)
if self.tools.xy_distance(center, stamped) < Params().max_loading_station_distance_from_center:
yaw = self.tools.get_yaw(stamped.pose)
projected = self.tools.set_orientation_from_yaw(stamped.pose, yaw)
projected.position.z = 0
offset = self.tools.set_orientation_from_yaw(Pose(), yaw)
offset.position.x = Params().approach_distance
approach.pose = self.tools.set_orientation_from_yaw(self.tools.add_poses(projected, offset), yaw+math.pi)
else:
yaw = self.tools.get_yaw(stamped.pose)
try:
yaw = self.get_direction_from_closest_wall(stamped.pose.position)
except NotEnoughDataException:
pass
projected = self.tools.set_orientation_from_yaw(stamped.pose, yaw)
projected.position.z = 0
offset = self.tools.set_orientation_from_yaw(Pose(), yaw)
offset.position.x = Params().approach_distance
approach.pose = self.tools.set_orientation_from_yaw(self.tools.add_poses(projected, offset), yaw+math.pi)
return approach
def get_direction_from_closest_wall(self, point):
closest_wall = self.get_closest_wall(point)
return self.tools.get_yaw(closest_wall.pose.pose)
def sample_scan_pose(self):
rospy.loginfo('sampling scan pose')
center = self.estimate_center_from_map()
current = self.tools.get_current_pose()
distance = 0.0
scan_distance = Params().optimal_exploration_distance
reachable = False
sampled_poses = []
for i in range(25):
map_yaw = self.tools.rnd.uniform(-math.pi, math.pi)
center_distance = self.tools.rnd.uniform(Params().min_travel_distance_for_rescan, scan_distance)
scan = copy.deepcopy(current)
scan.pose.position.x += center_distance * math.cos(map_yaw)
scan.pose.position.y += center_distance * math.sin(map_yaw)
rotation = tf.transformations.quaternion_from_euler(0, 0, map_yaw + self.tools.rnd.uniform(-math.pi, math.pi)/2.0)
scan.pose.orientation.x = rotation[0]
scan.pose.orientation.y = rotation[1]
scan.pose.orientation.z = rotation[2]
scan.pose.orientation.w = rotation[3]
distance = self.tools.xy_distance(current, scan)
if self.check_path(current, scan):
return scan
sampled_poses.append(scan)
self.tools.visualize_poses(sampled_poses, ns='failed scan poses')
raise NotEnoughDataException('could not sample exploration pose.')
def enable_barcode_detection(self):
if not self.detection_enabled:
rospy.loginfo('enable barcode detection')
self.detection_enabled = True
self.barcode_enable_service.call()
def disable_barcode_detection(self):
if self.detection_enabled:
rospy.loginfo('disable barcode detection')
self.detection_enabled = False
self.barcode_disable_service.call()
def worldmodel_cb(self, msg):
self.model = msg
def map_cb(self, msg):
self.map = msg
self.map_center_need_update = True
def cube_sensor_cb(self, msg):
self.cube = msg
if not self.cube.data and self.cube_number != -1:
rospy.loginfo('clear cube number')
self.cube_number = -1
if self.cube.data and self.cube_number == -1:
self.enable_barcode_detection()
def barcode_cb(self, msg):
self.barcode = msg
if self.barcode.data != -1 and self.cube.data:
self.cube_number = self.barcode.data
rospy.loginfo('new cube number: {}'.format(self.cube_number))
self.disable_barcode_detection()
class RandomMoveStrategy(object):
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
EstopGuard.add_callback(self.estop_changed_cb)
# align camera
self.actions.look_to(self.look_done_cb)
def decide(self, event):
if not self.decision_required:
return
try:
Params.update()
self.percepts.estimate_center_from_map()
#self.percepts.estimate_center_from_worldmodel()
# scan
if not self.previous_scan_pose:
rospy.loginfo('initial sweeping...')
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
current_pose = self.tools.get_current_pose()
if self.tools.xy_distance(self.previous_scan_pose, current_pose) > Params().min_travel_distance_for_rescan:
rospy.loginfo('sweeping...')
self.actions.stop()
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
# explore
rospy.loginfo('exploring...')
exploration_pose = self.percepts.sample_scan_pose()
self.actions.move_to(exploration_pose, self.move_done_cb, self.move_timeout_cb)
self.decision_required = False
except Exception as e:
rospy.logwarn('exception {}: {}'.format(type(e), e.message))
rospy.sleep(1.0)
def sweep_done_cb(self, status, result):
rospy.loginfo('camera_sweep: done')
self.previous_scan_pose = self.tools.get_current_pose()
self.actions.look_to(self.look_done_cb)
self.decision_required = True
def look_done_cb(self, status, result):
rospy.loginfo('camera_look: done')
self.decision_required = True
def move_done_cb(self, status, result):
rospy.loginfo('random_move: done')
self.actions.cancel_move_timeout()
self.decision_required = True
def move_timeout_cb(self, event):
rospy.loginfo('random_move: timeout')
self.decision_required = True
def estop_changed_cb(self, stop):
if stop:
rospy.loginfo('estop triggered.')
self.decision_required = False
rospy.Rate(4).sleep() # let current decisions finish...
self.actions.cancel_all_actions() # ... and then cancel them.
else:
rospy.loginfo('estop released.')
self.decision_required = True
class DeliverCubesStrategy(object):
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
self.at_approach = False
self.at_ring = False
self.loading_cube = False
self.unloading_cube = False
self.cube_operation_failure = False
self.number_approach_failure = False
self.standstill_failure = False
self.scan_scheduled = False
self.camera_aligned = False
self.cube_status_string = ''
self.status_string = ''
EstopGuard.add_callback(self.estop_changed_cb)
def print_state(self):
rospy.loginfo('***')
if self.cube_operation_failure:
self.cube_status_string = 'recovery'
elif self.percepts.cube_loaded():
self.cube_status_string = 'cube: {}'.format(self.percepts.current_number())
else:
self.cube_status_string = 'no cube'
rospy.loginfo(self.cube_status_string)
rospy.loginfo(self.status_string)
def decide(self, event):
try:
self.print_state()
try:
self.percepts.visualize_worldmodel('{}\n{}'.format(self.cube_status_string, self.status_string))
except NotEnoughDataException:
pass
if not self.decision_required:
return
Params().update()
self.percepts.estimate_center_from_map()
# cube operation failure recovery
if self.cube_operation_failure:
if self.at_ring:
self.retreat()
return
else:
self.cube_operation_failure = False
self.previous_scan_pose = None
self.explore()
return
# number operation failure recovery
if self.number_approach_failure:
self.number_approach_failure = False
self.previous_scan_pose = self.tools.get_current_pose()
self.explore()
return
# no move failure recovery
if self.standstill_failure:
self.standstill_failure = False
self.previous_scan_pose = self.tools.get_current_pose()
self.explore()
return
# cube operations
if self.at_ring:
if self.loading_cube:
self.load_cube()
return
elif self.unloading_cube:
self.unload_cube()
return
else:
# cube operation complete
self.retreat()
return
if not self.camera_aligned:
self.actions.look_to(self.look_ahead_done_cb)
self.actions.start_standstill_timer(self.standstill_timeout_cb)
# drive and camera operations
if not self.percepts.cube_loaded():
try:
# approach loading station
if Params().hack_octa_loading_station:
self.approach_average_of_two_loading_station()
else:
self.approach_loading_station()
return
except NotEnoughDataException as e:
# not enough data to find loading station
self.explore()
return
else:
try:
# approach number
self.approach_number()
return
except NotEnoughDataException as e:
# not enough data to find number
self.explore()
return
except NotEnoughDataException as e:
rospy.logwarn('exception {}: {}'.format(type(e), e.message))
rospy.sleep(1.0)
except Exception as e:
rospy.logerr(traceback.format_exc())
def load_cube(self):
self.actions.enable_LEDs()
if not self.percepts.cube_loaded():
self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.status_string = 'waiting for cube...'
rospy.loginfo(self.status_string)
return # wait
number = self.percepts.current_number()
self.status_string = 'cube received, number {}.'.format(number)
rospy.loginfo(self.status_string)
self.loading_cube = False
self.actions.cancel_cube_timeout()
def unload_cube(self):
self.actions.enable_LEDs()
if self.percepts.cube_loaded():
self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.status_string = 'waiting for cube removal...'
rospy.loginfo(self.status_string)
return # wait
self.status_string = 'cube removed.'
rospy.loginfo(self.status_string)
self.unloading_cube = False
self.actions.cancel_cube_timeout()
def approach_loading_station(self):
current = self.tools.get_current_pose()
loading_station = self.percepts.get_best_loading_station(current)
if self.tools.is_good_approach_pose(current, loading_station):
self.status_string = 'approaching loading station...'
rospy.loginfo(self.status_string)
stamped = PoseStamped()
stamped.header = loading_station.header
stamped.pose = self.tools.project_pose(loading_station.pose.pose)
stamped.pose.position.z = -0.01
self.actions.approach(stamped, self.approach_loading_station_done_cb, self.approach_timeout_cb)
else:
self.status_string = 'moving to loading station...'
rospy.loginfo(self.status_string)
approach = self.percepts.sample_approach_pose(loading_station.pose.pose, loading_station.header.frame_id)
self.actions.move_to(approach, self.preapproach_done_cb, self.move_timeout_cb)
self.decision_required = False
def approach_average_of_two_loading_station(self):
current = self.tools.get_current_pose()
approach = self.percepts.get_best_average_approach_from_two_line_markers(current)
if self.tools.xy_distance(current, approach) < 0.1:
self.status_string = 'approaching loading station...'
rospy.loginfo(self.status_string)
stamped = PoseStamped(header=approach.header)
offset = Pose()
offset.position.x = Params().approach_distance
offset.orientation.w = 1
stamped.pose = self.tools.add_poses(approach.pose, offset)
stamped.pose.position.z = -0.01
self.actions.approach(stamped, self.approach_loading_station_done_cb, self.approach_timeout_cb)
else:
self.status_string = 'moving to loading station...'
rospy.loginfo(self.status_string)
self.actions.move_to(approach, self.preapproach_done_cb, self.move_timeout_cb)
self.decision_required = False
def approach_number(self):
number = self.percepts.current_number()
banner = self.percepts.get_number_banner(number)
current = self.tools.get_current_pose()
if banner.info.support < Params().min_trusted_support:
# go to some good pose
# look at the number for x seconds
if self.tools.is_good_verification_pose(current, banner):
# look at number
self.status_string = 'looking at {}'.format(number)
self.actions.look_at(PoseStamped(header=banner.header, pose = banner.pose.pose), done_cb=self.verification_lookat_done_cb)
else:
for i in range(20):
stamped = self.tools.sample_verification_pose(banner, self.percepts.estimate_center_from_map())
sampled_poses = []
if self.percepts.check_path(current, stamped):
self.status_string = 'moving to look at {}'.format(number)
rospy.loginfo(self.status_string)
self.actions.move_to(stamped, done_cb=self.move_to_verify_done_cb, timeout_cb=self.move_timeout_cb)
self.decision_required = False
return
sampled_poses.append(stamped)
self.tools.visualize_poses(sampled_poses, ns='failed_verification_poses')
self.status_string = 'failure at {}'.format(number)
self.number_approach_failure = True
elif self.tools.is_good_approach_pose(current, banner):
self.status_string = 'approaching number {}...'.format(number)
rospy.loginfo(self.status_string)
stamped = PoseStamped()
stamped.header = banner.header
stamped.pose = self.tools.project_pose(banner.pose.pose)
#stamped.pose = self.tools.set_orientation_from_yaw(banner.pose.pose, self.tools.get_yaw(banner.pose.pose) + math.pi)
stamped.pose.position.z = 0.01
self.actions.approach(stamped, self.approach_number_done_cb, self.approach_timeout_cb)
else:
self.status_string = 'moving to number {}...'.format(number)
rospy.loginfo(self.status_string)
approach = self.percepts.sample_approach_pose(banner.pose.pose, banner.header.frame_id)
self.actions.move_to(approach, self.preapproach_done_cb, self.move_timeout_cb)
self.decision_required = False
def retreat(self):
self.status_string = 'retreating...'
rospy.loginfo(self.status_string)
self.actions.disable_LEDs()
self.actions.retreat(self.retreat_done_cb, self.retreat_timeout_cb)
self.decision_required = False
def explore(self):
# scan
if not self.previous_scan_pose:
self.previous_scan_pose = self.tools.get_current_pose()
current_pose = self.tools.get_current_pose()
if self.tools.xy_distance(self.previous_scan_pose, current_pose) > Params().min_travel_distance_for_rescan:
self.status_string = 'sweeping...'
rospy.loginfo(self.status_string)
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
self.status_string = 'exploring...'
rospy.loginfo(self.status_string)
exploration_pose = self.percepts.sample_scan_pose()
self.actions.move_to(exploration_pose, self.move_to_scan_done_cb, self.move_to_scan_timeout_cb)
self.decision_required = False
def sweep_done_cb(self, status, result):
self.status_string = 'camera_sweep: done'
rospy.loginfo(self.status_string)
self.previous_scan_pose = self.tools.get_current_pose()
self.camera_aligned = False
self.decision_required = True
def look_ahead_done_cb(self, status, result):
self.status_string = 'camera_look_ahead: done'
rospy.loginfo(self.status_string)
self.camera_aligned = True
self.decision_required = True
def approach_loading_station_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'approach_loading_station: done'
rospy.loginfo(self.status_string)
self.loading_cube = True
self.actions.enable_LEDs()
#self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.at_ring = True
self.decision_required = True
def approach_number_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'approach_number: done'
rospy.loginfo(self.status_string)
self.unloading_cube = True
#self.actions.start_cube_operation_timer(self.cube_operation_timeout_cb)
self.at_ring = True
self.decision_required = True
def retreat_done_cb(self, status, result):
self.status_string = 'retreat: done'
rospy.loginfo(self.status_string)
self.at_ring = False
self.decision_required = True
def preapproach_done_cb(self, status, result):
if status == GoalStatus.SUCCEEDED:
self.status_string = 'pre-approach: done'.format(status, result)
rospy.loginfo(self.status_string)
self.decision_required = True
def move_to_scan_done_cb(self, status, result):
self.status_string = 'move to scan: done'
rospy.loginfo(self.status_string)
self.scan_scheduled = True
self.decision_required = True
def move_to_scan_timeout_cb(self, event):
self.status_string = 'move to scan: timeout'
rospy.loginfo(self.status_string)
self.decision_required = True
def move_to_verify_done_cb(self, status, result):
self.status_string = 'move to verify: done'
rospy.loginfo(self.status_string)
self.decision_required = True
def move_timeout_cb(self, event):
self.status_string = 'move to verify: timeout'
rospy.loginfo(self.status_string)
self.decision_required = True
def approach_timeout_cb(self, event):
self.status_string = 'approach: timeout'
rospy.loginfo(self.status_string)
self.at_ring = True
self.decision_required = True
def retreat_timeout_cb(self, event):
self.status_string = 'retreat: timeout'
rospy.loginfo(self.status_string)
# TODO: do something here... retreat?
self.decision_required = True
def cube_operation_timeout_cb(self, event):
self.status_string = 'cube_operation: timeout'
rospy.loginfo(self.status_string)
self.cube_operation_failure = True
self.decision_required = True
def verification_lookat_done_cb(self, status, result):
self.actions.start_verification_timer(self.verification_timeout_cb)
def verification_timeout_cb(self, event):
self.status_string = 'verification: done'
rospy.loginfo(self.status_string)
self.decision_required = True
def standstill_timeout_cb(self, event):
self.status_string = 'no_move: timeout'
rospy.loginfo(self.status_string)
self.standstill_failure = True
self.decision_required = True
def estop_changed_cb(self, stop):
if stop:
self.status_string = 'estop triggered.'
rospy.loginfo(self.status_string)
self.decision_required = False
rospy.Rate(4).sleep() # let current decisions finish...
self.actions.cancel_all_actions() # ... and then cancel them.
self.actions.cancel_standstill_timer()
else:
self.status_string = 'estop released.'
rospy.loginfo(self.status_string)
self.decision_required = True
class RandomMoveStrategy(object):
def __init__(self):
self.actions = Actions()
self.percepts = Percepts()
self.tools = Tools()
self.previous_scan_pose = None
self.decision_required = True
EstopGuard.add_callback(self.estop_changed_cb)
# align camera
self.actions.look_to(self.look_done_cb)
def decide(self, event):
if not self.decision_required:
return
try:
Params.update()
self.percepts.estimate_center_from_map()
#self.percepts.estimate_center_from_worldmodel()
# scan
if not self.previous_scan_pose:
rospy.loginfo('initial sweeping...')
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
current_pose = self.tools.get_current_pose()
if self.tools.xy_distance(
self.previous_scan_pose,
current_pose) > Params().min_travel_distance_for_rescan:
rospy.loginfo('sweeping...')
self.actions.stop()
self.actions.camera_sweep(self.sweep_done_cb)
self.decision_required = False
return
# explore
rospy.loginfo('exploring...')
exploration_pose = self.percepts.sample_scan_pose()
self.actions.move_to(exploration_pose, self.move_done_cb,
self.move_timeout_cb)
self.decision_required = False
except Exception as e:
rospy.logwarn('exception {}: {}'.format(type(e), e.message))
rospy.sleep(1.0)
def sweep_done_cb(self, status, result):
rospy.loginfo('camera_sweep: done')
self.previous_scan_pose = self.tools.get_current_pose()
self.actions.look_to(self.look_done_cb)
self.decision_required = True
def look_done_cb(self, status, result):
rospy.loginfo('camera_look: done')
self.decision_required = True
def move_done_cb(self, status, result):
rospy.loginfo('random_move: done')
self.actions.cancel_move_timeout()
self.decision_required = True
def move_timeout_cb(self, event):
rospy.loginfo('random_move: timeout')
self.decision_required = True
def estop_changed_cb(self, stop):
if stop:
rospy.loginfo('estop triggered.')
self.decision_required = False
rospy.Rate(4).sleep() # let current decisions finish...
self.actions.cancel_all_actions() # ... and then cancel them.
else:
rospy.loginfo('estop released.')
self.decision_required = True
