def sample_detection_search(self, sample):
#don't bother with this unless a latched filter id is active,
#and it matches the one we have been tasked with pursuing
latched_filter_id = self.state_machine.userdata.latched_filter_id
if (latched_filter_id is not None):
if (latched_filter_id == sample.filter_id):
point, pose = calculate_pursuit(self.tf_listener,
sample,
self.min_pursuit_distance,
self.odometry_frame)
self.state_machine.userdata.target_sample = point
#if the pursuit_goal is None, we are not actively in pursuit
if self.state_machine.userdata.pursuit_goal is not None:
#if the desired pursuit pose changes too much, update the state_machine's pose
pursuit_error = util.pose_distance_2d(self.state_machine.userdata.pursuit_goal.target_pose,
pose)
if (pursuit_error > self.max_pursuit_error):
self.state_machine.pursuit_pose = pose
goal = samplereturn_msg.VFHMoveGoal(target_pose = pose,
move_velocity = self.state_machine.userdata.pursuit_velocity,
spin_velocity = self.state_machine.userdata.spin_velocity,
orient_at_target = True)
self.state_machine.userdata.pursuit_goal = goal
elif not self.state_machine.userdata.filter_changed:
self.state_machine.userdata.filter_changed = True
self.announcer.say('New filter eye dee.')
def execute(self, userdata):
self.last_line_blocked = False
self.announcer.say("On search line, Yaw " + str(int(math.degrees(userdata.line_yaw))))
while not rospy.is_shutdown():
current_pose = util.get_current_robot_pose(self.listener)
distance = util.pose_distance_2d(current_pose, userdata.next_line_pose)
if distance < userdata.line_replan_distance:
self.announcer.say("Line is clear, continue ing")
new_pose = util.pose_translate_by_yaw(userdata.next_line_pose,
userdata.line_plan_step,
userdata.line_yaw)
userdata.next_line_pose = new_pose
if rospy.Time.now() > userdata.return_time:
self.announcer.say("Search time expired")
return 'return_home'
if self.new_costmap_available:
self.new_costmap_available = False
if self.line_blocked(userdata):
self.announcer.say("Line is blocked, rotate ing to new line")
return 'line_blocked'
#check preempt after deciding whether or not to calculate next line pose
if self.preempt_requested():
rospy.loginfo("PREEMPT REQUESTED IN LINE MANAGER")
self.service_preempt()
return 'preempted'
rospy.sleep(0.2)
return 'aborted'
