def publish_og(self): og = self.cleaningManager.mapManager.occupancy_grid OG = OccupancyGrid() altered_og = [] i = 0 for val in og: if val == -1: altered_og.append(val) else: altered_og.append(int(val * 100)) i = i + 1 OG.data = altered_og OG.info.width = int(math.sqrt(len(og))) OG.info.height = int(math.sqrt(len(og))) OG.info.resolution = (2 * X_MAX) / OG_WIDTH visible_corners = self.cleaningManager.mapManager.get_visible_area_corners( self.cleaningManager.mapManager.mapMaker.translation, self.cleaningManager.mapManager.mapMaker.orientation) og_origin = Pose() og_origin.position.x = visible_corners[3][0] og_origin.position.y = visible_corners[3][1] euler = self.quaternion_to_euler( self.cleaningManager.mapManager.mapMaker.orientation[0], self.cleaningManager.mapManager.mapMaker.orientation[1], self.cleaningManager.mapManager.mapMaker.orientation[2], self.cleaningManager.mapManager.mapMaker.orientation[3]) euler[0] = 0 euler[1] = 0 new_quat = self.euler_to_quaternion(euler[0], euler[1], euler[2]) og_origin.orientation.x = new_quat[0] og_origin.orientation.y = new_quat[1] og_origin.orientation.z = new_quat[2] og_origin.orientation.w = new_quat[3] OG.info.origin = og_origin self.OGpub.publish(OG)
def __init__(self): rospy.init_node('turtlebot', anonymous=True) # initialize a publisher for occupancy grid self.occupancy_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=10) # rospy.loginfo("got into initializaytion") self.occupancy_grid = OccupancyGrid() metadata = MapMetaData() # metadata.map_load_time = None metadata.resolution = resolution = 0.1 metadata.width = 250 metadata.height = 250 pos = np.array([ -metadata.width * resolution / 2, -metadata.height * resolution / 2, 0 ]) metadata.origin = Pose() metadata.origin.position.x, metadata.origin.position.y = pos[:2] self.conversion_m_to_xy = metadata.width / 30 + 10 #self.conversion_m_to_y = self. self.map_metadata = metadata self.occupancy_grid.info = self.map_metadata self.occupancy_grid.data = np.ones( metadata.height * metadata.width).astype(int) * O_0 self.laser_data = None self.position = None self.orientation = None self.covariance = None # initialize a subscriber to receive the estimated pose and the map rospy.Subscriber("/scan", LaserScan, self.laser_callback) # initialize a subscriber to receiver estimated posiition from the kalman filter output rospy.Subscriber('/indoor_pos', PoseWithCovarianceStamped, self.IPS_callback)
def publish_Q_table(self, table): costmap = OccupancyGrid() costmap.header.stamp = rospy.Time.now() costmap.header.frame_id = '/map' costmap.info.width = 16 costmap.info.height = 16 + 1 # Last one line is for extra information costmap.info.resolution = fieldScale / 16 costmap.info.origin.position.x = fieldScale / math.sqrt(2) costmap.info.origin.position.y = 0 costmap.info.origin.position.z = 0.01 q = tf.transformations.quaternion_from_euler(math.pi / 4, 0, 0) #q = tf.transformations.quaternion_from_euler(0, 0, 0) costmap.info.origin.orientation.x = q[1] costmap.info.origin.orientation.y = q[2] costmap.info.origin.orientation.z = q[3] costmap.info.origin.orientation.w = q[0] # Scale Q table q_min = np.min(table) q_max = np.max(table) data = np.flipud(table).reshape(-1) costmap.data = (data - q_min) / ( q_max - q_min ) * 99.0 + 1.5 # 1 to 100. 0(transparent), 1(blue) to 98(red), 99(cyan), 100(pink) #costmap.data = data * 50.0 + 50.0 # Add min and max to costmap costmap.data = np.hstack([costmap.data, np.zeros(16)]) costmap.data[256] = q_min * 50.0 + 50.5 costmap.data[257] = q_max * 50.0 + 50.5 # Cast float to int8 costmap.data = np.array(costmap.data, dtype='int8') # Publish rospy.loginfo('Q(min,max)=(%.3f, %.3f)' % (q_min, q_max)) self.q_table_pub.publish(costmap)
def talker(): pub = rospy.Publisher('fake_occupancy_grid', OccupancyGrid, queue_size=10) rospy.init_node('fake_occupancy_grid', anonymous=True) rate = rospy.Rate(10) # 10hz grid = OccupancyGrid() # see if you can change metadata to calling on a parameter of rviz? grid.info.height = 20 grid.info.width = 20 values = [-1] * (20 * 20) # do some stuff to values here grid.data = values time_up = rospy.get_time() + 3 height = 5 width = 3 while not rospy.is_shutdown(): if rospy.get_time() > time_up: known_cells = [] for i in range(height): for j in range(width): known_cells.append(10 + i * 20 + j - 1) known_cells.append(10 + i * 20 - j) for c in known_cells: if values[c] < 0: if (c == 32 or c == 31 or c == 53 or c == 52 or c == 103 or c == 104 or c == 124 or c == 125 or c == 146 or c == 145 or c == 144 or c == 333 or c == 334 or c == 353 or c == 335 or c == 222 or c == 223 or c == 224 or c == 243 or c == 263): values[c] = 80 else: values[c] = 20 if height < 20: height += 1 if width < 11: width += 1 time_up = rospy.get_time() + 1 pub.publish(grid) rate.sleep()
def update_map(self): background_image = self.canvas.pilimage.copy() draw = PILImageDraw.Draw(background_image) item_list = self.canvas.find_all() for item in item_list: tag = self.canvas.gettags(item)[0] if tag.startswith('wall_'): coords = self.canvas.coords(item) params = ['fill', 'width'] opt = dict() for p in params: opt[p] = self.canvas.itemcget(item, p) draw.line(coords, fill=opt['fill'], width=int(float(opt['width']))) if self.map_resolution != self.gui_resolution: factor = self.gui_resolution / self.map_resolution width = int(np.ceil(factor * background_image.size[0])) height = int(np.ceil(factor * background_image.size[1])) background_image = background_image.resize( (width, height), resample=PILImage.NEAREST) width, height = background_image.size map_quat = quaternion_from_euler(0.0, 0.0, 0.0) map_pose = Pose(Point(0.0, height * self.map_resolution, 0.0), Quaternion(*map_quat)) map_metadata = MapMetaData(rospy.Time.now(), self.map_resolution, width, height, map_pose) self.pub_map_metadata.publish(map_metadata) og_header = Header(0, rospy.Time.now(), 'map_frame') og_data = np.array(background_image) og_data = (100 - (og_data / 255.0) * 100).astype(np.uint8) og_data = og_data.reshape(height * width) occupancy_grid = OccupancyGrid(og_header, map_metadata, og_data.tolist()) self.pub_map.publish(occupancy_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 __init__(self, range_x, range_y, frame_id, resolution=1.5): ''' Set up the occupancy grid generator object. Args: - range_x (float): Total range of the occupancy grid width (the x-origin is at the center of this range). - range_y (float): Total range of the occupancy grid height (the y-origin is 0). - frame_id (str): The ROS frame ID at which the occupancy grid origin is aligned with. - resolution (float): The size of each cell in the occupancy grid (in meters). Return: None. ''' self.info = MapMetaData() self.info.resolution = resolution # Meters self.info.height = int(range_x / self.info.resolution) # Meters self.info.width = int(range_y / self.info.resolution) # Meters self.x_offset = range_x / 2 # Center left-right about ego vehicle self.info.origin = self.numpy_to_position([0, -self.x_offset, -0.1], Pose()) self.occupancy_grid = OccupancyGrid() self.occupancy_grid.info = self.info self.occupancy_grid.header.frame_id = frame_id self.bins_x = np.linspace(-range_x / 2, range_x / 2, self.info.height) self.bins_y = np.linspace(0, range_y, self.info.width) # Meshgrid for Gaussians. Y = np.linspace(-range_x // 2, range_x // 2, self.info.height) X = np.linspace(0, range_y, self.info.width) X, Y = np.meshgrid(X, Y) # Pack X and Y into a single 3-dimensional array. self.meshgrid = np.empty(X.shape + (2, )) self.meshgrid[:, :, 0] = X self.meshgrid[:, :, 1] = Y
def config_space_callback(event): if last_vision is None and last_lidar is None: return # Reset the hidden layer config_space = [0] * (200 * 200) combined_maps = None if last_vision is not None and last_lidar is not None: combined_maps = [x+y for x,y in zip(last_lidar.data, last_vision)] elif last_vision is not None: combined_maps = last_vision else: combined_maps = last_lidar.data # Update the hidden layer before applying the new map to the current configuration space for x in range(200): for y in range(200): if combined_maps[x + y * 200] > 0: for x_i, y_i, dist in circle_around_indicies: index = (x + x_i) + 200 * (y + y_i) if 0 <= (x + x_i) < 200 and 0 <= (y + y_i) < 200: val_at_index = config_space[index] linear_t = max_range**2 - ((dist - no_go_range) / (max_range - no_go_range) * max_range**2) if dist <= no_go_range: # obstacle expansion config_space[index] = 100 elif dist <= 100 and val_at_index <= linear_t: # linearly decay config_space[index] = int(linear_t) # Publish the configuration space header.stamp = rospy.Time.now() config_msg = OccupancyGrid(header=header, info=metadata, data=config_space) config_pub.publish(config_msg)
def make_grid(width=100, height=100, resolution=0.05): # Configure the Header message file h = Header() h.stamp = rospy.Time.now() h.frame_id = 'map' map_load_time = rospy.Time(0) # resolution is in unit m/cell # width is in unit cells # height is in unit cells q = quaternion_from_euler(0, np.pi, -np.pi / 2) # (rot about x, rot about y, rot about z) xq = q[0] yq = q[1] zq = q[2] wq = q[3] orientation = Quaternion(xq, yq, zq, wq) origin = Pose(Point(0, 0, 0), orientation) info = MapMetaData(map_load_time, resolution, width, height, origin) n = width * height data = np.zeros(n) return OccupancyGrid(h, info, data)
def to_message(self): grid_msg = OccupancyGrid() # Set up the header grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" # info is a nav_msgs/MapMetaData message grid_msg.info.resolution = self.resolution grid_msg.info.width = self.width grid_msg.info.height = self.height # Rotated maps are not supported.. quaternion represents no rotation grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0), Quaternion(0, 0, 0, 1)) # Flatten the numpy array into a list of integers from 0-100 # This assumes that the grid entries are probabilities in the range 0-1 flat_grid = self.grid.reshape((self.grid.size, )) * 100 grid_msg.data = list(np.round(flat_grid)) return grid_msg
def __init__(self, footprint): #----- Global path -------# self.global_path = Path() #----- Current Pose ------# self.current_position = Pose2D() #------ Goal ---------# self.navi_goal = None # idx #------- A* -------# # self.state = "stand_by" # "planning" , "finish" , "unreachable", "timeout" self.pq = heapdict() # pq[index] = cost self.came_from = {} # came_from['index'] = index of predesessusor self.is_need_pub = False #----- Parameter (Subject to .launch file) ------# self.DEBUG_DRAW_DEBUG_MAP = True self.TIME_ANALYSE = False self.EXPLORE_ANIMATE_INTERVEL = 0 # 0 means dont show animation self.USE_DIJKSTRA = False self.OBSTACLE_FACTOR = 0.1 # bigger #------- Debug draw -------# if self.DEBUG_DRAW_DEBUG_MAP: self.costs = {} self.debug_map = OccupancyGrid()
def __init__(self,mav): self.mav = mav self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_callback, queue_size=1) self.pose_sub = rospy.Subscriber("/slam_out_pose", PoseStamped, self.pose_callback, queue_size=1) self.map_data = OccupancyGrid() self.pose_data = PoseStamped() self.inflated_grid = [] self.grid_2d = [] self.rectangle_point1 = [5,-5.25] self.rectangle_point2 = [17.5,-5.25] self.rectangle_point3 = [5,5.25] self.rectangle_point4 = [17.5,5.25] #drone radius self.r = 0.4 #safety parameter self.safety = 1 #speed parameters self.speed_multiplier = 5 rospy.wait_for_message("/map", OccupancyGrid) self.n = int((self.r + self.safety)/self.map_data.info.resolution)
def __init__(self): self.ID = 0 self.listener = tf.TransformListener() self.PCD = PointCloud() # occupancyGrid is a row major 1D list, loc = width * Y(Row) + X(Col) self.mapWidth = 500 self.mapHeight = 500 self.RES = 1 ### self.grid_pub = rospy.Publisher( "OccupancyGrid_ANM", OccupancyGrid, queue_size=1) # queue size of length 1 i.e. hold only one object and process it self.grid = OccupancyGrid() self.grid.info.height = self.mapWidth self.grid.info.width = self.mapHeight self.grid.info.origin.position.x = -self.mapWidth / (2 * self.RES) self.grid.info.origin.position.y = -self.mapHeight / (2 * self.RES) self.grid.info.origin.position.z = 0 self.grid.info.origin.orientation.x = 0 self.grid.info.origin.orientation.y = 0 self.grid.info.origin.orientation.z = 0 self.grid.info.origin.orientation.w = 1 self.grid.info.resolution = 1.0 / self.RES self.grid.header.frame_id = "world_ned"
def voronoi(): global newData global width newNewData = list(newData) global pub_voro pub_voro = rospy.Publisher('/voronoi_grid', OccupancyGrid, queue_size=10) for i in range(0, width * width): if localMax(i): newNewData[i] = 0 else: newNewData[i] = 127 print(newNewData) banana = OccupancyGrid() banana.data = newNewData banana.info.width = width banana.info.height = width banana.info.resolution = .05 rospy.sleep(2) pub_voro.publish(banana) pub_voro.publish(banana) pub_voro.publish(banana)
def send_map_ros_msg(landmarks, empty_landmarks, publisher, resolution = 0.01, width = 2048, height = 2048): map_msg = OccupancyGrid() map_msg.header.frame_id = 'map' map_msg.info.resolution = resolution map_msg.info.width = width map_msg.info.height = height data = -np.ones(shape = (width,height)) for ii in range(len(landmarks)): on_x = landmarks[ii,0] // resolution + width // 2 on_y = landmarks[ii,1] // resolution + height // 2 if on_x < width and on_x > 0 and on_y < height and on_y > 0: data[int(on_x), int(on_y)] = 100 for ii in range(len(empty_landmarks)): off_x = empty_landmarks[ii,0] // resolution + width // 2 off_y = empty_landmarks[ii,1] // resolution + height // 2 if off_x < width and off_x > 0 and off_y < height and off_y > 0: data[int(off_x), int(off_y)] = 0 data_out = data.reshape((-1)) map_msg.data = data_out publisher.publish(map_msg)
def dynamic_map_visual(self): # tell imshow about color map so that only set colors are used # pyplot.close() # cmap = matplotlib.colors.ListedColormap(['grey', 'white','black']) # bounds=[-2,0,90,120] # norm = matplotlib.colors.BoundaryNorm(bounds, cmap.N) # img = pyplot.imshow(self.dynamic_map,cmap = cmap,norm=norm) # pyplot.show(block=False) #pyplot.show(block=False) map_dyn = OccupancyGrid() map_dyn.info.width = self.width map_dyn.info.height = self.height map_dyn.info.origin.position.x = -10.0 map_dyn.info.origin.position.y = -10.0 map_dyn.info.origin.position.z = 0.0 map_dyn.info.origin.orientation.x = 0.0 map_dyn.info.origin.orientation.y = 0.0 map_dyn.info.origin.orientation.z = 0.0 map_dyn.info.origin.orientation.w = 1.0 map_dyn.info.resolution = self.resolution dyn = (self.dynamic_map.T).astype(int) map_dyn.data = np.reshape(dyn.tolist(), (self.width * self.height)) self.pub_dyn.publish(map_dyn)
def costMapUpdateCallback(msg): global costMap global costMapWidthUpdate global costMapHeightUpdate global costMapOrigin global newCostMap costMapPub = OccupancyGrid() costMapUpdateData = [] costMap = list(costMap) costMapWidthUpdate = msg.width costMapHeightUpdate = msg.height costMapUpdateData = msg.data startX = msg.x startY = msg.y index = 0 # print "costMap update x: %f y: %f" % (startX,startY) # print "costMap origin is at x: %f y:: %f" %(costMapOrigin.position.x,costMapOrigin.position.y) # print "costMapWidth %f costMapHeight %f" % (costMapWidth,costMapHeight) for y in range(startY, startY + costMapHeightUpdate): for x in range(startX, startX + costMapWidthUpdate): #print "%f %f %f %f" % (len(costMap),costMapWidth*y + x,len(costMapUpdateData),index) if (index < len(costMapUpdateData)): costMap[costMapWidth * y + x] = costMapUpdateData[index] index = index + 1 else: #print" %f %f" %(len(costMapUpdateData),index) break costMapPub.header.frame_id = '/map' costMapPub.header.stamp = rospy.Time.now() costMapPub.info.resolution = costMapRes costMapPub.info.origin = costMapOrigin costMapPub.info.width = costMapWidth costMapPub.info.height = costMapHeight costMapPub.data = costMap costMap_pub.publish(costMapPub) newCostMap = True
def map_callback(self, msg): # Convert the map data into an opencv image img_width = msg.info.width img_height = msg.info.height img = np.asarray(msg.data, dtype='uint8') # Convert all of the -1 spaces (value = 255 when a 'uint8') into obstacle value 100 img[np.where(img == 255)[0]] = 100 img = np.reshape(img, [img_height, img_width, 1]) # Decrease the resolution of the map img_rescaled = cv2.resize(img, None, fx=self.rescale, fy=self.rescale) # Show the image for debugging # cv2.namedWindow('cartographer_map', 2) # cv2.imshow('cartographer_map', img_rescaled) # cv2.waitKey(0) # Convert image back into an occupancy grid message rescaled_height, rescaled_width = img_rescaled.shape rescaled_data = img_rescaled.flatten('C') new_map = OccupancyGrid() new_map.info.width = rescaled_width new_map.info.height = rescaled_height new_map.header.stamp = msg.header.stamp new_map.header.frame_id = msg.header.frame_id new_map.info.map_load_time = msg.info.map_load_time new_map.info.resolution = msg.info.resolution / self.rescale new_map.info.origin.position.x = self.rescale * msg.info.origin.position.x new_map.info.origin.position.y = self.rescale * msg.info.origin.position.y new_map.info.origin.orientation.x = msg.info.origin.orientation.x new_map.info.origin.orientation.y = msg.info.origin.orientation.y new_map.info.origin.orientation.z = msg.info.origin.orientation.z new_map.info.origin.orientation.w = msg.info.origin.orientation.w new_map.data = rescaled_data.astype('int8').tolist() # Publish new map self.low_res_map_pub.publish(new_map)
def generateFrontierDebugMap(self, frontier_msg_list, current_map): local_map_dw = current_map.info.width local_map_dh = current_map.info.height frontiers_index_list_w = [] frontiers_index_list_h = [] # for f_connect in local_frontiers_cell: # for f_cell in f_connect: # frontiers_index_list.append(f_cell[1]*local_map_dw + f_cell[0]) for f_connect_msg in frontier_msg_list: for f_pt in f_connect_msg.frontier: f_cell = ((int)( (f_pt.point.x - current_map.info.origin.position.x) / current_map.info.resolution), (int)( (f_pt.point.y - current_map.info.origin.position.y) / current_map.info.resolution)) frontiers_index_list_h.append(f_cell[1]) frontiers_index_list_w.append(f_cell[0]) frontier_debug_map = OccupancyGrid() frontier_debug_map.header = copy.deepcopy(current_map.header) frontier_debug_map.info = copy.deepcopy(current_map.info) temp_array = np.asarray(current_map.data, dtype=np.int8).reshape(local_map_dh, local_map_dw) temp_array[:] = (int)(-1) temp_array[frontiers_index_list_h, frontiers_index_list_w] = 0 # temp_array = np.zeros((1, local_map_dw*local_map_dh), dtype=np.int8) frontier_debug_map.data = temp_array.ravel().tolist() # for idx in frontiers_index_list: # if int(idx) < 0 or int(idx) > len(frontier_debug_map.data)-1: # continue # frontier_debug_map.data[int(idx)] = 0 # frontier_map_width = frontier_debug_map.info.width # frontier_map_height = frontier_debug_map.info.height return frontier_debug_map
def _receive_message(self, message): global my rospy.loginfo(rospy.get_caller_id() + " Message type %s ", self._message_type) rospy.loginfo( rospy.get_caller_id() + " Time from previous message %s ", (rospy.get_time() - my)) my = rospy.get_time() msg = OccupancyGrid() msg.header.seq = message['header']['seq'] msg.header.stamp = rospy.Time.now( ) #.secs=message['header']['stamp']['secs'] msg.header.frame_id = message['header']['frame_id'] #type unicode msg.info.map_load_time.secs = message['info']['map_load_time']['secs'] msg.info.map_load_time.nsecs = message['info']['map_load_time'][ 'nsecs'] msg.info.resolution = message['info']['resolution'] msg.info.width = message['info']['width'] msg.info.height = message['info']['height'] msg.info.origin.position.x = message['info']['origin']['position']['x'] msg.info.origin.position.y = message['info']['origin']['position']['y'] msg.info.origin.position.z = message['info']['origin']['position']['z'] msg.info.origin.orientation.x = message['info']['origin'][ 'orientation']['x'] msg.info.origin.orientation.y = message['info']['origin'][ 'orientation']['y'] msg.info.origin.orientation.z = message['info']['origin'][ 'orientation']['z'] msg.info.origin.orientation.w = message['info']['origin'][ 'orientation']['w'] msg.data = message['data'] self._rosPub = rospy.Publisher(self._local_topic_name, OccupancyGrid, queue_size=10) self._rosPub.publish(msg) time.sleep(3) self.start()
def __init__(self): # Have ROS initialize this script as a node in rqt. rospy.init_node('camp_map', anonymous=False) # Subscribe to LiDAR. rospy.Subscriber('/scan', LaserScan, self.updateLidarScan) # Subscribe to odometry. rospy.Subscriber('/odom', Odometry, self.updateOdom) # This will publish the computed map information. self.map_publisher = rospy.Publisher('map', OccupancyGrid, queue_size=10) # Initialize important data types. For this script, we need access to the OccupancyGrid produced # by SLAM. We need the Lidar for obstacle detection. We need the odometry for positional data. self.map = OccupancyGrid() self.lidar = LaserScan() self.odom = Odometry() self.map.header.frame_id = "map" self.map.header.seq = 0 self.map.info.resolution = 0.05 self.map.info.width = 100 self.map.info.height = 100 self.map.info.origin.position.x = -self.map.info.width / 2 * self.map.info.resolution self.map.info.origin.position.y = -self.map.info.height / 2 * self.map.info.resolution self.map.info.origin.position.z = 0 self.map.info.origin.orientation.x = 0 self.map.info.origin.orientation.y = 0 self.map.info.origin.orientation.z = 0 self.map.info.origin.orientation.w = 0 data = [] data = [ 50 for i in range(0, self.map.info.width * self.map.info.height) ] self.map.data = data
def image2map(image): img_size = image.shape rows = img_size[0] cols = img_size[1] #print(img_size) grid_map = OccupancyGrid() grid_map.header.frame_id = "map" grid_map.info.width = img_size[0] grid_map.info.height = img_size[1] grid_map.info.resolution = 0.05 width = grid_map.info.width * grid_map.info.resolution height = grid_map.info.height * grid_map.info.resolution grid_map.info.origin.position.x = -width / 2 grid_map.info.origin.position.y = -height / 2 grid_map.info.origin.position.z = 0.0 grid_map.info.origin.orientation.x = 0.0 grid_map.info.origin.orientation.y = 0.0 grid_map.info.origin.orientation.z = 0.0 grid_map.info.origin.orientation.w = 1.0 grid_map.data = [0] * (img_size[0] * img_size[1]) for col in range(cols): for row in range(rows): index = col * rows + row val = image[row, col] if (val == 0): grid_map.data[index] = 100 #occupy elif (val == 127): grid_map.data[index] = -1 #unkown elif (val == 255): grid_map.data[index] = 0 #idle return grid_map
def __init__(self): # Initialization self.map_topic = rospy.get_param('~map_topic', '/map') self.info_radius = rospy.get_param('~info_radius', 0.2) self.frontiers_topic = rospy.get_param('~frontiers_topic', '/filtered_frontiers') self.n_robots = rospy.get_param('~n_robots') self.delay_after_assignement = rospy.get_param( '~delay_after_assignement', 0.2) self.rateHz = rospy.get_param('~rate', 1) self.robot_number = rospy.get_param('~robot_number') self.rate = rospy.Rate(self.rateHz) self.frontiers = PointArray() # Subscribers rospy.Subscriber(self.map_topic, OccupancyGrid, self.map_callback) rospy.Subscriber(self.frontiers_topic, PointArray, self.frontier_callback) # Initialization self.mapData = OccupancyGrid() self.move_base_error_point = {} # Current goals for all robots self.current_goals = {} self.wait_for_map() self.wait_for_frontiers() # Get the name of the robot and creat robots self.append_robot_name() # Rosservice definition and wait for availability self.mission_service = rospy.Service( 'startmission' + str(self.robot_number), RobotService, self.handleStartmission) self.wait_for_all_services() print("Services are ready!")
def publish_map_image(): grid_msg = OccupancyGrid() grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" grid_msg.info.resolution = gp_map.map_res grid_msg.info.width = gp_map.width grid_msg.info.height = gp_map.height grid_msg.info.origin = Pose( Point(gp_map.map_limit[0], gp_map.map_limit[2], 0), Quaternion(0, 0, 0, 1)) flat_grid = gp_map.map.copy() flat_grid = flat_grid.reshape((gp_map.map_size, )) ''' for i in range(gp_map.map_size): if flat_grid[i] > 0.65: flat_grid[i] = 100 elif flat_grid[i] < 0.4: flat_grid[i] = 0 else: flat_grid[i] = -1 ''' flat_grid[np.where(flat_grid < 0.4)] = 0 flat_grid[np.where(flat_grid > 0.65)] = -100 flat_grid[np.where(flat_grid > 0.4)] = 1 #flat_grid = gp_map.threshold(flat_grid) flat_grid = -flat_grid #np.round(-flat_grid) flat_grid = flat_grid.astype(int) grid_msg.data = flat_grid.tolist() occ_map_pub.publish(grid_msg)
def __init__(self): rospy.init_node('RosGridMapping', anonymous=True) self.is_gridmapping_initialized = False self.map_last_publish = rospy.Time() self.prev_robot_x = -99999999 self.prev_robot_y = -99999999 self.sensor_model_p_occ = rospy.get_param('~sensor_model_p_occ', 0.75) self.sensor_model_p_free = rospy.get_param('~sensor_model_p_free', 0.45) self.sensor_model_p_prior = rospy.get_param('~sensor_model_p_prior', 0.5) self.robot_frame = rospy.get_param('~robot_frame', 'base_link') self.map_frame = rospy.get_param('~map_frame', 'map') self.map_center_x = rospy.get_param('~map_center_x', -1.0) self.map_center_y = rospy.get_param('~map_center_y', -1.0) self.map_size_x = rospy.get_param('~map_size_x', 32.0) self.map_size_y = rospy.get_param('~map_size_y', 12.0) self.map_resolution = rospy.get_param('~map_resolution', 0.1) self.map_publish_freq = rospy.get_param('~map_publish_freq', 1.0) self.update_movement = rospy.get_param('~update_movement', 0.1) # Creata a OccupancyGrid message template self.map_msg = OccupancyGrid() self.map_msg.header.frame_id = self.map_frame self.map_msg.info.resolution = self.map_resolution self.map_msg.info.width = int(self.map_size_x / self.map_resolution) self.map_msg.info.height = int(self.map_size_y / self.map_resolution) self.map_msg.info.origin.position.x = self.map_center_x self.map_msg.info.origin.position.y = self.map_center_y self.laser_sub = rospy.Subscriber("scan", LaserScan, self.laserscan_callback, queue_size=2) self.map_pub = rospy.Publisher('map', OccupancyGrid, queue_size=2) self.tf_sub = tf.TransformListener()
def cb_map(self, msg): Omap = OccupancyGrid() Omap.header = msg.header Omap.info = msg.info tmp_map = [0] * 2500 for i in range(2500): tmp_map[i] = msg.data[i] if i % 50 < 25: tmp_map[i] = 100 for y in range(25): x = y / math.atan(math.radians(40)) for i in range(2500): if i < 1250: for xx in range(int(x) + 24): tmp_map[50 * abs(y - 25) + xx] = 100 else: for xx in range(int(x) + 24): tmp_map[50 * abs(y + 24) + xx] = 100 Omap.data = tmp_map self.pub_testmap.publish(Omap) print('Pub Omap')
def publish_map(): # If continuous publishing is needed: # Init publisher (to topic modified_occupancy_grid) pub = rospy.Publisher('modified_occupancy_grid', OccupancyGrid, queue_size=100) rospy.loginfo(rospy.get_caller_id( ) + "\tMap transformer publisher (to modified_occupancy_grid topic) initialized") # Setup grid type, which can be understood by ROS ros_map = OccupancyGrid() first_time = True while not rospy.is_shutdown(): if new_map: # Only when a new map exists: ros_map.data = new_map # Edit meta data of new_map ros_map.info = MapMetaData() ros_map.info.map_load_time = rospy.Time.now() ros_map.info.resolution = SIZE ros_map.info.height = HEIGHT ros_map.info.width = WIDTH ros_map.info.origin = ORIGIN # Edit header ros_map.header = Header() ros_map.header.stamp = rospy.Time.now() # Publish new generated map pub.publish(ros_map) if PRINT_ON: rospy.loginfo(rospy.get_caller_id() + "\tModified occupancy map was published") # Outputs every new map once for the first time: if first_time: print_map() first_time = False rospy.sleep(1)
def __init__(self, robot_name): super().__init__(robot_name + '_robot_map_node') self.publisher_ = self.create_publisher(OccupancyGrid, robot_name + '/robot_map', 10) timer_period = 1 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) self.i = 0 self.robot_name = robot_name self.received_merged_map_yet_ = False # self.navigation_map_pub_ = self.create_publisher(OccupancyGrid, 'robot_map', 10) self.merged_map_sub_ = self.create_subscription( OccupancyGrid, self.robot_name + '/merged_map_debug', self.mergedMapCallback, 10) self.merged_map_sub_ # prevent unused variable warning self.single_map_sub_ = self.create_subscription( OccupancyGrid, self.robot_name + '/inflated_map_debug', self.singleMapCallback, 10) self.single_map_sub_ # prevent unused variable warning self.merged_map_update_ = False self.local_map_update_ = False self.merged_map_msg_ = OccupancyGrid() self.map_merger = MapAndFrontierMerger(self.robot_name)
def __init__(self): rospy.loginfo('Mapping node started, getting parameters') # subscribe to laser scanner reading rospy.Subscriber('~scan', LaserScan, self.laserCallback, queue_size=1) # to publish the map self.pub_map = rospy.Publisher('~map', OccupancyGrid, queue_size=1) # flag to indicate that a topic msg was received self.scan_received = False # to control the frequency at which the node will run self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 10.0)) # the msg to publish the updated map self.updated_map_msg = OccupancyGrid() # to store incomming laser scaner data self.scan_msg = None # flag used to enable/disable laser scanner updates self.lock_scan = False # to store object of inverse range sensor model self.irsm_object = None # map parameters self.map_resolution = None self.map_width = None # to store the sensor pose(query from tf) self.sensor_pose = None # to get and store the pose of the sensor(query from tf) self.listener = tf.TransformListener() self.wait_for_transform = rospy.get_param('~sensor_pose_transform_tolerance', 0.1) # inverse range sensor model params self.obstacle_thickness = rospy.get_param('~obstacle_thickness', 0.08) # nan laser scanner ranges will be replaced with this number self.replace_nan_with = rospy.get_param('~replace_laser_nan_with', 1000.0) # perform map setup one time only, get params, etc.(check function documentation) self.update_map_setup() # compute the length of the map in array self.map_length = self.map_width * self.map_height # sleep to give some time for publishers ans subscribers to register into the network rospy.sleep(0.5)
def publishMapArray(msg): global mapMetaData, header, oldMap msg[msg==5]=50 msg[msg==256]=0 processedMap = OccupancyGrid() data =[] for i in range(len(msg)): #processedMap.data.append(msg[i]) for j in range(len(msg[i])): data.append(int(msg[i][j])) processedMap.info = mapMetaData processedMap.header = header processedMap.data=data pub = rospy.Publisher('ProcessedMap', OccupancyGrid, queue_size = 10) rate = rospy.Rate(10) # 10hz while (not rospy.is_shutdown() and oldMap): str_log = "message sent at: %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(processedMap) rate.sleep() print "publishing..." pub.publish(processedMap) msg[msg==50]=5 msg[msg==0]=256