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