def init_grid_publisher(self): self.grid_publisher = rospy.Publisher('/indoor/occupancy_grid_topic', OccupancyGrid, queue_size=10) # OccupancyGrid documentation: # http://docs.ros.org/melodic/api/nav_msgs/html/msg/MapMetaData.html occ_grid_msg = OccupancyGrid() rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): m = MapMetaData() # Grid metadata m.resolution = self.res # Grid resolution m.width = self.x_lim[1] - self.x_lim[0] # Grid width in world CS m.height = self.y_lim[1] - self.y_lim[ 0] # Grid height in worlds CS m.origin = Pose( ) # The grid origin in world CS (there is no need for indoor navigation) occ_grid_msg.info = m occ_grid_msg.header.stamp = rospy.Time.now() occ_grid_msg.header.frame_id = "/indoor/occupancy_grid" # Convert the matrix from 2D fload64 to 1D int8 list occ_grid_msg.data = list( np.asarray(self.matrix.flatten(), dtype=np.int8)) # Publish the message self.grid_publisher.publish(occ_grid_msg) rate.sleep()
def get_ros_message(self): header = Header() header.seq = 0 header.stamp = rospy.Time.now() header.frame_id = '/odom' meta_data = MapMetaData() meta_data.resolution = self._resolution meta_data.width = self._values.shape[X] meta_data.height = self._values.shape[Y] p = Pose() p.position.x = self._origin[X] p.position.y = self._origin[Y] p.position.z = 0.01 p.orientation.x = 0. p.orientation.y = 0. p.orientation.z = 0. p.orientation.w = 1. meta_data.origin = p grid_msg = ros_nav.OccupancyGrid(header=header, info=meta_data, data=[]) for row in self._values: for v in row: grid_msg.data.append(int(v * (100 / OCCUPIED))) return grid_msg
def combine_maps(self, map1, map2, map1_metadata, map2_metadata): """ The algo: stitch map2 into map1, using the resolution of map1 1. Get xmin/max, ymin/ymax of both maps. 2. Build two empty occupancy grids of the correct shape. 3. Put each map into these occupancy grids (scale map2) 4. Combine maps with max. """ metadata_out = MapMetaData() m1_r = map1_metadata.resolution m2_r = map2_metadata.resolution m1_xmin = map1_metadata.origin.position.x m1_ymin = map1_metadata.origin.position.y m2_xmin = map2_metadata.origin.position.x m2_ymin = map2_metadata.origin.position.y m1_w = map1_metadata.width m1_h = map1_metadata.height m2_w = map2_metadata.width m2_h = map2_metadata.height m1_xmax = m1_xmin + m1_r * m1_w m2_xmax = m2_xmin + m2_r * m2_w m1_ymax = m1_ymin + m1_r * m1_h m2_ymax = m2_ymin + m2_r * m2_h map2_2_map1_scaling = m2_r / m1_r metadata_out.resolution = map1_metadata.resolution metadata_out.origin.position.x = min(m1_xmin, m2_xmin) metadata_out.origin.position.y = min(m1_ymin, m2_ymin) out_xmax = max(m1_xmax, m2_xmax) out_ymax = max(m1_ymax, m2_ymax) metadata_out.width = int((out_xmax - metadata_out.origin.position.x) / metadata_out.resolution) + 1 metadata_out.height = int((out_ymax - metadata_out.origin.position.y) / metadata_out.resolution) + 1 map2_scaled = rescale(map2, map2_2_map1_scaling, multichannel=False, anti_aliasing=False) map1_acc = np.zeros([metadata_out.width, metadata_out.height]) map2_acc = np.zeros([metadata_out.width, metadata_out.height]) m1_start_x = int((m1_xmin - metadata_out.origin.position.x) / metadata_out.resolution) m1_start_y = int((m1_ymin - metadata_out.origin.position.y) / metadata_out.resolution) m2_start_x = int((m2_xmin - metadata_out.origin.position.x) / metadata_out.resolution) m2_start_y = int((m2_ymin - metadata_out.origin.position.y) / metadata_out.resolution) map1_acc[m1_start_x:m1_start_x + map1.shape[0], m1_start_y:m1_start_y + map1.shape[1]] = np.copy(map1) map2_acc[m2_start_y:m2_start_y + map2_scaled.shape[0], m2_start_x:m2_start_x + map2_scaled.shape[1]] = np.copy(map2_scaled) out = np.maximum(map1_acc, map2_acc) return out, metadata_out
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 publishMap(): meta = MapMetaData() meta.width = 10 meta.height = 10 meta.resolution = 0.2 for c in range(-1,100): map = OccupancyGrid() map.header.frame_id = 'map' for i in range(0,100): val = c + i if val > 100: val = val - 101 map.data.append(val) map.info = meta mapPub.publish(map) publishGrid()
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 __init__(self, image_path=None): height = int(rospy.get_param("~grid_height", 800)) width = int(rospy.get_param("~grid_width", 800)) resolution = rospy.get_param("~grid_resolution", .3) ogrid_topic = rospy.get_param("~grid_topic", "/ogrid") self.grid_drawer = DrawGrid(height, width, image_path) self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1) m = MapMetaData() m.resolution = resolution m.width = width m.height = height pos = np.array([-width * resolution / 2, -height * resolution / 2, 0]) quat = np.array([0, 0, 0, 1]) m.origin = Pose() m.origin.position.x, m.origin.position.y = pos[:2] self.map_meta_data = m rospy.Timer(rospy.Duration(1), self.pub_grid)
def __init__(self, image_path=None): height = int(rospy.get_param("~grid_height", 800)) width = int(rospy.get_param("~grid_width", 800)) resolution = rospy.get_param("~grid_resolution", .3) ogrid_topic = rospy.get_param("~grid_topic", "/ogrid") self.grid_drawer = DrawGrid(height, width, image_path) self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1) m = MapMetaData() m.resolution = resolution m.width = width m.height = height pos = np.array([-width * resolution / 2, -height * resolution / 2, 0]) m.origin = Pose() m.origin.position.x, m.origin.position.y = pos[:2] self.map_meta_data = m rospy.Timer(rospy.Duration(1), self.pub_grid)
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 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
height_offset = 58 captured_width = 70 captured_height = 50 frame_rate = 14 # Hz preview_pub = rospy.Publisher("/igvc/preview", Image, queue_size=1) image_pub = rospy.Publisher("/igvc/lane_map", OccupancyGrid, queue_size=1) header = Header() header.frame_id = "base_link" map_info = MapMetaData() map_info.width = occupancy_grid_size map_info.height = occupancy_grid_size map_info.resolution = 0.1 map_info.origin = Pose() map_info.origin.position.x = -10 map_info.origin.position.y = -10 cam = None class PerspectiveTransform: def __init__(self, camera_angle): self.top_trim_ratio = 0.0 # Camera angle in degrees, where 0 is when camera is facing perpendicular to the ground self.camera_angle = camera_angle # Ratio of number of pixels between top points of the trapezoid and their nearest vertical border
#!/usr/bin/env python import rospy from nav_msgs.msg import MapMetaData from sdpp_modules.sdpp_agents import WalkingPerson if __name__ == '__main__': rospy.init_node("grid_node") MapMeta = MapMetaData() MapMeta.resolution = .1 MapMeta.width = 200 #in cells MapMeta.height = 200 #in cells MapMeta.origin.position.x = -10 #in what? MapMeta.origin.position.y = -10 #in what? MapMeta.origin.position.z = 0 agent_1 = WalkingPerson(MapMeta, "person_1") agent_1.build_heading_graph((3, 4)) agent_1.set_update_rate(20) try: while not rospy.is_shutdown(): agent_1.update_step() except rospy.ROSInterruptException: pass
def talker(): odom_pub = rospy.Publisher("odom1", Odometry, queue_size=50) map_pub = rospy.Publisher("map1", OccupancyGrid, queue_size=10) rospy.init_node('talker', anonymous=True) odom_broadcaster = tf.TransformBroadcaster() #x = 0.0 #y = 0.0 th = 0.0 #vx = 0.1 #vy = -0.1 vth = 0.1 current_time = rospy.Time.now() last_time = rospy.Time.now() prefix = "out_turtle_map_" ext = ".owl" times = "1713" ## update init owl file file_input = prefix + times + ext r = rospy.Rate(1.0) while not rospy.is_shutdown(): current_time = rospy.Time.now() g = Graph() ''' #to Cloud print "file to download "+ file_input myCmd = 'gsutil -m cp gs://slam-up-bucket/'+file_input+' ./input' os.system(myCmd) local_file_input = 'input/'+file_input print "file que se va parsear "+local_file_input ''' local_file_input = 'out/' + file_input ## INFO:: en el local if os.path.exists(local_file_input): print "exist path " + local_file_input g.parse(local_file_input, format="turtle") print("graph has %s statements." % len(g)) px, py, pz = get_position(g) ox, oy, oz, ow = get_orientation(g) matrix = get_covar_matrix(g) covar = np.array([0.0] * 36).reshape(6, 6) for cell in matrix: row = int(cell[0]) col = int(cell[1]) value = float(cell[2]) covar[row, col] = value covar_list = tuple(covar.ravel().tolist()) #Creacion de nav_msg/Odometry # compute odometry in a typical way given the velocities of the robot '''dt = (current_time - last_time).to_sec() delta_x = (vx * cos(th) - vy * sin(th)) * dt delta_y = (vx * sin(th) + vy * cos(th)) * dt delta_th = vth * dt x += delta_x y += delta_y th += delta_th ''' # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) # first, we'll publish the transform over tf odom_broadcaster.sendTransform((px, py, pz), odom_quat, current_time, "base_link1", "odom1") # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom1" # set the position odom.pose.pose = Pose(Point(px, py, pz), Quaternion(ox, oy, oz, ow)) odom.pose.covariance = covar_list # set the velocity vx = vy = 0 odom.child_frame_id = "base_link1" odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) odom_pub.publish(odom) ##map width = height = resolution = 0 mapa_info = get_map_info(g) if len(mapa_info) > 0: flagMap = True if flagMap is not True and var_m is not None and var_grid_list is not None: print "using saved" m = var_m grid_list = var_grid_list else: print "reading map" for row in mapa_info: p = row[1] o = p[p.find('#') + 1:len(p)] if o == "Width": width = int(row[2]) if o == "Height": height = int(row[2]) resolution = get_map_resolution(g) #Creacion de nav_msg/occupancyGrid len_grid = width * height grid_map = np.array([-1] * len_grid).reshape(width, height) objects = get_map_objects(g) print("objects detected") print(len(objects)) for row in objects: p = row[0] obj_prefix = "http://github.com/Alex23013/slam-up/individual/obj/" obj_pos = p[len(obj_prefix):len(p)] parts = obj_pos.split('_') grid_map[int(parts[0]), int(parts[1])] = int(row[1]) grid_list = tuple(grid_map.ravel().tolist()) map_broadcaster = tf.TransformBroadcaster() map_broadcaster.sendTransform( (0, 0, 0), odom_quat, current_time, "base_link1", "map1") m = MapMetaData() m.resolution = resolution m.width = width m.height = height pos = np.array( [-width * resolution / 2, -height * resolution / 2, 0]) quat = np.array([0, 0, 0, 1]) m.origin = Pose() m.origin.position.x, m.origin.position.y = pos[:2] ogrid = OccupancyGrid() ogrid.header.frame_id = 'map1' ogrid.header.stamp = rospy.Time.now() ogrid.info = m ogrid.data = grid_list var_m = m var_grid_list = grid_list map_pub.publish(ogrid) rospy.loginfo("publishing map:") rospy.loginfo(times) last_time = current_time else: print local_file_input, "file not found" rospy.loginfo("publishing odometry:") rospy.loginfo(times) times = str(1 + int(times)) file_input = prefix + times + ext print "antes sleep" r.sleep() print "fin_loop"
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()
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 create_local_map(self, event): if self.odom is None or self.scan is None: return occupancy_grid = OccupancyGrid() occupancy_grid.header.stamp = rospy.Time.now() occupancy_grid.header.frame_id = self.frame_id # Map data information map_data_info = MapMetaData() map_data_info.resolution = self.cell_size # [m/cell] map_data_info.width = self.cell_length # [cells] map_data_info.height = self.cell_length q = (self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w) self.vehicle_yaw = tf.transformations.euler_from_quaternion(q)[2] ''' x_orig = -self.map_length*cos(self.vehicle_yaw) + self.map_length*sin(self.vehicle_yaw) y_orig = -self.map_length*sin(self.vehicle_yaw) - self.map_length*cos(self.vehicle_yaw) map_data_info.origin.position.x = int( x_orig + self.odom.pose.pose.position.x ) map_data_info.origin.position.y = int( y_orig + self.odom.pose.pose.position.y ) map_data_info.origin.position.z = int(self.odom.pose.pose.position.z) map_data_info.origin.orientation = self.odom.pose.pose.orientation ''' map_data_info.origin.position.x = self.odom.pose.pose.position.x - self.map_length map_data_info.origin.position.y = self.odom.pose.pose.position.y - self.map_length map_data_info.origin.position.z = 0 map_data_info.origin.orientation.x = 0 map_data_info.origin.orientation.y = 0 map_data_info.origin.orientation.z = 0 map_data_info.origin.orientation.w = 1.0 occupancy_grid.info = map_data_info ''' print (self.vehicle_yaw/3.14*180) print(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y) print(x_orig, y_orig) print(map_data_info.origin.position.x, map_data_info.origin.position.y) ''' # initial cell data = np.zeros(map_data_info.width * map_data_info.height) # read laser scan to grid map for laser in range(0, len(self.scan.ranges)): scan_range = self.scan.ranges[laser] rad = self.scan.angle_min + laser * self.scan.angle_increment + self.vehicle_yaw # remove points out of range if scan_range is None or scan_range < self.scan.range_min or scan_range > self.scan.range_max or scan_range > self.map_length - 1: # continue cell_num = self.get_cell_number(scan_range, rad, map_data_info) for value in cell_num: data[value] = 100 #for i in range(0, 200): # data[i] = -1 #print("==========================") occupancy_grid.data = data self.pub_grid_map.publish(occupancy_grid)
occ_grid.info.width = 512 occ_grid.info.height = 480 occ_grid.info.origin.position.x = 0.0 occ_grid.info.origin.position.y = 0.0 occ_grid.info.origin.position.z = 0.0 occ_grid.info.origin.orientation.x = 0.0 occ_grid.info.origin.orientation.y = 0.0 occ_grid.info.origin.orientation.z = 1.0 occ_grid.info.origin.orientation.w = 0.0 occ_grid.data = [0 for i in range(512 * 480)] metadata = MapMetaData() metadata.map_load_time.secs = 1381949468 metadata.map_load_time.nsecs = 318325596 metadata.resolution = 0.05 metadata.width = 512 metadata.height = 480 metadata.origin.position.x = 0.0 metadata.origin.position.y = 0.0 metadata.origin.position.z = 0.0 metadata.origin.orientation.x = 0.0 metadata.origin.orientation.y = 0.0 metadata.origin.orientation.z = 1.0 metadata.origin.orientation.w = 0.0 pub_mapserver.publish(occ_grid) while not rospy.is_shutdown(): pub_mapserver_metadata.publish(metadata)