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'