def obstacle_check(self, request ):
"""
Check a rectangular area in the costmap and return true if any lethal
cells are inside that area.
"""
#get current position from mover object
robot_position = self._mover.current_position
target_yaw = request.yaw
valid, robot_cmap_coords = self.world2map(robot_position[:2], self.costmap_info)
ll = self.bresenham_point(robot_cmap_coords,
request.width/2,
(target_yaw - np.pi/2),
self.costmap_info.resolution)
ul = self.bresenham_point(robot_cmap_coords,
request.width/2,
(target_yaw + np.pi/2),
self.costmap_info.resolution)
lr = self.bresenham_point(ll[0], request.distance, target_yaw, self.costmap_info.resolution)
ur = self.bresenham_point(ul[0], request.distance, target_yaw, self.costmap_info.resolution)
start_points = bresenham.points(ll, ul)
end_points = bresenham.points(lr, ur)
#check lines for lethal values
if self.any_line_blocked(start_points, end_points):
rospy.logdebug("PROBE SWATHE %f X %f BLOCKED " %(request.width, request.distance))
return True
else:
rospy.logdebug("PROBE SWATHE %f X %f CLEAR " %(request.width, request.distance))
return False
def line_blocked(self, userdata):
if self.debug_map_pub.get_num_connections() > 0:
publish_debug = True
else:
publish_debug = False
costmap = self.costmap
lethal_threshold = 90
blocked_threshold = 0.30
check_width = userdata.blocked_check_width/2
check_dist = userdata.blocked_check_distance
resolution = costmap.info.resolution
origin = (np.trunc(costmap.info.width/2),
np.trunc(costmap.info.height/2))
yaw = userdata.line_yaw
ll = self.check_point(origin, check_width, (yaw - math.pi/2), resolution)
ul = self.check_point(origin, check_width, (yaw + math.pi/2), resolution)
lr = self.check_point(ll[0], check_dist, yaw, resolution)
ur = self.check_point(ul[0], check_dist, yaw, resolution)
map_np = np.array(costmap.data, dtype='i1').reshape((costmap.info.height,costmap.info.width))
start_points = bresenham.points(ll, ul)
end_points = bresenham.points(lr, ur)
total_count = len(start_points)
#check lines for lethal values
blocked_count = 0
for start, end in zip(start_points, end_points):
line = bresenham.points(start[None,:], end[None,:])
line_vals = map_np[line[:,1], line[:,0]]
max_val = (np.amax(line_vals))
if np.any(line_vals > lethal_threshold):
blocked_count += 1
#for debug, mark lethal lines
if publish_debug: map_np[line[:,1], line[:,0]] = 64
#if anything is subscribing to the test map, publish it
if publish_debug:
map_np[start_points[:,1], start_points[:,0]] = 64
map_np[end_points[:,1], end_points[:,0]] = 64
costmap.data = list(np.reshape(map_np, -1))
self.debug_map_pub.publish(costmap)
if (blocked_count/float(total_count)) > blocked_threshold:
return True
return False
def any_line_blocked(self, start_points, end_points):
for start, end in zip(start_points, end_points):
line = bresenham.points(start[None,:], end[None,:])
line_vals = self.costmap[line[:,0], line[:,1]]
max_val = (np.amax(line_vals))
if np.any(line_vals > self._lethal_threshold):
#once an obstacle is found no need to keep checking this angle
return True
return False
def any_line_blocked(self, start_points, end_points, lethal_threshold, map_np):
for start, end in zip(start_points, end_points):
line = bresenham.points(start[None,:], end[None,:])
line_vals = map_np[line[:,1], line[:,0]]
max_val = (np.amax(line_vals))
if np.any(line_vals > lethal_threshold):
#for debug, mark lethal lines
if self.publish_debug: map_np[line[:,1], line[:,0]] = 64
#once an obstacle is found no need to keep checking this angle
return True
return False
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
