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'
def execute(self, userdata):
rospy.sleep(1.0) #wait for robot to settle
current_pose = util.get_current_robot_pose(self.tf_listener)
yaw_quat = tf.transformations.quaternion_from_euler(0, 0, userdata.line_yaw)
#stupid planner may not have the robot oriented along the search line,
#set orientation to that value anyway
current_pose.pose.orientation = geometry_msg.Quaternion(*yaw_quat)
yaw_changes = np.array(range(userdata.blocked_rotation_min,
userdata.blocked_rotation_max,
10))
yaw_changes = np.radians(yaw_changes)
yaw_changes = np.r_[yaw_changes, -yaw_changes]
yaw_change = random.choice(yaw_changes)
line_yaw = userdata.line_yaw + yaw_change
rotate_pose = util.pose_rotate(current_pose, yaw_change)
next_line_pose = util.pose_translate_by_yaw(rotate_pose,
userdata.line_plan_step,
line_yaw)
userdata.line_yaw = line_yaw
userdata.rotate_pose = rotate_pose
userdata.next_line_pose = next_line_pose
return 'next'
