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