def callback(data): global ranges global thetasens print 'camera' thetasens = np.linspace(angle_min, angle_max, 640) ranges = [] for a in range(0, 639): ranges.append(data.ranges[a]) mymap = OccupancyGrid(header=Header(seq=0, stamp=rospy.Time.now(), frame_id="map"), info=MapMetaData(width=100, height=100, resolution=0.1, map_load_time=rospy.Time.now())) mymap_thresh = OccupancyGrid(header=Header(seq=0, stamp=rospy.Time.now(), frame_id="map"), info=MapMetaData( width=100, height=100, resolution=0.1, map_load_time=rospy.Time.now())) k = -2 for i in range(0, 100): m = -2 for j in range(0, 100): test = inverse_range_sensor_model(m, k, positionx, positiony, yaw, ranges) if test != 0: occupancy[i][j] = occupancy[i][j] + test mymap.data.append(logodds(occupancy[i][j]) * 100) if logodds(occupancy[i][j]) > 0.75: mymap_thresh.data.append(100) elif logodds(occupancy[i][j]) < 0.25: mymap_thresh.data.append(0) else: mymap_thresh.data.append(50) else: mymap.data.append(logodds(occupancy[i][j]) * 100) if logodds(occupancy[i][j]) > 0.75: mymap_thresh.data.append(100) elif logodds(occupancy[i][j]) < 0.25: mymap_thresh.data.append(0) else: mymap_thresh.data.append(50) m = m + 0.1 k = k + 0.1 publisher = rospy.Publisher('/map', OccupancyGrid, queue_size=1) publisher2 = rospy.Publisher('/map2', OccupancyGrid, queue_size=1) rate = rospy.Rate(10) publisher.publish(mymap) publisher2.publish(mymap_thresh)
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 __init__(self, res, width, height, starting_pose, topic_name='/search_grid'): self.meta_data = MapMetaData() # Resolution is m/cell. Width is X, height is Y. self.meta_data.resolution = res # rospy.get_param("map_resolution") self.meta_data.width = width # rospy.get_param("map_width") self.meta_data.height = height # rospy.get_param("map_height") # Starting Position self.mid_x = -starting_pose.x self.mid_y = -starting_pose.y p = Point(x=-starting_pose.x * res, y=-starting_pose.y * res, z=0) q = Quaternion(*tf.transformations.quaternion_from_euler( 0, 0, starting_pose.theta)) self.meta_data.origin = Pose(position=p, orientation=q) rospy.Service('/reset_occ_grid', Empty, self.reset_grid) # Create array of -1's of the correct size self.occ_grid = np.zeros( (self.meta_data.height, self.meta_data.width)) - 1 self.searched = np.zeros((self.meta_data.height, self.meta_data.width)) self.markers = np.zeros((self.meta_data.height, self.meta_data.width)) self.occ_grid_pub = rospy.Publisher(topic_name, OccupancyGrid, queue_size=1) self.searcher = Searcher(self.searched, res, [self.mid_x, self.mid_y])
def __call__(self, msg): costmap = self._handler.costmap() if costmap is not None: cells = costmap.cells rows, cols = cells.shape cells = cv2.resize(cells, (rows / self._visualization_scale, cols / self._visualization_scale)) origin = costmap.origin / self._visualization_scale resolution = costmap.resolution * self._visualization_scale msg = OccupancyGrid() msg.header.frame_id = "map" msg.header.stamp = costmap.stamp msg.info = MapMetaData( map_load_time=rospy.Time.now(), resolution=resolution, width=cells.shape[0], height=cells.shape[1], origin=Pose(position=Point(origin[1] * resolution, origin[0] * resolution, 0), orientation=Quaternion(0, 0, 0, 1))) data = cells.flatten() data_out = np.full(data.shape, 100) mask = data <= 0.99 data_out[mask] = (data[mask] * 98 + 0.5).astype(int) msg.data = data_out try: self._pub.publish(msg) except rospy.ROSException as e: rospy.logdebug("GridMap2DListener: {}".format(e))
def __init__(self): self.map_data = MapMetaData() self.trace_pub = rospy.Publisher('/raytrace_output', occupancy_update, queue_size=5) self.meta_sub = rospy.Subscriber('/map_meta_data', MapMetaData, callback=self.NewMetaData) self.vis_pub = rospy.Publisher('/raytrace_cloud', PointCloud, queue_size=5) self.pose_sub = rospy.Subscriber('/ekf_est', PoseStamped, callback=self.pose_callback) self.laser_sub = rospy.Subscriber('/scan', LaserScan, callback=self.laser_callback) self.tf_listener = tf.TransformListener() #time.sleep(10.0) self.pose = None self.have_pose = False self.scan = None self.have_scan = False self.recieved_data = False
def __init__(self): rospy.init_node('camera_observed_area_tracker') rospy.loginfo('Camera observed area tracker node started') self.pose_converted = PoseConverted() self.pose = Pose() self.map = [[]] self.map_info = MapMetaData() self.map_camera_seen_initialised = False self.map_camera = [[]] self.map_camera_seen_seq = 0 self.robot_x_old = 0 self.robot_y_old = 0 self.robot_yaw_old = 0 # --- Publisher --- self.pub_seen_map = rospy.Publisher('camera_seen_map', OccupancyGrid, queue_size=1) # --- Subscriber --- self.pose_subscriber = rospy.Subscriber('simple_odom_pose', CustomPose, self._handle_pose_update) self.map_sub = rospy.Subscriber('map', OccupancyGrid, self._handle_map_update) self._setup_camera_seen_masks() rospy.loginfo("--- ready ---") rospy.spin()
def __init__(self): rospy.on_shutdown(self._shutdown) rospy.init_node('tag_manager') self.abs_file_path = rospy.get_param( "~tag_file", default= "/home/daniel/catkin_ws/src/tag_manager/StoredTag/tags.store") self.tag_list = [] self.tag_detection_radius = 10 self._load_tags() self.map_info = MapMetaData() self.markerArray = MarkerArray() # --- Publisher --- self.marker_publisher = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=100) # --- Services --- self.check_tag_known_service = rospy.Service( 'check_tag_known', CheckTagKnown, self._handle_check_tag_known) self.add_tag_service = rospy.Service('add_tag', AddTag, self._handle_add_tag) self.get_tags_service = rospy.Service('get_tags', GetTags, self._handle_get_tags) self._setup() rospy.spin()
def __init__(self, map_name): self.name = map_name self.raw_msg = OccupancyGrid() # OccupancyGrid message goes here self.map_meta = MapMetaData() # MapMetaData self.map = np.array([]) self.map_im = np.array([]) self.roi = np.array([]) self.frontiers = [] self.poi = [] self.debug_markers = [] self.map_is_loaded = False # ROI self.roi_origin = Pose() self.roi_max = Point() self.roi_min = Point() # HDBSCAN self.clusterer = hdbscan.HDBSCAN(min_cluster_size=50) # parameters self.occupancy_thresh = 50 # % probability of occupancy before considering as occupied. self.map_roi_boundary = 5 # extra cells to keep around known map # Plotting stuff # sns.set_context('poster') # sns.set_color_codes() # main map window cv2.namedWindow('map_im', cv2.WINDOW_NORMAL)
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 getOccupancyGrid(matrix, scale): stamp = rospy.Time.now() oc = OccupancyGrid() oc.header.frame_id = HEADER_FRAME oc.header.stamp = stamp # The time at which the map was loaded load_time = stamp # The origin of the map [m, m, rad]. This is the real-world pose of the # cell (0,0) in the map. origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 0)) matrix = matrix[::-1] width = len(matrix[0]) height = len(matrix) oc.info = MapMetaData(load_time, scale, width, height, origin) for row in range(height): for col in range(width): c = matrix[row][col] if c == 0: oc.data.append(100) elif c == 1: oc.data.append(0) else: oc.data.append(50) return oc, width * scale, height * scale
def __init__(self): #Declare the variables required for the program self.map = OccupancyGrid() self.metadata = MapMetaData() self.pose = PoseWithCovarianceStamped() self.point = Point() self.droneArray = [] self.NUMBER_OF_ILLETERATIONS = 1000 self.pheromonePath = [] rospy.loginfo("Starting aco_ros...") ##########################Drone Objects############################# self.myTopics = [[]] #Expect a double array in rospyPubList self.myTopics = rospy.get_published_topics() matching = [element for element in self.myTopics if "/droneLaser" in element] matching.sort() droneNumber = len(matching) #Match the amount of drones to the amount of laserscans we found #rospy.loginfo(str(droneNumber)) #################Subscribers for maps ############################## rospy.Subscriber("/map",OccupancyGrid, self.callForMap) #rospy.Subscriber("/map_metadata",MapMetaData,self.callForMetadata) rospy.Subscriber("/initialpose",PoseWithCovarianceStamped, self.callFinalPose) rospy.Subscriber("/goal",Point,self.callPoint) ####################################################Run calcuations if (droneNumber != 0): i = 1 #counter while (i != droneNumber): #Instantciate an array of drone Objects self.droneArray[i] = Drone(i,"drone_"+str(i),True,False,self.point,"/myDrone") #Set drone name and set the sensors that work at the moment rospy.loginfo("Found drone" + str(i)) i += 1 self.main() else: rospy.loginfo("No drones to spawn!")
def __init__(self): rospy.init_node('turtlebot_state_machine', anonymous=True) self.mode = Mode.EXPLORE # current nav cmd self.cmd_nav = Pose2D() # map self.map = OccupancyGrid() self.map_meta_data = MapMetaData() self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max) self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max) self.heading_controller = HeadingController(self.kp_th, self.om_max) # publishers self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=10) self.map_meta_data_pub = rospy.Publisher('/map_meta_data', MapMetaData, queue_size=10) self.cmd_nav_pub = rospy.Publisher('/cmd_nav',Pose2D, queue_size=10) # subscribers #rospy.Subscriber('/map', OccupancyGrid, self.map_callback) #rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback) #rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback) print "finished init"
def __init__(self): # Initialize an empty message self.data = -np.ones((1, 1)) self.info = MapMetaData() self.info.width = 1 self.info.height = 1 # Initialize space for the dilated grid self.grid_dilated = np.zeros((1, 1), dtype=np.int8) # Publish a list of occupied points self.occupied_pub = rospy.Publisher(self.OCCUPIED_TOPIC, Marker, queue_size=1) # Publish to the map self.map_pub = rospy.Publisher(self.CARTOGRAPHER_DILATED_TOPIC, numpy_msg(OccupancyGrid), queue_size=1) # Subscribe to the map self.map_sub = rospy.Subscriber(self.CARTOGRAPHER_TOPIC, numpy_msg(OccupancyGrid), self.map_cb, queue_size=1)
def heightmap_msg(heightmap, metadata): """ Build the heightmap from the given numpy file and metadata For now, say that each elem in occupancy grid = 1cm """ msg = OccupancyGrid() msg.header.frame_id = '/map' hmap_int = (heightmap * 100).astype(np.uint8) msg.data = hmap_int.flatten() msg.info = MapMetaData(resolution=metadata['resoluion'], width=metadata['width'], height=metadata['height'], origin=Pose( position=Point( x=metadata['origin']['position']['x'], y=metadata['origin']['position']['y'], z=metadata['origin']['position']['z'], ), orientation=Quaternion( x=metadata['origin']['orientation']['x'], y=metadata['origin']['orientation']['y'], z=metadata['origin']['orientation']['z'], w=metadata['origin']['orientation']['w'], ))) assert len(msg.data) == msg.info.width * msg.info.height, 'BAD' return msg
def __init__(self): rospy.init_node('explorer', anonymous=True) self.is_navigating = False self.is_searching_unknown_space = False self.robot_pose_available = False self.robot_radius = 1 self.blowUpCellNum = 3 self.map_info = MapMetaData() self.map_complete = False self.map_complete_print = False self.map_camera_complete = False self.all_maps_complete_printed = False self.waypoints = [] self.waypointsAvailable = False self.pose = Pose() self.pose_converted = PoseConverted() self.folder = rospy.get_param( "~map_folder", default= "/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/mymap/") # --- Subscriber --- rospy.loginfo('setup subscriber') self.pose_subscriber = rospy.Subscriber('simple_odom_pose', CustomPose, self._handle_update_pose) # --- Service wait --- rospy.loginfo('wait find_unkown_service') rospy.wait_for_service('find_unkown_service') rospy.loginfo('wait find_unseen_service') rospy.wait_for_service('find_unseen_service') # --- service connect --- rospy.loginfo('setup services') self.find_unknown_service = rospy.ServiceProxy('find_unkown_service', FindUnknown) self.find_unseen_service = rospy.ServiceProxy('find_unseen_service', FindUnseen) # --- Action server connect --- rospy.loginfo('action client') self.client = actionlib.SimpleActionClient('path_drive_server', PathDriveAction) self.client.wait_for_server() rospy.loginfo('setup') self._setup() rospy.loginfo('ready') self.rate = rospy.Rate(20)
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from std_msgs.msg import Header self.header = kwargs.get('header', Header()) from nav_msgs.msg import MapMetaData self.info = kwargs.get('info', MapMetaData()) self.data = array.array('b', kwargs.get('data', []))
def __init__(self): rospy.init_node('PRM_node') # Initial pose as the start location, and goal (stored as strings) self.start = None self.goal = None # Map self.map = None self.map_meta_data = MapMetaData() # Robot radii -NOT diameter. Added in buffer self.robot_width = .45 + .1 self.robot_height = .3 + .1 # List of robot points for checking for valid points self.robot_points = [] # Gain for how much to weigh orientation #self.c = .1 # How many verticies do we want to randomly generate self.K = 10000 # How far away can we connect points (in cells) self.max_rad = 25 self.far = 10 # graph is a dictionary # each key is the pose (string form) # each entry contains a list of keys that it has edges with self.g = {} self.blank_graph = {} # Path object self.path = Path() # For processing the PRM points self.first = True # For drawing the PRM in RViz self.prm_points = PointCloud() # Publishers self.path_pub = rospy.Publisher('/path', Path, queue_size=1) self.prm_pub = rospy.Publisher('/prm', PointCloud, queue_size=1) self.start_goal_pub = rospy.Publisher('/start_goal', Odometry, queue_size=1) # Subscribers self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.setMap) self.pose_sub = rospy.Subscriber('/robot0/odom', Odometry, self.setPose) self.goal_sub = rospy.Subscriber('/goal', Pose2D, self.createPlan) rospy.spin()
def numpyToOcc(grid, origMsg): msg = OccupancyGrid() msg.header.frame_id = "map" msg.data = grid.ravel() msg.info = MapMetaData() msg.info.height = grid.shape[0] msg.info.width = grid.shape[1] msg.info.resolution = 0.05 msg.info.origin = origMsg.info.origin return msg
def numpyToOcc(self, grid): msg = OccupancyGrid() msg.header.frame_id = "base_link" msg.data = grid.ravel() msg.info = MapMetaData() msg.info.height = self.h msg.info.width = self.w msg.info.resolution = self.res msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) return msg
def numpyToOcc(grid, res): msg = OccupancyGrid() msg.header.frame_id = "map" msg.data = grid.ravel() msg.info = MapMetaData() msg.info.height = grid.shape[0] msg.info.width = grid.shape[1] msg.info.resolution = res msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0,0,0,1)) return 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): rospy.init_node('mapping_node') # Mapping Constants self.origin = Pose() self.origin.position.x = -25. self.origin.position.y = -25. self.origin.orientation.w = 1. self.map_info = MapMetaData() self.map_info.map_load_time = rospy.Time.now() self.map_info.resolution = .1 self.map_info.height = 1000 self.map_info.width = 1000 self.map_info.origin = self.origin # Sensor Model self.alpha = 0.9 self.beta = 0.7 # Transform(s) # sensor to robot self.T_sensor = np.array([[np.cos(0), -1*np.sin(0), 0.27], [np.sin(0), np.cos(0), 0.0], [0.0, 0.0, 1.0]]) # robot to world self.T_robot = np.array([[np.cos(0), -1*np.sin(0), 0.0], [np.sin(0), np.cos(0), 0.0], [0.0, 0.0, 1]]) # Occupancy grid variable (array) # occ_grid is a numpy matrix # Prefill with 0.5 for unknown occupancy self.occ_grid = np.repeat(np.matrix(np.repeat(.5, self.map_info.width)), self.map_info.height, axis=0) # ros_occ_grid is a ROS OccupancyGrid instance self.ros_occ_grid = OccupancyGrid() self.ros_occ_grid.info = self.map_info # Publishers self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=1) # Subscribers # pose_sub and scan_sub are synchronized. # when the updateMap callback is executed it receives a message from both. pose_sub = message_filters.Subscriber('/groundtruth', Odometry) scan_sub = message_filters.Subscriber('/r2d2/laser/scan', LaserScan) ts = message_filters.TimeSynchronizer([pose_sub, scan_sub], 10) ts.registerCallback(self.updateMap) rospy.spin()
def start(): # Initialize publisher and node pub = rospy.Publisher('map', OccupancyGrid, queue_size=1) rospy.init_node('occupancy_publish') # Create empty map map_generated = np.zeros((2000, 2000, 3), np.uint8) resolution = 0.01 # Draw lines on the map points = parse_points(file_name='room_318.txt') points_normilized = [(np.float32(x[0] / resolution), np.float32(x[1] / resolution)) for x in points] font = cv2.FONT_HERSHEY_SIMPLEX lines = [[0, 1], [1, 23], [23, 22], [22, 21], [21, 20], [20, 19], [19, 18], [18, 17], [17, 11], [11, 13], [12, 13], [12, 10], [10, 9], [9, 0], [4, 6], [6, 14], [14, 16], [16, 8], [8, 2], [2, 3], [3, 15], [15, 5], [5, 4]] # for i in range(len(points_normilized)-1): # cv2.putText(map_generated,str(i),points_normilized[i], font, 1, (200,255,155), 2, cv2.LINE_AA) # cv2.line(map_generated, points_normilized[i], points_normilized[i+1], (255, 255, 255), thickness=1) for i in range(len(lines)): # cv2.putText(map_generated,str(i),points_normilized[i], font, 1, (200,255,155), 2, cv2.LINE_AA) cv2.line(map_generated, points_normilized[lines[i][0]], points_normilized[lines[i][1]], (255, 255, 255), thickness=1) map_generated = cv2.flip(map_generated, 0) map_generated = cv2.flip(map_generated, 1) map_generated_gray = cv2.cvtColor(map_generated, cv2.COLOR_BGR2GRAY) # cv2.namedWindow('image', cv2.WINDOW_NORMAL) # cv2.imshow('image', map_generated_gray) # cv2.waitKey(delay=0) print(map_generated_gray) # Convert map in appropriate form map_generated_flat = list(map_generated_gray.flatten()) map_generated_flat[:] = [x / 255.0 for x in map_generated_flat] map_generated_flat[:] = [x * 100.0 for x in map_generated_flat] map_width, map_height = map_generated_gray.shape # Create an Occupancy Grid message map_msg = OccupancyGrid(info=MapMetaData(width=map_width, height=map_height, resolution=resolution), data=map_generated_flat) # ROS working loop rate = rospy.Rate(10) while not rospy.is_shutdown(): pub.publish(map_msg) rate.sleep()
def get_new_occupancy_grid(self): from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import MapMetaData from roslib.msg import Header from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion import random r = random.Random(1234) return OccupancyGrid(Header(0, rospy.Time(0,0), "/map"), MapMetaData(rospy.Time(123,456), 0.1, 100, 100, Pose(Point(1.23, 4.56, 7.89), Quaternion(0,0,0,1))), [r.randint(-1,100) for x in xrange(0,1000)])
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 getROSOccupancyMap(self, resolution): # First figure out how large the occupancy grid needs to be in cells widthInCells = math.ceil(self.extent[0] / resolution)) heightInCells = int(math.ceil(self.extent[1] / resolution)) # Create the occupancy grid object rosMAP = OccupancyGrid() # Assign the map meta data rosMap.info = MapMetaData() rosMAP.info.resolution = resolution rosMAP.info.width = uint(widthInCells) rosMAP.info.height = uint(heightInCells) rosMAP.info.pose = new Pose() ROSMAP(widthInCells, heightInCells, resolution) # First add a border. This stops STDR falling off the edge of the world. # Don't have time to work out what the proper Python iterator is for this. for y in range(heightInCells): occupancyGrid.grid[0][y] = 1 occupancyGrid.grid[widthInCells-1][y] = 1 for x in range(1, widthInCells-1): occupancyGrid.grid[x][0] = 1 occupancyGrid.grid[x][heightInCells] = 1 # Rasterise into cells. This can be SLOW print 'Rasterising workspace into cells' lastPercentPrint = 10 print 'Finished 0.0%' for x in range(widthInCells): percentProgress = math.floor(100.0 * x / widthInCells) if (percentProgress > lastPercentPrint): print 'Finished ' + str(percentProgress) + '%' lastPercentPrint = lastPercentPrint + 10 for y in range(heightInCells): xl = x * resolution + 1e-3 xr = xl + resolution - 2e-3 yb = y * resolution + 1e-3 yt = yb + resolution - 2e-3 cellPolygon = Polygon(((xl, yb), (xr, yb), (xr, yt), (xl, yt), (xl, yb))) if self.polygonInFreeSpace(cellPolygon, ignoreBorder = True) == True: occupancyGrid.grid[x][y] = 0 else: occupancyGrid.grid[x][y] = 1 return occupancyGrid
def test_add_map_and_set_active(self): im = Image.new('L', (800, 800)) d = ImageDraw.Draw(im) d.rectangle(((100, 100), (600, 600)), fill=100) d.rectangle(((300, 300), (350, 350)), fill=60) d.rectangle(((200, 200), (400, 400)), fill=0) b = BytesIO() im.save(fp=b, format='PNG') png_map_bytes = b.getvalue() map_info = MapMetaData(map_load_time=rospy.Time(0), resolution=0.02, width=800, height=800, origin=Pose(position=Point(x=-10, y=-10, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1))) map_name = 'test_map' add_map_srv = rospy.ServiceProxy('/map_manager/add_map', AddMap) add_map_srv.wait_for_service(timeout=10) request = AddMapRequest( map=Map(info=MapInfo(name=map_name, meta_data=map_info), markers=[ Marker(name='marker_{}'.format(i), pose=Pose(position=Point(x=i, y=i), orientation=Quaternion(w=1))) for i in range(10) ]), occupancy_grid=CompressedImage(format='png', data=png_map_bytes), map_data=[1, 2, 3, 4]) rospy.loginfo('Adding a Map') response = add_map_srv.call(request) self.assertIsInstance(response, AddMapResponse) self.assertTrue(response.success, msg=response.message) set_map_srv = rospy.ServiceProxy('/map_manager/set_active_map', SetActiveMap) set_map_srv.wait_for_service(timeout=10) rospy.loginfo('Setting active map') response = set_map_srv.call(SetActiveMapRequest(map_name=map_name)) self.assertIsInstance(response, SetActiveMapResponse) self.assertTrue(response.success, msg=response.message) md = rospy.wait_for_message('/map_manager/map_data', UInt8MultiArray, timeout=10) # type: UInt8MultiArray md_d = struct.unpack('<%sb' % len(md.data), md.data) self.assertTrue( all(int(a) == int(b) for a, b in zip(md_d, request.map_data)))
def slam_map(data): #slam_msg = OccupancyGrid() nav_pixel_msg = MapMetaData() resolution = data.info.resolution #print("Resolution:",resolution) width = nav_pixel_msg.width height = nav_pixel_msg.height # for i in range(len(slam_msg.data)): x = width * resolution + resolution / 2 y = height * resolution + resolution / 2
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 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 locateFrontierPt( self, laserscanIn, mapIn ): # escapePath is a bool that deternimes if this is a escape routine myScan = LaserScan() myLocation = Pose() mymap = OccupancyGrid() myLocation = MapMetaData() mymap = mapIn myScan = laserscanIn myLocation = mymap.info.origin width = mymap.info.width densityOccupation = [] densityClusters = [] # Generate a radius of density around the robot for point in range(0, 667 - 1, 1): rospy.loginfo(len(myScan.ranges)) x_other = myLocation.position.x + myScan.ranges[point] * math.cos( self.angles + myLocation.orientation.z) y_other = myLocation.position.y + myScan.ranges[point] * math.sin( self.angles + myLocation.orientation.z) #rospy.loginfo(myScan.ranges[point]) point = x_other + width * y_other #rospy.loginfo(point) if (not math.isinf(point) and not math.isnan(point)): densityOccupation.append(int(point)) self.angles += 0.35982 # Locate a midpoint of density #rospy.loginfo(densityOccupation) for mapPoint in densityOccupation: if (mymap.data[mapPoint] > 0): densityClusters.append(0) else: densityClusters.append(1) #rospy.loginfo(densityClusters) points = [] for point in densityClusters: # Convert all possible "walkable" points into real points instead of map points. if (point == 1): myPoint = self.convertMapArrayIntoXY(point, width) points.append(myPoint) #rospy.loginfo(len(points)) if ( len(points) < 100 ): # We want to verify that we can walk foward if we can possibly can do so. return "Blocked", False else: return points, self.firstStart # Returns an array of points