def execute(self, userdata):
if userdata.search_count >= userdata.search_try_limit:
self.announcer.say("Search limit reached")
return 'no_sample'
if not userdata.lighting_optimized:
userdata.lighting_optimized = True
return 'optimize_lighting'
point_list = collections.deque([])
try:
for position in userdata.spiral_search_positions:
start_pose = util.get_current_robot_pose(self.tf_listener,
userdata.odometry_frame)
next_pose = util.translate_base_link(self.tf_listener,
start_pose,
position['x'],
position['y'],
userdata.odometry_frame)
point_list.append(next_pose.pose.position)
userdata.point_list = point_list
self.announcer.say("Search ing area")
userdata.search_count += 1
return 'sample_search'
except tf.Exception, e:
rospy.logwarn("PURSUE_SAMPLE failed to transform %s-%s in GetSearchMoves: %s",
'base_link', userdata.odometry_frame, e)
return 'aborted'
def publish_path_markers(self, event):
try:
self.path_marker.pose = util.get_current_robot_pose(self.tf_listener, self.reality_frame).pose
except:
return
self.path_marker.id = self.path_counter
self.path_counter += 1
self.path_marker_pub.publish(self.path_marker)
def yaw_error(self):
"""
Compute difference between present orientation and
goal orientation.
"""
current_pose = util.get_current_robot_pose(self._tf,
self.odometry_frame)
raw_error = euler_from_orientation(self._goal_odom.pose.orientation)[-1] - \
euler_from_orientation(current_pose.pose.orientation)[-1]
return util.unwind(raw_error)
def position_error(self):
"""
Compute difference between present position and
goal position.
"""
current_pose = util.get_current_robot_pose(self._tf,
self.odometry_frame)
return Point(
self._goal_odom.pose.position.x -
current_pose.pose.position.x,
self._goal_odom.pose.position.y -
current_pose.pose.position.y,
0)
def publish_point_cloud(self, event):
#rospy.loginfo("Starting PC generation")
now = event.current_real
#get latest available transforms to map
header = std_msg.Header(0, rospy.Time(0), self.reality_frame)
target_frame = 'navigation_center_left_camera'
try:
current_pose = util.get_current_robot_pose(self.tf_listener,
self.reality_frame)
transform = self.tf_listener.asMatrix(target_frame, header)
except ( tf.Exception ), e:
#rospy.loginfo("Failed to transform map for PC")
#rospy.loginfo("Exception: %s"%e)
return
def calculate_pursuit(_tf, pursuit_point, min_pursuit_distance, odometry_frame):
try:
current_pose = util.get_current_robot_pose(_tf, odometry_frame)
_tf.waitForTransform(odometry_frame,
pursuit_point.header.frame_id,
pursuit_point.header.stamp,
rospy.Duration(1.0))
point_in_frame = _tf.transformPoint(odometry_frame, pursuit_point)
pointing_quat = util.pointing_quaternion_2d(current_pose.pose.position, point_in_frame.point)
distance_to_point = util.point_distance_2d(current_pose.pose.position, point_in_frame.point)
pursuit_pose = util.pose_translate_by_quat(current_pose,
(distance_to_point - min_pursuit_distance),
pointing_quat)
#recalculate the quaternion pointing from goal point to sample point
pointing_quat = util.pointing_quaternion_2d(pursuit_pose.pose.position, point_in_frame.point)
pursuit_pose.pose.orientation = pointing_quat
pursuit_pose.header.stamp = rospy.Time(0)
#rospy.loginfo("CALCULATE PURSUIT: pursuit_point: {!s}, point_in_frame: {!s}, pursuit_pose: {!s}".format(pursuit_point, point_in_frame, pursuit_pose))
return point_in_frame, pursuit_pose
except tf.Exception, e:
rospy.logwarn("PURSUE_SAMPLE failed to transform pursuit point %s->%s: %s",
pursuit_point.header.frame_id, odometry_frame, e)
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'
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
#ignore samples after this (applies to mount moves)
#clear previous move_point_map
userdata.stop_on_sample = False
userdata.move_point_map = None
map_header = std_msg.Header(0, rospy.Time(0), userdata.world_fixed_frame)
#get our position in map
current_pose = util.get_current_robot_pose(self.tf_listener,
userdata.world_fixed_frame)
#this is possibly a highly inaccurate number. If we get to the approach point,
#and don't see the beacon, the map is probably messed up
distance_to_approach_point = util.point_distance_2d(current_pose.pose.position,
userdata.beacon_approach_pose.pose.position)
#if we have been ignoring beacon detections prior to this,
#we should clear them here, and wait for a fresh detection
if not userdata.stop_on_beacon:
userdata.beacon_point = None
rospy.sleep(4.0)
if userdata.beacon_point is None: #beacon not in view
#first hope, we can spin slowly and see the beacon
if not self.tried_spin:
self.announcer.say("Beacon not in view. Rotate ing")
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
angle = math.pi*2,
velocity = userdata.spin_velocity)
userdata.stop_on_beacon = True
self.tried_spin = True
return 'spin'
#already tried a spin, drive towards beacon_approach_point, stopping on detection
elif distance_to_approach_point > 5.0:
#we think we're far from approach_point, so try to go there
self.announcer.say("Beacon not in view. Search ing")
userdata.move_point_map = geometry_msg.PointStamped(map_header,
userdata.beacon_approach_pose.pose.position)
goal = samplereturn_msg.VFHMoveGoal(target_pose = userdata.beacon_approach_pose,
move_velocity = userdata.move_velocity,
spin_velocity = userdata.spin_velocity)
userdata.move_goal = goal
userdata.stop_on_beacon = True
self.tried_spin = False
return 'move'
else:
#gotta add some heroic shit here
#we think we're near the beacon, but we don't see it
#right now, that is just to move 30 m on other side of the beacon that we don't know about
self.announcer.say("Close to approach point in map. Beacon not in view. Search ing")
search_pose = deepcopy(userdata.beacon_approach_pose)
#invert the approach_point, and try again
search_pose.pose.position.x *= -1
#save point
userdata.move_point_map = geometry_msg.PointStamped(map_header,
search_pose.pose.position)
goal = samplereturn_msg.VFHMoveGoal(target_pose = userdata.search_pose,
move_velocity = userdata.move_velocity,
spin_velocity = userdata.spin_velocity)
userdata.move_goal = goal
userdata.stop_on_beacon = True
self.tried_spin = False
return 'move'
else: #beacon is in view
#need to add some stuff here to get to other side of beacon if viewing back
current_yaw = util.get_current_robot_yaw(self.tf_listener,
userdata.world_fixed_frame)
yaw_to_platform = util.pointing_yaw(current_pose.pose.position,
userdata.platform_point.point)
yaw_error = util.unwind(yaw_to_platform - current_yaw)
userdata.stop_on_beacon = False
self.tried_spin = False
#on the back side
if current_pose.pose.position.x < 0:
front_pose = deepcopy(userdata.beacon_approach_pose)
#try not to drive through the platform
front_pose.pose.position.y = 5.0 * np.sign(current_pose.pose.position.y)
goal = samplereturn_msg.VFHMoveGoal(target_pose = front_pose,
move_velocity = userdata.move_velocity,
spin_velocity = userdata.spin_velocity)
userdata.move_goal = goal
self.announcer.say("Back of beacon in view. Move ing to front")
return 'move'
elif distance_to_approach_point > 2.0:
#on correct side of beacon, but far from approach point
goal = samplereturn_msg.VFHMoveGoal(target_pose = userdata.beacon_approach_pose,
move_velocity = userdata.move_velocity,
spin_velocity = userdata.spin_velocity,
orient_at_target = True)
userdata.move_goal = goal
self.announcer.say("Beacon in view. Move ing to approach point")
return 'move'
elif np.abs(yaw_error) > .2:
#this means beacon is in view and we are within 1 meter of approach point
#but not pointed so well at beacon
self.announcer.say("At approach point. Rotate ing to beacon")
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
angle = yaw_error,
velocity = userdata.spin_velocity)
return 'spin'
else:
self.announcer.say("Initiate ing platform mount")
userdata.stop_on_beacon = False
return 'mount'
return 'aborted'
def execute(self, userdata):
#set the move manager key for the move mux
userdata.active_manager = userdata.manager_dict[self.label]
userdata.report_sample = False
userdata.report_beacon = True
self.beacon_enable.publish(True)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
#get our position in map
current_pose = util.get_current_robot_pose(self.tf_listener,
userdata.platform_frame)
#hopeful distance to approach point
distance_to_approach_point = util.point_distance_2d(current_pose.pose.position,
userdata.beacon_approach_pose.pose.position)
#if we have been ignoring beacon detections prior to this,
#we should clear them here, and wait for a fresh detection
if not userdata.stop_on_detection:
self.announcer.say("Wait ing for beacon.")
userdata.beacon_point = None
start_time = rospy.Time.now()
while not rospy.core.is_shutdown_requested():
rospy.sleep(0.5)
if (userdata.beacon_point is not None) \
or ((rospy.Time.now() - start_time) > userdata.beacon_observation_delay):
break
if userdata.beacon_point is None: #beacon not in view
#first hope, we can spin slowly and see the beacon
if not self.tried_spin:
self.announcer.say("Beacon not in view. Rotate ing")
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
angle = np.pi*2,
velocity = userdata.spin_velocity)
userdata.stop_on_detection = True
self.tried_spin = True
return 'spin'
#already tried a spin, drive towards beacon_approach_point, stopping on detection
elif distance_to_approach_point > 5.0:
#we think we're far from approach_point, so try to go there
self.announcer.say("Beacon not in view. Moving to approach point.")
userdata.stop_on_detection = True
self.tried_spin = False
userdata.move_target = userdata.beacon_approach_pose
return 'move'
else:
#we think we're looking at the beacon, but we don't see it, this is probably real bad
self.announcer.say("Close to approach point in map. Beacon not in view. Search ing")
search_pose = deepcopy(current_pose)
#try a random position nearby-ish, ignore facing
search_pose.pose.position.x += random.randrange(-30, 30, 10)
search_pose.pose.position.y += random.randrange(-30, 30, 10)
userdata.stop_on_detection = True
self.tried_spin = False
userdata.move_target = geometry_msg.PointStamped(search_pose.header,
search_pose.pose.position)
return 'move'
else: #beacon is in view
current_yaw = util.get_current_robot_yaw(self.tf_listener,
userdata.platform_frame)
yaw_to_platform = util.pointing_yaw(current_pose.pose.position,
userdata.platform_point.point)
yaw_error = util.unwind(yaw_to_platform - current_yaw)
userdata.stop_on_detection = False
self.tried_spin = False
#on the back side
if current_pose.pose.position.x < 0:
#try not to drive through the platform
self.announcer.say("Back of beacon in view. Move ing to front")
front_pose = deepcopy(userdata.beacon_approach_pose)
front_pose.pose.position.y = 5.0 * np.sign(current_pose.pose.position.y)
pointing_quat = util.pointing_quaternion_2d(front_pose.pose.position,
userdata.platform_point.point)
front_pose.pose.orientation = pointing_quat
userdata.move_target = front_pose
return 'move'
elif distance_to_approach_point > 2.0:
#on correct side of beacon, but far from approach point
self.announcer.say("Beacon in view. Move ing to approach point")
userdata.move_target = userdata.beacon_approach_pose
return 'move'
elif np.abs(yaw_error) > .2:
#this means beacon is in view and we are within 1 meter of approach point
#but not pointed so well at beacon
self.announcer.say("At approach point. Rotate ing to beacon")
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
angle = yaw_error,
velocity = userdata.spin_velocity)
return 'spin'
else:
self.announcer.say("Measure ing beacon position.")
userdata.stop_on_detection = False
return 'mount'
return 'aborted'
def execute(self, userdata):
result = samplereturn_msg.GeneralExecutiveResult()
result.result_string = 'approach failed'
result.result_int = samplereturn_msg.NamedPoint.NONE
userdata.action_result = result
#default velocity for all moves is in simple_mover!
#initial approach pursuit done at pursuit_velocity
userdata.lighting_optimized = True
userdata.search_count = 0
userdata.grab_count = 0
rospy.loginfo("SAMPLE_PURSUIT input_point: %s" % (userdata.action_goal.input_point))
#store the filter_id on entry, this instance of pursuit will only try to pick up this hypothesis
userdata.latched_filter_id = userdata.action_goal.input_point.filter_id
userdata.filter_changed = False
point, pose = calculate_pursuit(self.tf_listener,
userdata.action_goal.input_point,
userdata.min_pursuit_distance,
userdata.odometry_frame)
try:
approach_point = geometry_msg.PointStamped(pose.header,
pose.pose.position)
yaw, distance = util.get_robot_strafe(self.tf_listener, approach_point)
robot_yaw = util.get_current_robot_yaw(self.tf_listener, userdata.odometry_frame)
except tf.Exception:
rospy.logwarn("PURSUE_SAMPLE failed to get base_link -> %s transform in 1.0 seconds", sample_frame)
return 'aborted'
rospy.loginfo("INITIAL_PURSUIT calculated with distance: {:f}, yaw: {:f} ".format(distance, yaw))
#this is the initial vfh goal pose
goal = samplereturn_msg.VFHMoveGoal(target_pose = pose,
move_velocity = userdata.pursuit_velocity,
spin_velocity = userdata.spin_velocity,
orient_at_target = True)
userdata.pursuit_goal = goal
#create return destination
current_pose = util.get_current_robot_pose(self.tf_listener,
userdata.odometry_frame)
current_pose.header.stamp = rospy.Time(0)
goal = samplereturn_msg.VFHMoveGoal(target_pose = current_pose,
move_velocity = userdata.pursuit_velocity,
spin_velocity = userdata.spin_velocity,
orient_at_target = False)
userdata.return_goal = goal
#if distance to sample approach point is small, use a simple move
if distance < userdata.simple_pursuit_threshold:
facing_yaw = euler_from_orientation(pose.pose.orientation)[-1]
dyaw = util.unwind(facing_yaw - robot_yaw)
userdata.approach_strafe = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE,
angle = yaw,
distance = distance,
velocity = userdata.pursuit_velocity)
userdata.face_sample = SimpleMoveGoal(type=SimpleMoveGoal.SPIN,
angle = dyaw,
velocity = userdata.spin_velocity)
return 'simple'
#if further than threshold make a vfh move to approach point
else:
return 'goal'
def is_stop_requested(self):
return self.stop_requested
#sets the target point and the start point in odom frame
def set_path_points_odom(self, target_stamped):
target_point_odom = None
try:
target_point_odom = self._tf.transformPoint(self.odometry_frame,
target_stamped)
except tf.Exception, exc:
rospy.logwarn("VFH MOVE SERVER:could not transform point: %s"%exc)
self._as.set_aborted(result = VFHMoveResult(outcome = VFHMoveResult.ERROR))
return False
self._target_point_odom = target_point_odom
current_pose = util.get_current_robot_pose(self._tf, self.odometry_frame)
self._start_point = current_pose.pose.position
def execute_movement(self, move_velocity, spin_velocity):
##### Begin rotate to clear section
#if requested rotate until the forward sector is clear, then move
#clear_distance straight ahead, then try to move to the target pose
if self._rotate_to_clear:
start_dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom)
rot_sign = np.sign(start_dyaw)
#first try rotating toward the target 120 degrees or so
error = self._mover.execute_spin(rot_sign*self._clear_first_angle,
max_velocity = spin_velocity,
stop_function=self.clear_check)
if self.exit_check(): return
