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 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 sample_verification_pose(self, banner, center): banner_pose = self.project_pose(banner.pose.pose) distance = self.rnd.uniform(Params().min_verification_distance, Params().max_verification_distance) angle = self.rnd.uniform(-Params().max_verification_angle, Params().max_verification_angle) offset = Pose() offset.position.x = distance * math.cos(angle) offset.position.y = distance * math.sin(angle) offset.orientation.w = 1 offset = self.set_orientation_from_yaw(offset, angle-math.pi) verification = PoseStamped(header=center.header) verification.pose = self.add_poses(banner_pose, offset) return verification
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 is_good_approach_pose(self, stamped, banner): robot_frame = pm.fromMsg(stamped.pose) banner_frame = pm.fromMsg(banner.pose.pose) view_ray = banner_frame.p - robot_frame.p distance = view_ray.Norm() if distance > Params().approach_distance_tolerance: rospy.loginfo('bad approach pose: distance {}'.format(distance)) return False angle = self._get_viewing_angle(robot_frame, banner_frame) if abs(angle) > Params().max_approach_angle: rospy.loginfo('bad approach pose: angle {}'.format(math.degrees(angle))) return False return True
def is_good_verification_pose(self, stamped, banner): robot_frame = pm.fromMsg(stamped.pose) banner_frame = pm.fromMsg(banner.pose.pose) view_ray = banner_frame.p - robot_frame.p distance = view_ray.Norm() if distance < Params().min_verification_distance or distance > Params().max_verification_distance*1.5: rospy.loginfo('bad verification pose: distance {}'.format(distance)) return False # angle = self._get_facing_angle(robot_frame, banner_frame) # if abs(angle) > Params().max_verification_angle: # rospy.loginfo('bad verification pose: angle {}'.format(math.degrees(angle))) # return False return True
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 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 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 add_callback(estop_changed_cb): if not EstopGuard._observers: EstopGuard._observers = [] EstopGuard._instances = [] for topic in Params().estop_topics: EstopGuard._instances.append(EstopGuard(topic)) rospy.loginfo('estop guard initialized.') EstopGuard._observers.append(estop_changed_cb)
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 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 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 sample_approach_from_averaged(self, line1, line2): line1_frame = pm.fromMsg(line1) line2_frame = pm.fromMsg(line2) averaged_frame = PyKDL.Frame() averaged_frame.p = (line1_frame.p + line2_frame.p) * 0.5 [roll, pitch, yaw1] = line1_frame.M.GetRPY() [roll, pitch, yaw2] = line2_frame.M.GetRPY() averaged_frame.M = PyKDL.Rotation.RPY(0, 0, (yaw1+yaw2)/2.0) approach_frame = PyKDL.Frame() approach_frame.p = averaged_frame.p + averaged_frame.M * PyKDL.Vector(Params().approach_distance * 1.5, 0, 0) [roll, pitch, yaw] = averaged_frame.M.GetRPY() approach_frame.M = PyKDL.Rotation.RPY(0, 0, yaw+math.pi) return pm.toMsg(approach_frame)
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 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 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 ageing_cb(self, event): if not self.stop: self.worldmodel_ageing_publisher.publish( Float32(data=Params().worldmodel_ageing_rate))
def start_verification_timer(self, timeout_cb): self.verification_timer = rospy.Timer(rospy.Duration( Params().verification_duration), timeout_cb, oneshot=True)
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())