def expandMap(): global expandedMap global expandedMap2 global expandedWidth expandedMap = copy.deepcopy(grid) print "Expanded is %f" % len(grid) print "expanded width %f height %f" % (width, height) for i in range(0, height): for j in range(0, width): if (grid[j + (width * i)].intensity >= 100): for k in range(j - int(round(expandBuffer / res)), j + int(round(expandBuffer / res)) + 1): for l in range(i - int(round(expandBuffer / res)), i + int(round(expandBuffer / res)) + 1): if (k > 0 and k < width and l > 0 and l < height): #print "Trying index %f k %f l %f" % (k + (width * l),k,l) expandedMap[k + (width * l)].intensity = 100 expandedMap2 = [] added = False block = False for i in range(0, int(height / (expandedGridRes / res))): for j in range(0, int(width / (expandedGridRes / res))): block = False for y in range(0, int(expandedGridRes / res)): for x in range(0, int(expandedGridRes / res)): if (expandedMap[int(i * width * (expandedGridRes / res) + y * width + j * (expandedGridRes / res) + x)].intensity > expandThreshold): block = True if (block): expandedMap2.append( Node(j * expandedGridRes + gridOrigin.position.x, i * expandedGridRes + gridOrigin.position.y, 100)) else: expandedMap2.append( Node(j * expandedGridRes + gridOrigin.position.x, i * expandedGridRes + gridOrigin.position.y, 0)) newWidth = int(width / (expandedGridRes / res)) newHeight = int(height / (expandedGridRes / res)) expandedWidth = newWidth ocGrid = OccupancyGrid() pub_map = [] meta = MapMetaData() meta.map_load_time = rospy.Time.now() meta.width = newWidth meta.height = newHeight meta.resolution = expandedGridRes for next in expandedMap2: pub_map.append(next.intensity) ocGrid.header.frame_id = '/map' ocGrid.header.stamp = rospy.Time.now() ocGrid.info = meta ocGrid.info.origin = gridOrigin ocGrid.data = pub_map expanded_pub.publish(ocGrid)
def publishOccupancyGrid(self): grid = self.senseOccupancyGrid() header = Header() header.stamp = rospy.Time.now() header.frame_id = 'odom' map_meta_data = MapMetaData() map_meta_data.map_load_time = rospy.Time.now() map_meta_data.resolution = grid_resolution # each cell is 15cm map_meta_data.width = int(grid_size / grid_resolution) map_meta_data.height = int(grid_size / grid_resolution) map_meta_data.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) grid_msg = OccupancyGrid() grid_msg.header = header grid_msg.info = map_meta_data grid_msg.data = list(grid) self.occupancyGridPublisher.publish(grid_msg)
def receive_point_cloud(self, msg): ''' Processes the pointcloud into a costmap ''' xyz_arr = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg) # Flatten the points onto a grid obs_grid = self.project_point_cloud_onto_plane(xyz_arr) obs_grid[obs_grid != 0] = 1 header = Header() header.stamp = msg.header.stamp header.frame_id = 'base_link' # local grid is in base_link frame map_meta_data = MapMetaData() map_meta_data.map_load_time = header.stamp map_meta_data.resolution = self.resolution map_meta_data.width = self.grid_size map_meta_data.height = self.grid_size map_meta_data.origin = Pose( Point(self.camera_x_offset, self.grid_size * self.resolution / 2 + self.camera_y_offset, 0), Quaternion(0, 0, -sqrt(2) / 2, sqrt(2) / 2)) # 90 degree rotation grid_msg = OccupancyGrid() grid_msg.header = header grid_msg.info = map_meta_data grid_msg.data = list( np.int8(obs_grid.flatten() * 100)) # occupany grid message requires values 0-100 try: self.grid_pub.publish(grid_msg) except rospy.ROSInterruptException as e: print(e.getMessage()) pass
def local_grid_callback(self, msg): grid_size = msg.info.width self.local_grid = np.flipud( np.reshape(msg.data, (grid_size, grid_size))) # Transform center of grid in robot coordinate frame to map frame grid_origin = PointStamped( msg.header, Point(self.camera_offset[0] + grid_size * self.resolution / 2, self.camera_offset[1], 0)) try: grid_origin = self.tf_buffer.transform(grid_origin, "map", rospy.Duration(0.1)) rot = self.tf_buffer.lookup_transform( "base_link", "map", msg.header.stamp, rospy.Duration(0.1)).transform.rotation except Exception as e: rospy.logwarn(e) return grid_origin = np.array([grid_origin.point.x, grid_origin.point.y]) robot_angle = R.from_quat([rot.x, rot.y, rot.z, rot.w]).as_euler('zyx')[0] if self.save_data: print('Saving...') np.save( '%s/%d.npy' % (self.data_dir, len(os.listdir(self.data_dir))), self.local_grid) np.save( '%s/%d.npy' % (self.data_dir + 'localization', len(os.listdir(self.data_dir + 'localization'))), np.array( [self.robot_x[-1], self.robot_y[-1], self.robot_pitch[-1]])) indices = np.indices(self.local_grid.shape) mask = abs(indices[1] - self.local_grid.shape[1] / 2) < ( self.local_grid.shape[0] - indices[0]) mask = np.reshape(mask, (grid_size, grid_size)) self.local_grid[~mask] = -1 # Rotate image using degrees for some dumb reason rotated_grid = ndimage.rotate( self.local_grid, (-robot_angle * 180 / np.pi + self.camera_offset[2]), mode='constant', cval=-1, reshape=True) # rotate grid by camera + robot angle # convert to grid indices: grid_origin = grid_origin / self.resolution # flip due to coordinate system of matrix being top left # Switch local origin from center of image to top left based on new width of rotated image grid_origin = [ int( np.round(self.global_grid_shape[0] - grid_origin[1] - rotated_grid.shape[0] / 2)), int(np.round(grid_origin[0] - rotated_grid.shape[1] / 2)) ] # Account for map origin not being the 0,0 cell grid_origin = [ grid_origin[0] - self.map_buffer, grid_origin[1] + self.map_buffer ] counts = np.ones_like( rotated_grid) # Count how many times each cell was measured counts[rotated_grid == -1] = -1 counts = self.paste(self.global_counts, counts, grid_origin) roi = (counts != -1) counts[~roi] = 0 global_additions = self.paste(self.global_totals, rotated_grid, grid_origin) window_size = 100 self.global_totals[roi] = (self.global_totals[roi] * np.minimum(self.global_counts[roi], window_size) + global_additions[roi]) \ / (np.minimum(self.global_counts[roi], window_size) + 1) self.global_counts += counts # self.global_grid = np.divide(self.global_totals, self.global_counts, out=np.zeros_like(self.global_totals), where=self.global_counts!=0) # self.global_grid = np.nan_to_num(self.global_grid, copy=False) # self.global_grid /= np.max(self.global_grid) # ensure values are 0-1 self.global_grid = ndimage.maximum_filter(self.global_totals, size=6, mode='nearest') self.global_grid[self.global_grid >= 50] = 100 self.global_grid = ndimage.gaussian_filter(self.global_grid, sigma=1.5) self.global_grid[self.global_grid >= 50] = 100 self.global_grid[self.global_totals >= 50] = 150 + self.global_totals[ self.global_totals >= 50] header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' map_meta_data = MapMetaData() map_meta_data.map_load_time = rospy.Time.now() map_meta_data.resolution = self.resolution map_meta_data.width = self.global_grid_shape[1] map_meta_data.height = self.global_grid_shape[0] map_meta_data.origin = Pose( Point(-self.map_buffer * self.resolution, -self.map_buffer * self.resolution, 0), Quaternion(0, 0, 0, 1)) grid_msg = OccupancyGrid() grid_msg.header = header grid_msg.info = map_meta_data grid_msg.data = list((np.int8(np.flipud(self.global_grid).flatten()))) try: pub = rospy.Publisher("global_occupancy_grid", OccupancyGrid, queue_size=1) pub.publish(grid_msg) except rospy.ROSInterruptException as e: print(e.getMessage()) pass if self.save_imgs: fig = plt.figure(figsize=(15, 5)) ax = plt.subplot(131) ax.imshow(self.local_grid, cmap='Reds', vmin=0, vmax=100) ax.set_title('Local Occupancy Grid') ax = plt.subplot(132) ax.imshow(rotated_grid, cmap='Reds', vmin=0, vmax=100) ax.set_title('Rotated Occupancy Grid') ax = plt.subplot(133) ax.imshow(self.global_grid, cmap='Reds', vmin=0, vmax=100) ax.set_title('Global Occupancy Grid') fig.savefig('%s/%d' % (self.viz_dir, len(os.listdir(self.viz_dir)))) plt.close()
def getMap(self, wall_objects): if len(wall_objects) == 0: return None now = rospy.Time.now() half_wall_length = self.wall_size.x / 2.0 #TODO: add/subtract half the length from each object as go along min_x = min([wall['pose'].position.x for wall in wall_objects]) min_y = min([wall['pose'].position.y for wall in wall_objects]) max_x = max([wall['pose'].position.x for wall in wall_objects]) max_y = max([wall['pose'].position.y for wall in wall_objects]) min_x -= half_wall_length min_y -= half_wall_length max_x += half_wall_length max_y += half_wall_length map_origin = Pose() map_origin.position.x = min_x map_origin.position.y = min_y # map_origin.orientation.w = 1 # .505 map_origin.orientation.z = 0 #.505 map_width = int((max_x - min_x) / self.resolution) map_height = int((max_y - min_y) / self.resolution) empty_map = self.OCC_UNKNOWN * np.ones(shape=(map_width, map_height), dtype=np.int8) map_image = empty_map #pts = np.array(shape=(len(wall_poses),2),dtype=np.int32) #pose_ind = 0 for wall in wall_objects: pose = wall['pose'] x_im = pose.position.y - min_y y_im = pose.position.x - min_x quat = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] angle = transformations.euler_from_quaternion(quat)[2] wall_size = wall['size'] half_wall_length = wall_size.x / 2.0 wall_thickness = int(wall_size.y / self.resolution) p1x = x_im + math.sin(angle) * half_wall_length p2x = x_im - math.sin(angle) * half_wall_length p1y = y_im + math.cos(angle) * half_wall_length p2y = y_im - math.cos(angle) * half_wall_length p1x = int(p1x / self.resolution) p2x = int(p2x / self.resolution) p1y = int(p1y / self.resolution) p2y = int(p2y / self.resolution) cv2.line(img=map_image, pt1=(p1x, p1y), pt2=(p2x, p2y), color=self.OCC_OBSTACLE, thickness=wall_thickness) #pts.append(((p1x,p1y),(p2x,p2y))) #cv2.imshow(winname="map",mat=map_image) #cv2.waitKey(10) #cv2.polylines(img=map_image,pts=pts,isClosed=False,color=self.OCC_OBSTACLE,thickness=wall_thickness) #cv2.imshow(winname="map",mat=map_image) #cv2.waitKey(1) map_msg = OccupancyGrid() map_msg.header.frame_id = self.gazebo_frame_id map_msg.header.stamp = now map_msg.data = np.reshape(a=map_image, newshape=-1, order='F').tolist( ) #First flatten the array, then convert it to a list metadata = MapMetaData() metadata.resolution = self.resolution metadata.map_load_time = now metadata.width = map_width metadata.height = map_height metadata.origin = map_origin map_msg.info = metadata return map_msg
def gridmap_publisher(): gridmap_pub = rospy.Publisher('gridmap', OccupancyGrid, queue_size=1) pose_pub = rospy.Publisher('pose', geometry_msgs.msg.PoseStamped, queue_size=10) scan_pub = rospy.Publisher('scan', LaserScan, queue_size=10) rospy.init_node('gridmap_publisher', sys.argv) rospy.loginfo('Initialized gridmap_publisher') # Gridmap laser_data = LaserDataMatlab(sys.argv[1]) gridmap = GridMap(grid_size=0.1) gridmap.init_from_laserdata(laser_data) # Setup ros message timestep_list = laser_data.get_timestep_list() timesteps = timestep_gen(timestep_list) timestep_len = len(timestep_list) map_meta = MapMetaData() # map_meta.map_load_time = rospy.Time().now() map_meta.resolution = gridmap._grid_size grid_map = gridmap.get_prob_map() * 100 grid_map = grid_map.astype(np.int8) # Width = cols map_meta.width = grid_map.shape[1] # Height = rows map_meta.height = grid_map.shape[0] map_meta.origin.position.x = 0 map_meta.origin.position.y = 0 map_meta.origin.position.z = 0 # Broadcast robot transform pose_tf_br = tf2_ros.TransformBroadcaster() rbase_tf = geometry_msgs.msg.TransformStamped() rbase_tf.header.stamp = rospy.Time.now() rbase_tf.header.frame_id = 'map' rbase_tf.child_frame_id = 'robot_base' rate = rospy.Rate(10) rx = 0 - gridmap._offset[0] ry = 0 - gridmap._offset[1] rtheta = 0 pose_msg = geometry_msgs.msg.PoseStamped() pose_msg.header.frame_id = 'map' scan_msg = LaserScan() scan_msg.header.frame_id = 'robot_base' # Get info about laser scan laser_scan_temp = laser_data.get_range_scan(0) # This info doesn't change start_angle = laser_scan_temp['start_angle'] angular_resolution = laser_scan_temp['angular_resolution'] scan_msg.angle_min = start_angle scan_msg.angle_increment = angular_resolution scan_msg.range_min = 0 scan_msg.time_increment = 0 robot_pose = None range_scan = None while not rospy.is_shutdown(): try: t = next(timesteps) robot_pose = laser_data.get_pose(t) range_scan = laser_data.get_range_scan(t) gridmap.update(robot_pose, range_scan) except StopIteration: robot_pose = laser_data.get_pose(timestep_len - 1) range_scan = laser_data.get_range_scan(timestep_len - 1) grid_map = gridmap.get_prob_map() * 100 grid_map = grid_map.astype(np.int8) rx = robot_pose.item(0) - gridmap._offset[0] ry = robot_pose.item(1) - gridmap._offset[1] rtheta = robot_pose.item(2) # Robot_base transform rbase_tf.transform.translation.x = rx rbase_tf.transform.translation.y = ry rbase_tf.transform.translation.z = 0 quat = tf.transformations.quaternion_from_euler(0, 0, rtheta) rbase_tf.transform.rotation.x = quat[0] rbase_tf.transform.rotation.y = quat[1] rbase_tf.transform.rotation.z = quat[2] rbase_tf.transform.rotation.w = quat[3] # Robot pose pose_msg.pose.position.x = rx pose_msg.pose.position.y = ry pose_msg.pose.position.z = 0 pose_quat = tf.transformations.quaternion_from_euler(0, 0, rtheta) pose_msg.pose.orientation.x = pose_quat[0] pose_msg.pose.orientation.y = pose_quat[1] pose_msg.pose.orientation.z = pose_quat[2] pose_msg.pose.orientation.w = pose_quat[3] # Laser scan # Get changing info from current scan num_beams = len(range_scan['ranges']) max_range = range_scan['maximum_range'] laser_ranges = range_scan['ranges'] valid_endpoints = (laser_ranges < max_range) & (laser_ranges > 0) laser_ranges = laser_ranges[valid_endpoints] scan_msg.angle_max = start_angle + num_beams * angular_resolution scan_msg.range_max = max_range scan_msg.ranges = laser_ranges scan_msg.time_increment = (1 / 50) / num_beams scan_msg.scan_time = rospy.Time.now().nsecs - scan_msg.scan_time # Publish everything rbase_tf.header.stamp = rospy.Time.now() pose_msg.header.stamp = rospy.Time.now() scan_msg.header.stamp = rospy.Time.now() pose_tf_br.sendTransform(rbase_tf) pose_pub.publish(pose_msg) scan_pub.publish(scan_msg) h = Header() h.stamp = rospy.Time.now() map_meta.map_load_time = rospy.Time().now() gridmap_pub.publish(header=h, info=map_meta, data=grid_map.flatten()) rate.sleep()