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.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.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