def execute(self, userdata):
#disable obstacle checking!
rospy.sleep(10.0) #wait a sec for beacon pose to catch up
target_point = deepcopy(userdata.platform_point)
target_point.header.stamp = rospy.Time(0)
yaw, distance = util.get_robot_strafe(self.tf_listener,
target_point)
move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE,
angle = yaw,
distance = distance,
velocity = 0.5)
userdata.simple_move = move
self.announcer.say("Execute ing final mount move")
return 'final'
def check_beacon_pose(self, event):
if not self.publish_beacon:
return
if not self.beacon_enabled:
return
try:
#get distances and yaws from reality frame origin
platform_origin = PointStamped(std_msg.Header(0, rospy.Time(0),
'platform'),
Point(0,0,0))
angle_to_platform, dist_from_platform = util.get_robot_strafe(self.tf_listener,
platform_origin)
angle_to_robot = util.get_robot_yaw_from_origin(self.tf_listener,
'platform')
except tf.Exception, exc:
print("Transforms not available in publish beacon: {!s}".format(exc))
return
def execute(self, userdata):
#wait a sec for beacon pose to adjust the localization filter
saved_point_odom = None
#wait for a maximum of one minute, then mount anyway
timeout_time = rospy.Time.now() + rospy.Duration(60.0)
in_tolerance_count = 0
while not rospy.core.is_shutdown_requested():
rospy.sleep(3.0)
try:
platform_point_odom = self.tf_listener.transformPoint(userdata.odometry_frame,
userdata.platform_point)
except tf.Exception:
rospy.logwarn("LEVEL_TWO calculate_mount failed to transform platform point")
if saved_point_odom is None:
saved_point_odom = platform_point_odom
else:
correction_error = util.point_distance_2d(platform_point_odom.point,
saved_point_odom.point)
saved_point_odom = platform_point_odom
if correction_error < userdata.beacon_mount_tolerance:
in_tolerance_count += 1
else:
in_tolerance_count = 0
#if we get 3 in_tolerance corrections, mount
if in_tolerance_count > 2:
break
if rospy.Time.now() > timeout_time:
self.announcer.say("Beacon correction did not converge.")
break
else:
self.announcer.say("Correction. {:.3f}".format(correction_error))
yaw, distance = util.get_robot_strafe(self.tf_listener,
userdata.platform_point)
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE,
angle = yaw,
distance = distance,
velocity = 0.5)
self.announcer.say("Initiate ing mount move.")
return 'move'
def execute(self, userdata):
if (len(userdata.point_list) > 0):
rospy.loginfo("MOVE TO POINTS list: %s" %(userdata.point_list))
target_point = userdata.point_list[0]
header = std_msg.Header(0, rospy.Time(0), userdata.odometry_frame)
target_stamped = geometry_msg.PointStamped(header, target_point)
try:
yaw, distance = util.get_robot_strafe(self.tf_listener, target_stamped)
except(tf.Exception):
rospy.logwarn("MOVE_TO_POINTS failed to transform search point (%s) to base_link in 1.0 seconds", search_point.header.frame_id)
return 'aborted'
userdata.point_list.popleft()
userdata.simple_move = SimpleMoveGoal(type=SimpleMoveGoal.STRAFE,
angle = yaw,
distance = distance,
velocity = userdata.velocity)
return 'next_point'
else:
return 'complete'
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 update_sectors(self, sectors, sector_angle):
#sector_count and zero offset
sector_count = len(sectors)
zero_offset = np.rint(sector_count/2.0)
#get current position from mover object
robot_position = self._mover.current_position
robot_yaw = self._mover.current_yaw
yaw_to_target, distance_to_target = util.get_robot_strafe(self._tf, self._target_point_odom)
target_index = int(round(yaw_to_target/sector_angle) + zero_offset)
#if we are too far off the line between start point, and goal, initiate stop
distance_off_course = util.point_distance_to_line(geometry_msg.Point(*robot_position),
self._start_point,
self._target_point_odom.point)
valid, robot_cmap_coords = self.world2map(robot_position[:2], self.costmap_info)
obstacle_density = np.zeros(len(sectors))
inverse_costs = np.zeros(len(sectors))
#nonzero_coords = np.transpose(np.nonzero(self.costmap))
for index in range(sector_count):
#yaw in odom
sector_yaw = robot_yaw + (index - zero_offset) * sector_angle
#get start and end points for the bresenham line
start = self.bresenham_point(robot_cmap_coords,
self.min_obstacle_distance,
sector_yaw,
self.costmap_info.resolution)
#if we are close to the target, truncate the check distance
end = self.bresenham_point(robot_cmap_coords,
min(self.max_obstacle_distance,
(distance_to_target+self._goal_obstacle_radius)),
sector_yaw,
self.costmap_info.resolution)
sector_line = bresenham.points(start, end)
for coords in sector_line:
cell_value = self.costmap[tuple(coords)]
position = self.costmap_info.resolution * (coords - robot_cmap_coords)
distance = np.linalg.norm(position)
#m is the obstacle vector magnitude
m = cell_value * (self.max_obstacle_distance - distance)
obstacle_density[index] += m
#set sectors above threshold density as blocked, those below as clear, and do
#not change the ones in between
if obstacle_density[index] >= self.threshold_high:
sectors[index] = True
inverse_costs[index] = 0 #inversely infinitely expensive
if obstacle_density[index] <= self.threshold_low:
sectors[index] = False
#if sector is clear (it may not have been changed this time!) set its cost, making it a candidate sector
if not sectors[index]:
inverse_costs[index] = 1.0 / ( self.u_goal*np.abs(yaw_to_target - (index - zero_offset)*sector_angle)
+ self.u_current*np.abs(self.current_sector_index - index)*sector_angle)
#START DEBUG CRAP
if self.publish_debug:
rospy.logdebug("TARGET SECTOR index: %d" % (target_index))
rospy.logdebug("SECTORS: %s" % (sectors))
rospy.logdebug("INVERSE_COSTS: %s" % (inverse_costs))
vfh_debug_array = []
vfh_debug_marker = vis_msg.Marker()
vfh_debug_marker.pose = Pose()
vfh_debug_marker.header = std_msg.Header(0, rospy.Time(0), self.odometry_frame)
vfh_debug_marker.type = vis_msg.Marker.ARROW
vfh_debug_marker.color = std_msg.ColorRGBA(0, 0, 0, 1)
arrow_len = min(self.max_obstacle_distance,
(distance_to_target + self._goal_obstacle_radius))
vfh_debug_marker.scale = geometry_msg.Vector3(arrow_len, .02, .02)
vfh_debug_marker.lifetime = rospy.Duration(0.2)
for index in range(sector_count):
debug_marker = deepcopy(vfh_debug_marker)
sector_yaw = robot_yaw + (index - zero_offset) * sector_angle
quat_vals = tf.transformations.quaternion_from_euler(0, 0, sector_yaw)
debug_marker.pose.orientation = geometry_msg.Quaternion(*quat_vals)
debug_marker.pose.position = geometry_msg.Point(robot_position[0],
robot_position[1], 0)
if self.current_sector_index == index:
debug_marker.color = std_msg.ColorRGBA(0.1, 1.0, 0.1, 1)
elif not sectors[index]:
debug_marker.color = std_msg.ColorRGBA(0.1, 0.1, 1.0, 1)
else:
debug_marker.color = std_msg.ColorRGBA(1.0, 0.1, 0.1, 1)
debug_marker.id = self.marker_id
self.marker_id += 1
vfh_debug_array.append(debug_marker)
vfh_debug_msg = vis_msg.MarkerArray(vfh_debug_array)
self.debug_marker_pub.publish(vfh_debug_msg)
#load the state dictionary
vfh_state = {}
min_cost_index = np.argmax(inverse_costs)
vfh_state['minimum_cost_index'] = min_cost_index
vfh_state['minimum_cost_angle'] = (min_cost_index - zero_offset) * sector_angle
vfh_state['target_index'] = target_index
vfh_state['distance_to_target'] = distance_to_target
vfh_state['distance_off_course'] = distance_off_course
return sectors, vfh_state
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
#if that doesn't work, spin all the way around
if np.abs(error) < self._goal_orientation_tolerance:
self._mover.execute_spin(-1*rot_sign*2*np.pi,
max_velocity = spin_velocity,
stop_function=self.clear_check)
if self.exit_check(): return
dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom)
rospy.loginfo("VFH finished rotate_to-clear, start dyaw: {:f}, finish dyaw: {:f}".format(start_dyaw, dyaw))
if np.abs(start_dyaw - dyaw) < self._goal_orientation_tolerance:
#we spun all the way around +/- a little, so we are trapped
self._action_outcome == VFHMoveResult.BLOCKED
else:
#we found a clear spot, drive clear_distance meters ahead
saved_odom_point = deepcopy(self._target_point_odom)
#save the final goal for later
header = std_msg.Header(0, rospy.Time(0), 'base_link')
point = geometry_msg.Point(self._clear_distance, 0, 0)
target_point_base = geometry_msg.PointStamped(header, point)
self.set_path_points_odom(target_point_base)
if self.publish_debug: self.publish_debug_path()
self.vfh_running = True
self._mover.execute_continuous_strafe(0.0,
max_velocity = move_velocity,
stop_function=self.is_stop_requested)
self.vfh_running = False
if self.exit_check(): return
#reload previous goal
self.set_path_points_odom(saved_odom_point)
if self.publish_debug: self.publish_debug_path()
##### Begin normal movement section
self._action_outcome = VFHMoveResult.ERROR #initialize outcome to error
##### Turn to face goal
dyaw, d = util.get_robot_strafe(self._tf, self._target_point_odom)
if np.abs(dyaw) > self._goal_orientation_tolerance:
rospy.loginfo("VFH Rotating to point at goal by: %f.", dyaw)
self._mover.execute_spin(dyaw,
max_velocity = spin_velocity,
stop_function=self.is_stop_requested)
if self.exit_check(): return
##### Strafe to goal pose using VFH
yaw, distance = util.get_robot_strafe(self._tf, self._target_point_odom)
#start vfh and wait for one update
self.vfh_running = True
start_time = rospy.Time.now()
while not self._as.is_preempt_requested():
rospy.sleep(0.1)
if self.last_vfh_update > start_time: break
#If we received a valid goal (not too short, or out of target window), then
#the outcome should still be un-set (still ERROR).
if self._action_outcome == VFHMoveResult.ERROR:
rospy.loginfo("VFH Moving %f meters ahead.", distance)
self._mover.execute_continuous_strafe(0.0,
max_velocity = move_velocity,
stop_function=self.is_stop_requested)
self.vfh_running = False
if self.exit_check(): return
elif self._action_outcome == VFHMoveResult.BLOCKED:
rospy.loginfo("VFH reporting STARTED_BLOCKED")
#we found all sectors blocked before we even tried to move
self._action_outcome = VFHMoveResult.STARTED_BLOCKED
#if we got to the goal(ish), turn and face in requested orientation
if self._action_outcome in [VFHMoveResult.COMPLETE,
VFHMoveResult.MISSED_TARGET]:
# turn to requested orientation
dyaw = self.yaw_error()
if self._orient_at_target and (np.abs(dyaw) > self._goal_orientation_tolerance):
rospy.loginfo("VFH Rotating to goal yaw: %f.", dyaw)
self._mover.execute_spin(dyaw,
spin_velocity,
stop_function=self.is_stop_requested)
if self.exit_check(): return
rospy.logdebug("Successfully completed goal.")
self._as.set_succeeded(result =
VFHMoveResult(outcome = self._action_outcome,
position_error = self.position_error(),
yaw_error = Float64(self.yaw_error())))
