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 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 __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 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
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 self.horizontal_corner_cut_ratio = 0.3
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"