def setUp(self):
og = OccupancyGrid()
og.info.resolution = 0.5
og.info.height = 20
og.info.width = 20
og.info.origin.position.x = 0.0
og.info.origin.position.y = 0.0
og.info.origin.position.y = 0.0
og.info.origin.orientation.z = 0.0
og.info.origin.orientation.w = 0.0
og.info.origin.orientation.x = 0.0
og.info.origin.orientation.y = 0.0
mapArray = np.array([0] * 400)
mapArray.shape = (20, 20)
for xi in range(20):
for yi in range(20):
if xi < 3 or xi > 15:
mapArray[xi, yi] = 100
if yi < 3 or yi > 15:
mapArray[xi, yi] = 100
if yi < 10 and xi > 10:
mapArray[xi, yi] = 75
mapArray.shape = 400
og.data = list(mapArray)
self.mapData = MapData(og)
def atlaas_grid(filepath, var_threshold=0.1, stamp=None, frame_id="/map"):
""" Usage:
atlaas.merge("atlaas.*x*.tif", "out.tif")
if os.path.isfile("out.tif"):
atlaas_grid("out.tif")
"""
g = gdal2(filepath)
bnp = g.bands[g.names["N_POINTS"]]
bma = g.bands[g.names["Z_MAX"]]
bme = g.bands[g.names["Z_MEAN"]]
bva = g.bands[g.names["VARIANCE"]]
data = numpy.where(bva > var_threshold, bma, bme)
delta = data.max() - data.min()
ddis = (100.0 * (data - data.min()) / delta).astype("uint8")
ddis[bnp < 1] = 0 # no points, unknown
og = OccupancyGrid()
og.data = ddis.flatten()
if stamp:
og.header.stamp = stamp
og.header.frame_id = frame_id
og.info.resolution = g.scale_x
og.info.height, og.info.width = data.shape
og.info.origin.position.x, og.info.origin.position.y = g.u2c(g.utm_x, g.utm_y)
og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0)
return og
def viz_data(grid, closed):
# Visualize DATA
global pub
global grid_res, grid_transform
occ_msg = OccupancyGrid()
occ_msg.info.resolution = grid_res
occ_msg.info.width = len(grid.nodelist)
occ_msg.info.height = len(grid.nodelist[0])
occ_msg.header.frame_id = "map"
occ_msg.info.origin.position.x = grid_transform[0, 3]
occ_msg.info.origin.position.y = grid_transform[1, 3]
quaternion = tf.transformations.quaternion_from_matrix(grid_transform)
occ_msg.info.origin.orientation.x = quaternion[0]
occ_msg.info.origin.orientation.y = quaternion[1]
occ_msg.info.origin.orientation.z = quaternion[2]
occ_msg.info.origin.orientation.w = quaternion[3]
occ_data = [0] * (occ_msg.info.width * occ_msg.info.height)
width = occ_msg.info.width
height = occ_msg.info.height
# occ_data[width * start.x + start.y] = 100
# occ_data[width * goal.x + goal.y] = 100
top_cost = 1
for node in closed:
if node.g_cost > top_cost:
top_cost = node.g_cost
for node in closed:
cost = node.g_cost / top_cost
cost *= 225
if cost > 99:
cost -= 228
occ_data[width * node.x + node.y] = cost
occ_msg.data = occ_data
pub.publish(occ_msg)
def updateCoverage(self):
# Reinitialize coverage map
ogm_shape = self.ogm.shape
self.coverage = numpy.zeros(ogm_shape)
# YOUR CODE HERE ------------------------------------------------------
# Update the coverage field using the self.robot_path veriable.
# We suppose that in every pose the robot covers an area of 2m x 2m
# around it
# 0 is for the uncovered, 100 is for the covered
# PS. Try to make it fast :)
# PS2. Do not have coverage values on obstacles or unknown space!
# If done correctly, the coverage will appear purple in rviz
# ---------------------------------------------------------------------
# Publishing coverage ogm to see it in rviz
coverage_ogm = OccupancyGrid()
coverage_ogm.header.frame_id = "map"
coverage_ogm.info = self.ogm_info
coverage_ogm.data = numpy.zeros(self.ogm_info.width * self.ogm_info.height)
for i in range(0, self.ogm_info.width):
for j in range(0, self.ogm_info.height):
coverage_ogm.data[i + self.ogm_info.width * j] = self.coverage[i][j]
self.coverage_publisher.publish(coverage_ogm)
def setUp(self):
self.bg = BumpGuesser("bump_filter", 1000, (10, 10), 0.8)
og = OccupancyGrid()
og.info.resolution = 0.5
og.info.height = 20
og.info.width = 20
og.info.origin.position.x = 0.0
og.info.origin.position.y = 0.0
og.info.origin.orientation.z = 0.0 # 5 * 2.0**0.5
og.info.origin.orientation.w = 0.0 # 5 * 2.0**0.5
og.info.origin.orientation.x = 0.0
og.info.origin.orientation.y = 0.0
mapArray = np.array([0] * 400)
mapArray.shape = (20, 20)
for xi in range(20):
for yi in range(20):
if xi < 3 or xi > 15:
mapArray[xi, yi] = 100
if yi < 3 or yi > 15:
mapArray[xi, yi] = 100
if yi < 10 and xi > 10:
mapArray[xi, yi] = 75
mapArray.shape = 400
og.data = list(mapArray)
mapData = MapData(og)
self.bg.addMap(mapData)
def map_message(self):
""" Return a nav_msgs/OccupancyGrid representation of this map. """
grid_msg = OccupancyGrid()
grid_msg.header.stamp = rospy.Time.now()
grid_msg.header.frame_id = "map"
grid_msg.info.resolution = self.occ_map.map_res
grid_msg.info.width = self.occ_map.width
grid_msg.info.height = self.occ_map.height
grid_msg.info.origin = Pose(Point(self.occ_map.map_limit[0], self.occ_map.map_limit[2], 0),
Quaternion(0, 0, 0, 1))
flat_grid = copy.deepcopy(self.map.reshape((self.occ_map.map_size,)))
for i in range(self.occ_map.map_size):
if flat_grid[i] > self.occ_map.prob_occ:
flat_grid[i] = 100
elif flat_grid[i] < self.occ_map.prob_free:
flat_grid[i] = 0
else:
flat_grid[i] = -1
flat_grid = np.round(flat_grid)
flat_grid = flat_grid.astype(int)
grid_msg.data = list(flat_grid)
return grid_msg
def read_map(data):
global workingOnMap
workingOnMap = True
print "manually reducing map resolution"
# change the map's resolution
newOccGrid, newMapHeight, newMapWidth = increaseMapResolution(data)
# expand the obstacles
newOccGrid = expandObstacles(newOccGrid, desiredMapRes, newMapWidth, newMapHeight)
# publish the new grid
newPublishedGrid = OccupancyGrid()
newPublishedGrid.info.resolution = desiredMapRes
newPublishedGrid.info.origin.position.x= data.info.origin.position.x
newPublishedGrid.info.origin.position.y= data.info.origin.position.y
newPublishedGrid.info.width = newMapWidth
newPublishedGrid.info.height = newMapHeight
newPublishedGrid.data = tuple(newOccGrid)
print "publishing map"
scaledMapPub.publish(newPublishedGrid)
# publish the amount that the obstacles were expanded by
expansionVal = Float64()
expansionVal.data = expansionWidth
obstacleExpansionDimensionPub.publish(expansionVal)
workingOnMap = False
def load_map(fp):
ogrid = OccupancyGrid()
step = None
offset = None
yaml_file = fp + ".yaml"
csv_file = fp + ".csv"
with open(yaml_file, "r") as infile:
for k, v in yaml.load(infile.read()).iteritems():
if k == "Header":
ogrid.header = v
elif k == "MapMetaData":
ogrid.info = v
step = v.resolution
offset = (v.origin.position.x, v.origin.position.y)
elif k == "GridFile":
print "GridFile: ", v
else:
print "Unexpected k : ", k
raise TypeError("Unexpected key type in yaml file: " + yaml_file)
ogrid.data = list(np.loadtxt(csv_file, dtype=np.int8, delimiter=","))
print "Map loaded"
return ogrid, step, offset
def handleSearchedCombinedMessage(data): global findFrontierPub #get map dimensions data width=data.info.width height=data.info.height searchedMap = data.data frontier = [0] * (width * height) for h in range(height): for w in range(width): if searchedMap[h*width+w] == -1: frontier[h*width+w]=-1 if searchedMap[h*width+w] == 100: if isFrontier(searchedMap,h,w,height,width): frontier[h*width+w]=100 mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=data.header.frame_id mapMsg.info.resolution=data.info.resolution mapMsg.info.width=width mapMsg.info.height=height mapMsg.info.origin=data.info.origin mapMsg.data=frontier findFrontierPub.publish(mapMsg)
def to_message(self):
""" Return a nav_msgs/OccupancyGrid representation of this map. """
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 probalities in the
# range 0-1. This code will need to be modified if the grid
# entries are given a different interpretation (like
# log-odds).
flat_grid = self.grid.reshape((self.grid.size,)) * 100
grid_msg.data = list(np.round(flat_grid))
return grid_msg
def publish(self, publisher, real = False):
costGrid = OccupancyGrid()
costGrid.header.frame_id = self.frame_id
costGrid.header.stamp = rospy.Time.now()
costGrid.info = self.map_info
temparr = copy.deepcopy(self.data) #protect ourselves from volitility
if not real:
# map cost_map to between 0 and 127 for fancy colors in rviz
maxVal = max(temparr)
minVal = float('inf')
for cost in temparr:
if not (cost == 0) and (cost < minVal): minVal = cost
minVal *= 1.01
factor = 90.0/(maxVal - minVal)
if(maxVal == minVal): return
# costGrid.data = [(int((i - minVal) * factor) if (i != 0) else 0) for i in cost_map.data]
costGrid.data = []
for i in temparr:
if(i!=0):
costGrid.data.append(int((i - minVal) * factor))
else:
costGrid.data.append(0)
else: costGrid.data = temparr
publisher.publish(costGrid)
def pubMap(self):
# Publish updated map
grid_msg = OccupancyGrid()
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.map_info.width
grid_msg.info.height = self.map_info.height
# Rotated maps are not supported... quaternion represents no
# rotation.
grid_msg.info.origin = Pose(Point(self.map_origin[0], self.map_origin[1], 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 probalities in the
# range 0-1. This code will need to be modified if the grid
# entries are given a different interpretation (like
# log-odds).
flat_grid = self.grid.reshape((self.grid_map.size,)) * 100
grid_msg.data = list(np.round(flat_grid))
self._map_data_pub.publish(grid_msg.info)
self._map_pub.publish(grid_msg)
return grid_msg
def occ_grid_cb(data):
global p,ready,first
rospy.loginfo("map_reduction.py:occ_grid_cb() /ready_for_wps: %s",str(ready))
if not ready.data:
return
nav_map = OccupancyGrid()
w,h = data.info.width,data.info.height
pt = data.data
p = array(pt).reshape(w,h)#.T #<-- Transposes
if not 'grid_loc' in globals():
return
x,y = grid_loc.position.x, grid_loc.position.y
nav_map.info = data.info
if not 'size' in globals():
size = 64
if 'rtl' in globals() and rtl:
##Intelligently set mapsize, scale map, and send waypoints to home
pass
nav_map.info.width = size
nav_map.info.height = size
tmp_map = p[int(x)-size/2:int(x)+size/2,int(y)-size/2:int(y)+size/2]
if first:
tmp_map[:,:size/2] = 100
nav_map.data = tmp_map.flatten()
nav_map_pub.publish(nav_map)
def processOccupancyGrid(gridMessage, scaleFactor, cacheEmptyCells):
grid = OccupancyGrid(gridMessage.data, gridMessage.info.height, gridMessage.info.width, gridMessage.info.resolution,
(gridMessage.info.origin.position.x, gridMessage.info.origin.position.y))
grid.scale(scaleFactor, cacheEmptyCells=cacheEmptyCells)
grid.expandObstacles()
return grid
def updateCombined():
global data0
global data1
global data2
global mapData
global searchedCombinePub
rospy.wait_for_message('robot_0/robotSearched', OccupancyGrid, timeout=None)
rospy.wait_for_message('robot_1/robotSearched', OccupancyGrid, timeout=None)
rospy.wait_for_message('robot_2/robotSearched', OccupancyGrid, timeout=None)
mapMsg=OccupancyGrid()
while not rospy.is_shutdown():
map0 = numpy.array(data0.data)
map1 = numpy.array(data1.data)
map2 = numpy.array(data2.data)
combined = numpy.minimum(map0,map1)
combined = numpy.minimum(combined,map2)
mapMsg.header.stamp=rospy.Time.now()
mapMsg.header.frame_id=mapData.header.frame_id
mapMsg.info.resolution=mapData.info.resolution
mapMsg.info.width=mapData.info.width
mapMsg.info.height=mapData.info.height
mapMsg.info.origin=mapData.info.origin
mapMsg.data=combined.tolist()
searchedCombinePub.publish(mapMsg)
rospy.sleep(1.0)
def run(self):
while self.is_looping():
navigation_tf_msg = TFMessage()
try:
motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
localization = self.navigation.getRobotPositionInMap()
exploration_path = self.navigation.getExplorationPath()
except Exception as e:
navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
self.publishers["map_tf"].publish(navigation_tf_msg)
self.rate.sleep()
continue
if len(localization) > 0 and len(localization[0]) == 3:
navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
self.publishers["map_tf"].publish(navigation_tf_msg)
if len(localization) > 0:
if self.publishers["uncertainty"].get_num_connections() > 0:
uncertainty = Marker()
uncertainty.header.frame_id = "/base_footprint"
uncertainty.header.stamp = rospy.Time.now()
uncertainty.type = Marker.CYLINDER
uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
uncertainty.pose.position = Point(0, 0, 0)
uncertainty.pose.orientation = self.get_ros_quaternion(
almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
self.publishers["uncertainty"].publish(uncertainty)
if self.publishers["map"].get_num_connections() > 0:
aggregated_map = None
try:
aggregated_map = self.navigation.getMetricalMap()
except Exception as e:
print("error " + str(e))
if aggregated_map is not None:
map_marker = OccupancyGrid()
map_marker.header.stamp = rospy.Time.now()
map_marker.header.frame_id = "/map"
map_marker.info.resolution = aggregated_map[0]
map_marker.info.width = aggregated_map[1]
map_marker.info.height = aggregated_map[2]
map_marker.info.origin.orientation = self.get_ros_quaternion(
almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
map_marker.info.origin.position.x = aggregated_map[3][0]
map_marker.info.origin.position.y = aggregated_map[3][1]
map_marker.data = aggregated_map[4]
self.publishers["map"].publish(map_marker)
if self.publishers["exploration_path"].get_num_connections() > 0:
path = Path()
path.header.stamp = rospy.Time.now()
path.header.frame_id = "/map"
for node in exploration_path:
current_node = PoseStamped()
current_node.pose.position.x = node[0]
current_node.pose.position.y = node[1]
path.poses.append(current_node)
self.publishers["exploration_path"].publish(path)
self.rate.sleep()
def get_message(self):
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def pub_grid(self, *args):
grid = self.grid_drawer.img
ogrid = OccupancyGrid()
ogrid.header.frame_id = '/enu'
ogrid.header.stamp = rospy.Time.now()
ogrid.info = self.map_meta_data
ogrid.data = np.subtract(np.flipud(grid).flatten(), 1).astype(np.int8).tolist()
self.ogrid_pub.publish(ogrid)
def publishCombined(self): #Enter Main Loop while not rospy.is_shutdown(): #Convert to Numpy Arrays map = [] for i in range(0, self.numRobots): map.append(numpy.array(self.searchedData[i].data)) combined2 = map[0] if self.numRobots > 1: #Find Minimum of all maps for i in range(1, self.numRobots): combined2 = numpy.minimum(combined2,map[i]) #Pack Occupancy Grid Message mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=self.mapData.header.frame_id mapMsg.info.resolution=self.mapData.info.resolution mapMsg.info.width=self.mapData.info.width mapMsg.info.height=self.mapData.info.height mapMsg.info.origin=self.mapData.info.origin mapMsg.data=combined2.tolist() #Convert combined Occupancy grid values to grayscal image values combined2[combined2 == -1] = 150 #Unknown -1->150 (gray) combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white) #Searched=0 (black) #Calculate percentage of open area searched numNotSearched = combined2[combined2==255].size numSearched = combined2[combined2==0].size percentSearched = 100*float(numSearched)/(numNotSearched+numSearched) percentSearchedMsg = Float32() percentSearchedMsg.data = percentSearched self.percentPub.publish(percentSearchedMsg) #Pack Image Message imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = self.mapData.header.frame_id imageMsg.height = self.mapData.info.height imageMsg.width = self.mapData.info.width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = self.mapData.info.width imageMsg.data = combined2.tolist() #Publish Combined Occupancy Grid and Image self.searchedCombinePub.publish(mapMsg) self.imagePub.publish(imageMsg) #Update Every 0.5 seconds rospy.sleep(1.0)
def convert_to_occ_grid(self):
res = OccupancyGrid()
res.header.stamp = rospy.Time.now()
res.header.frame_id = "odom_combined"
res.info.resolution = 0.1
res.info.width = 1000
res.info.height = 1000
res.info.origin.position.x = -50
res.info.origin.position.y = -50
res.data = self.grid
return res
def to_message(self):
""" Return a nav_msgs/OccupancyGrid representation of this map. """
grid_msg = OccupancyGrid()
grid_msg.header.stamp = rospy.Time.now()
grid_msg.header.frame_id = "map"
grid_msg.info.resolution = self.resolution
grid_msg.info.width = self.width
grid_msg.info.height = self.height
grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0), Quaternion(0, 0, 0, 1))
grid_msg.data = self._numpy_to_data()
return grid_msg
def publish_grid_map():
grid_map = OccupancyGrid()
grid_map.info.map_load_time = rospy.Time.now()
grid_map.info.width = slam_map.size
grid_map.info.height = slam_map.size
grid_map.info.resolution = slam_map.scale
grid_map.info.origin.position.x = -slam_map.size / 2 * slam_map.scale
grid_map.info.origin.position.y = -slam_map.size / 2 * slam_map.scale
grid_map.info.origin.position.z = 0
grid_map.data = slam_map.to_ros_grid()
pub.publish(grid_map)
def grilla2occupancy_grid(self):
mapa_msg = OccupancyGrid()
mapa_msg.header.frame_id = 'mapa'
mapa_msg.info.resolution = self.resolucion
mapa_msg.info.width = self.dimension_x
mapa_msg.info.height = self.dimension_y
mapa_msg.data = range(self.dimension_x * self.dimension_y)
for i in range(len(self.grilla.flat)):
mapa_msg.data[i] = self.grilla.flat[i]
return mapa_msg
def updateMap(e_o_indexes, w_map):
newGrid = OccupancyGrid()
newGrid.info = w_map.info
tmp_data = list(w_map.data)
for index in e_o_indexes:
if index >= 0 and index < len(w_map.data):
tmp_data[index] = 100
newGrid.data = tuple(tmp_data)
return newGrid
def publishListAsOccupancyGrid(list, publisher):
# TODO: does this need to be a deep copy?
occupancyGrid = OccupancyGrid()
occupancyGrid.info.height = G_GridRows
occupancyGrid.info.width = G_GridCols
occupancyGrid.info.resolution = G_GridResolution
occupancyGrid.info.origin.position.x = G_GridOriginX
occupancyGrid.info.origin.position.y = G_GridOriginY
occupancyGrid.info.origin.orientation.w = 1
occupancyGrid.header.frame_id = "map"
occupancyGrid.data = tuple(list)
publisher.publish(occupancyGrid)
def mapResize(newRes, mapInfo, mapData):
oldRes = mapInfo.resolution
oldw = mapInfo.width
oldh = mapInfo.height
oldMapInfo = copy.deepcopy(mapInfo)
nMapData = []
nMapDataD = {}
#change the size of the new map. The offsets of the new map are already copied from the old map.
mapInfo.resolution = newRes
mapInfo.width = int(round( (oldw*(oldRes/newRes)) ))
mapInfo.height = int(round( (oldh*(oldRes/newRes)) ))
#print "old map dimensions"
#print oldMapInfo.width
#print oldMapInfo.height
#print "new map dimensions w x h"
#print mapInfo.width
#print mapInfo.height
#populate the new map at the defined resolution with all cells at -1
#print "generating the new map"
for x in range(mapInfo.width * mapInfo.height):
nMapDataD[x] = 0
#print "summing new blocks"
for i in range(len(mapData)):
gp = gridToGlobal(indexToGrid(i, oldMapInfo), oldMapInfo)
nIndex = gridToIndex(globalToGrid(gp, mapInfo), mapInfo)
if nIndex >= (mapInfo.width * mapInfo.height -1):
nIndex = mapInfo.width * mapInfo.height-1
nMapDataD[nIndex] = (nMapDataD[nIndex] + mapData[i])
for i in nMapDataD:
value = nMapDataD[i] / ((newRes*newRes) - (oldRes * oldRes))
if value > 5:
value = 100
elif value < 0:
value = -1
else:
value = 0
nMapData.append(numpy.int8(value))
newMapOC = OccupancyGrid()
newMapOC.info = mapInfo
newMapOC.data = tuple(nMapData)
return newMapOC
def to_message(self):
""" Return a nav_msgs/OccupancyGrid representation of this map. """
grid_msg = OccupancyGrid()
grid_msg.header.stamp = rospy.Time.now()
grid_msg.header.frame_id = "map"
grid_msg.info.resolution = self.resolution
grid_msg.info.width = self.width
grid_msg.info.height = self.height
grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0),
Quaternion(0, 0, 0, 1))
grid_msg.data = self._numpy_to_data()
return grid_msg
def main():
# Inicializar el nodo de ROS y asignarle un nombre
rospy.init_node('nodo_publicador')
# Publicador de mensajes
pub = rospy.Publisher('/mygrid', OccupancyGrid, queue_size=10)
# Crear el mensaje a enviar (sin contenido)
msg = OccupancyGrid()
# Frecuencia con la que se envia el mensaje (en Hz)
rate = rospy.Rate(10)
# Contador
cnt = 0
# Tamano
w = 640
h = 480
# Datos del mensaje
msg.header.stamp = rospy.Time.now()
msg.info.width = w
msg.info.height = h
msg.info.resolution = 0.005
msg.info.origin.position.x = 0.0
msg.info.origin.position.y = 0.0
msg.info.origin.position.z = 0.0
msg.info.origin.orientation.x = 0.0
msg.info.origin.orientation.y = 0.0
msg.info.origin.orientation.z = 0.0
msg.info.origin.orientation.w = 1.0
msg.header.frame_id = 'mygrid'
# Valores
n = w * h
mymap = n * [
10.,
]
msg.data = mymap
# Bucle principal
while not rospy.is_shutdown():
# Ingresar los datos del mensaje
# if (cnt%20 == 0):
# mensaje.linear.x = -mensaje.linear.x
# mensaje.linear.y = 0.0
# mensaje.linear.z = 0.0
# mensaje.angular.x = 0.0
# mensaje.angular.y = 0.0
# mensaje.angular.z = 0.0
# Publicar el mensaje
pub.publish(msg)
# Mostrar el mensaje
# print(mensaje)
# Incrementar el contador
# cnt += 1
# Delay del bucle para que cumpla la frecuencia indicada
rate.sleep()
def publish_occupancy_grid(self):
grid = OccupancyGrid()
grid.data = []
grid.data = self.map.ravel().astype(int).tolist()
grid.header.stamp = rospy.Time.now()
grid.header.frame_id = '/world'
grid.info.height = np.float(self.map.shape[0])
grid.info.width = np.float(self.map.shape[1])
grid.info.resolution = self.res
grid.info.origin.position.x = -self.y_shape*self.res+self.initial_pose[1].astype(np.float)
grid.info.origin.position.y = -self.initial_pose[0].astype(np.float) #
self.map_publisher.publish(grid)
def listToMap(listMap, header, metadata):
grid = OccupancyGrid()
grid.info = metadata
grid.header = header
grid.header.frame_id='/map'
# For each row
for rowNumber in range(len(listMap)):
# For each column
for columnNumber in range(len(listMap[rowNumber])):
grid.data.append(listMap[rowNumber][columnNumber])
return grid
def publish_map(self):
now = self.get_clock().now()
msg = OccupancyGrid()
msg.header.stamp = now.to_msg()
msg.header.frame_id = 'map'
msg.info.resolution = RESOLUTION
msg.info.width = MAP_WIDTH
msg.info.height = MAP_HEIGHT
msg.info.origin.position.x = WORLD_ORIGIN_X
msg.info.origin.position.y = WORLD_ORIGIN_Y
msg.data = self.map
self.map_publisher.publish(msg)
def atlaas8u_grid(filepath, stamp=None, frame_id='/map'):
g = gdal2(filepath)
b = g.bands[0] if len(g.bands.shape) > 2 else g.bands
og = OccupancyGrid()
og.data = (b.astype('float') / 2.55).astype('uint8').flatten()
if stamp: og.header.stamp = stamp
og.header.frame_id = frame_id
og.info.resolution = g.scale_x
og.info.height, og.info.width = b.shape
og.info.origin.position.x, og.info.origin.position.y = g.u2c(
g.utm_x, g.utm_y)
og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0)
return og
def publish_score(self, obs_map):
SCALE = 255. / 100
grid = OccupancyGrid()
grid.header.stamp = rospy.Time.now()
grid.header.frame_id = 'map'
grid.header.seq = self.seq
grid.info = self.map_info
score_array = obs_map[::-1, :, M.SCORE] / SCALE
score_list = list(map(int, score_array.reshape(-1)))
grid.data = score_list
self.score_pub.publish(grid)
def mergemaps(self, object_map, hiddenpref_map):
merged = OccupancyGrid()
merged.header.frame_id = "map"
merged.info = self._static_map.info
for y in range(object_map.info.height):
for x in range(object_map.info.width):
v = (object_map.data[y * object_map.info.width + x] *
hiddenpref_map.data[y * hiddenpref_map.info.width + x])
#v = v/100
merged.data.append(v)
if not self.overlaps:
merged.data = map(lambda c: 100 if c > 0 else 0, merged.data)
self.mergemap_pub.publish(merged)
def atlaas8u_grid(filepath, stamp=None, frame_id="/map"):
g = gdal2(filepath)
b = g.bands[0] if len(g.bands.shape) > 2 else g.bands
og = OccupancyGrid()
og.data = (b.astype("float") / 2.55).astype("uint8").flatten()
if stamp:
og.header.stamp = stamp
og.header.frame_id = frame_id
og.info.resolution = g.scale_x
og.info.height, og.info.width = b.shape
og.info.origin.position.x, og.info.origin.position.y = g.u2c(g.utm_x, g.utm_y)
og.info.origin.orientation.x = 1 # flip to transform UTM-ROS (scale_y < 0)
return og
def publish_visibility_grid(self):
visible_grid = OccupancyGrid()
visible_grid.header.stamp = rospy.Time.now()
visible_grid.header.frame_id = "map"
visible_grid.info.resolution = self.height / self.division
visible_grid.info.width = self.division
visible_grid.info.height = self.division
visible_grid.info.origin.orientation.w = 1
visible_grid.data = [
self.convert(self.is_grid_active[x][y])
for y in range(self.division) for x in range(self.division)
]
self.pub_visibility_grid.publish(visible_grid)
def mapResize(newRes, mapInfo, mapData):
oldRes = mapInfo.resolution
oldw = mapInfo.width
oldh = mapInfo.height
oldMapInfo = copy.deepcopy(mapInfo)
nMapData = []
nMapDataD = {}
#change the size of the new map. The offsets of the new map are already copied from the old map.
mapInfo.resolution = newRes
mapInfo.width = int(round((oldw * (oldRes / newRes))))
mapInfo.height = int(round((oldh * (oldRes / newRes))))
#print "old map dimensions"
#print oldMapInfo.width
#print oldMapInfo.height
#print "new map dimensions w x h"
#print mapInfo.width
#print mapInfo.height
#populate the new map at the defined resolution with all cells at -1
#print "generating the new map"
for x in range(mapInfo.width * mapInfo.height):
nMapDataD[x] = 0
#print "summing new blocks"
for i in range(len(mapData)):
gp = gridToGlobal(indexToGrid(i, oldMapInfo), oldMapInfo)
nIndex = gridToIndex(globalToGrid(gp, mapInfo), mapInfo)
if nIndex >= (mapInfo.width * mapInfo.height - 1):
nIndex = mapInfo.width * mapInfo.height - 1
nMapDataD[nIndex] = (nMapDataD[nIndex] + mapData[i])
for i in nMapDataD:
value = nMapDataD[i] / ((newRes * newRes) - (oldRes * oldRes))
if value > 5:
value = 100
else:
value = 0
nMapData.append(numpy.int8(value))
newMapOC = OccupancyGrid()
newMapOC.info = mapInfo
newMapOC.data = tuple(nMapData)
return newMapOC
def get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow')
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = navigator_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def __init__(self):
print 'status init'
self.map = OccupancyGrid()
self.sharedmap = OccupancyGrid()
self.mission = StringStamped()
self.missionext = StringStamped()
self.robotpose = PoseWithCovarianceStamped()
self.robotposeext = PoseWithCovarianceStamped()
self.robotstatus = StringStamped()
self.robotstatusext = StringStamped()
self.cloud = PointCloud()
self.cloud_ext = PointCloud()
self.image = Image()
self.image_ext = Image()
def publish_test_map(self, points, metadata, map_header):
"""
For testing purposes, publishes a map highlighting certain points.
points is a list of tuples (x, y) in the map's coordinate system.
"""
test_map = np.zeros((metadata.height, metadata.width))
for x, y in points:
test_map[y, x] = 100
test_map_msg = OccupancyGrid()
test_map_msg.header = map_header
test_map_msg.header.stamp = rospy.Time.now()
test_map_msg.info = metadata
test_map_msg.data = list(np.ravel(test_map))
self.test_map_pub.publish(test_map_msg)
def __init__(self, posx, posy, block):
if posx:
self.pos = Odometry()
self.map_data = OccupancyGrid()
self.goal = PoseStamped()
self.pos.pose.pose.position.x = posx
self.pos.pose.pose.position.y = posy
self.map_data.info.origin.position.x = -1.0
self.map_data.info.origin.position.y = -1.0
self.map_data.info.resolution = 0.0500000007451
self.map_data.info.width = 1024
self.map_data.info.height = 640
self.goal.header.frame_id = 'map'
self.goal.pose.position.x = self.pos.pose.pose.position.x
self.goal.pose.position.y = self.pos.pose.pose.position.y + 2.0
self.goal.pose.orientation.z = 0.7
self.goal.pose.orientation.w = 0.7
self.yaw = pi / 2
self.block = block
#self.coord = {}
self.counter = -1
self.counter0 = 1
self.counter1 = 0
self.start = time.time() - 5
#self.lastx = self.pos.pose.pose.position.x
#self.lasty = self.pos.pose.pose.position.y
#self.lastyaw = self.yaw
else:
self.pos = Odometry()
self.map_data = OccupancyGrid()
self.goal = PoseStamped()
self.pos.pose.pose.position.x = -5.0
self.pos.pose.pose.position.y = -22.0
self.map_data.info.origin.position.x = -31.4
self.map_data.info.origin.position.y = -31.4
self.map_data.info.resolution = 0.0500000007451
self.map_data.info.width = 1056
self.map_data.info.height = 800
self.goal.header.frame_id = 'map'
self.goal.pose.position.x = self.pos.pose.pose.position.x
self.goal.pose.position.y = self.pos.pose.pose.position.y + 2.0
self.goal.pose.orientation.z = 0.7
self.goal.pose.orientation.w = 0.7
self.yaw = pi / 2
self.block = 'X -5.0Y -22.0'
#self.coord = {}
self.counter = -1
self.counter0 = 1
self.counter1 = 0
self.start = time.time() - 5
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def update_map(grid, update):
xgrid = OccupancyGrid()
xgrid.header.stamp = update.header.stamp
xgrid.header.frame_id = update.header.frame_id
xgrid.info = grid.info
xgrid.data = grid.data[:]
i = 0
for yi in range(update.height):
for xi in range(update.width):
index = (update.y+yi)*xgrid.info.width + xi + update.x
xgrid.data[index] = update.data[i]
i+=1
return xgrid
def make_ogrid(self, np_arr, size, center):
np_center = np.array(center)
np_origin = np.append((np_center - size / 2)[:2], 0)
origin = mil_tools.numpy_quat_pair_to_pose(np_origin, [0, 0, 0, 1])
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame='enu')
ogrid.info.origin = origin
ogrid.info.width = size / self.resolution
ogrid.info.height = size / self.resolution
ogrid.info.resolution = self.resolution
ogrid.data = np.clip(np_arr.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def get_message(self):
if self.grid is None:
fprint("Ogrid was requested but no ogrid was found. Using blank.", msg_color='yellow')
self.grid = np.zeros((self.height / self.resolution, self.width / self.resolution))
ogrid = OccupancyGrid()
ogrid.header = mil_tools.make_header(frame="enu")
ogrid.info.resolution = self.resolution
ogrid.info.height, ogrid.info.width = self.grid.shape
ogrid.info.origin = self.origin
grid = np.copy(self.grid)
ogrid.data = np.clip(grid.flatten() - 1, -100, 100).astype(np.int8)
return ogrid
def publish_map(self,data,map_ori):
data[np.where(data==1)]=100
data[np.where(data==-1)]=0
mapg=OccupancyGrid()
mapg.header.frame_id = "map"
mapg.header.stamp = rospy.Time.now()
mapg.info.resolution=self.map_reso
mapg.info.width=len(data)
mapg.info.height=len(data[0])
mapg.info.origin=self.origin
mapg.info.origin.position.x=map_ori[0]
mapg.info.origin.position.y=map_ori[1]
mapg.data=list(data.T.reshape(mapg.info.width*mapg.info.height,1)[:,0]) #T?
self.map_pub.publish(mapg)
def publish_grid(self):
'''
Take the occupancy grid and send it out over ros with timestamps and whatnot.
'''
t = rospy.Time.now()
header = Header(stamp=t, frame_id='/map')
# Populate occ grid msg
occ_msg = OccupancyGrid()
occ_msg.header = header
occ_msg.info = self.meta_data
# Make sure values don't go out of range
occ_grid = self.searched + self.markers - 1
occ_msg.data = np.clip(occ_grid.flatten(), -1, 100)
self.occ_grid_pub.publish(occ_msg)
def __init__(self):
# CONSTANT for Control varialbe range ---------------------------------------------------#
self.MAX_speed = 5.5 # [m/s]
self.MIN_speed = 0 # [m/s]
self.MAX_steer = 2000 / float(71) # [degree]
self.MIN_steer = -2000 / float(71) # [degree]
self.MAX_brake = 150
self.MIN_brake = 1
self.NO_CONTROL = 0
self.EMERGENCY_BRAKE = 1
self.NORMAL_S = 2 # Normal path following mode
#----------------------------------------------------------------------------------------#
# Comunication rate
self.pub_rate = 1.0 # [Hz] #IMPORTANT!!! Main control rate
self.access_wait_rate = 500 # [Hz]
# Map varialbe
self.latest_map = OccupancyGrid() # Last updated map
self.cur_map = OccupancyGrid() # Curent map of car
self.upsign_map = False # if updated map is different, then 1
# Hardware constant
self.L_wheelbase = 1.02 # [m] distance between two wheel axis
self.L_rear = 0.51 # [m] distance between cm and rear wheel
# Control variable
self.control = Control() # My order to the car
self.control_mode = self.NO_CONTROL
self.writing_state = 0
self.control_count = 0
self.control_buff = []
self.control_time_buff = []
self.control_buff_size = 600 #TODO: Tune this!
self.look_ahead = 20 #TODO: Tune this! now we choose 20th point as look_ahead_point
self.regular_speed = 2 #TODO[m/s]: Tune this!
self.k_gain = 0.28
self.look_ahead_point = PoseStamped
# Input variable
self.cur_state = Control() # Current state of car
self.upsign_state = False # if updated state is different, then 1
self.latest_path = Path() # Last updated path
self.cur_path = Path() # Current path of car
self.upsign_path = False # if updated path is different, then 1
def update_grid(self, msg):
xyz_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
#print(xyz_array)
#pose = self._tf_buffer.lookup_transform(self._fixed_frame, self._sensor_frame, rospy.Time())
#ros_pcl transform_pointcloud might be more efficient than transforming eadh point
for i in range(len(xyz_array)):
if xyz_array[i, 2] <= HEIGHT_THRESHOLD:
grid_point = self.PointToVoxel(xyz_array[i, 0], xyz_array[i,
1])
self._map += self._occupied_update
g = OccupancyGrid()
g.data = (self.LogOddsToProbability(self._map) * 100).astype(np.int8)
self._grid_pub.publish(g)
def initiateMapMsg(self):# written by Ambjorn Waldum
map_msg = OccupancyGrid()
map_msg.header.frame_id = 'manta/odom'
resolution = 0.1
width = 500
height = 500
map_msg.info.resolution = resolution
map_msg.info.width = width
map_msg.info.height = height
map_msg.data = np.arange(width*height, dtype=np.int8)
map_msg.info.origin.position.x = - width // 2 * resolution
map_msg.info.origin.position.y = - height // 2 * resolution
map_msg.header.stamp = rospy.Time.now()
return map_msg
def publish_grid(self):
'''
Take the occupancy grid and send it out over ros with timestamps and whatnot.
'''
t = rospy.Time.now()
header = Header(stamp=t, frame_id='/map')
# Populate occ grid msg
occ_msg = OccupancyGrid()
occ_msg.header = header
occ_msg.info = self.meta_data
# Make sure values don't go out of range
occ_grid = self.searched + self.markers - 1
occ_msg.data = np.clip(occ_grid.flatten(), -1, 100)
self.occ_grid_pub.publish(occ_msg)
def handle_og_request(self, og_request):
og = self.cleaningManager.mapManager.occupancy_grid
OG = OccupancyGrid()
altered_og = []
for val in og:
if val == -1:
altered_og.append(val)
else:
altered_og.append(int(val * 100 - 70))
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
#OG.info.origin = self.__get_og_origin()
zed_quat = self.cleaningManager.mapManager.mapMaker.orientation
zed_euler = self.quaternion_to_euler(zed_quat[0], zed_quat[1],
zed_quat[2], zed_quat[3])
o_quat = self.euler_to_quaternion(zed_euler[2], zed_euler[1],
zed_euler[0] + math.radians(180))
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[0][0]
og_origin.position.y = visible_corners[0][1]
og_origin.orientation.x = o_quat[0]
og_origin.orientation.y = o_quat[1]
og_origin.orientation.z = o_quat[2]
og_origin.orientation.w = o_quat[3]
OG.info.origin = og_origin
baseBotPose = Pose()
baseBotPose.position.x = self.cleaningManager.mapManager.mapMaker.translation[
0]
baseBotPose.position.y = self.cleaningManager.mapManager.mapMaker.translation[
1]
baseBotPose.position.z = self.cleaningManager.mapManager.mapMaker.translation[
2]
baseBotPose.orientation.x = self.cleaningManager.mapManager.mapMaker.orientation[
0]
baseBotPose.orientation.y = self.cleaningManager.mapManager.mapMaker.orientation[
1]
baseBotPose.orientation.z = self.cleaningManager.mapManager.mapMaker.orientation[
2]
baseBotPose.orientation.w = self.cleaningManager.mapManager.mapMaker.orientation[
3]
return [baseBotPose, OG]
def test_astar(self):
global_planner = GlobalPlanner()
# Info for testing a star alone
nmap = numpy.array([[0, 0, 254, 254, 0, 0], [0, 0, 100, 100, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 50, 50, 0, 0],
[0, 0, 50, 50, 0, 0]])
start_coord = (0, 0)
end_coord = (4, 5)
# Info for testing make_plan
occupancy_grid = OccupancyGrid()
occupancy_grid.header.frame_id = "/map"
occupancy_grid.info.resolution = 0.5
occupancy_grid.info.width = 6
occupancy_grid.info.height = 5
occupancy_grid.data = nmap.flatten()
start_pose = PoseStamped()
start_pose.pose.position.x = float(start_coord[0])
start_pose.pose.position.y = float(start_coord[1])
end_pose = PoseStamped()
end_pose.pose.position.x = (
float(end_coord[0]) + 0.5) * occupancy_grid.info.resolution # 2.25
end_pose.pose.position.y = (
float(end_coord[1]) + 0.5) * occupancy_grid.info.resolution # 2.75
# Results and check
path = global_planner.astar(nmap, start_coord, end_coord)
ros_path = global_planner.make_plan(occupancy_grid, start_pose,
end_pose)
# global_planner.print_path(nmap, path) # Uncomment to ease debugging
expected_path = [(0, 0), (1, 0), (2, 0), (3, 0), (4, 0), (4, 1),
(4, 2), (4, 3), (4, 4), (4, 5)]
expected_ros_path = [(0, 0), (0.75, 0.25), (1.25, 0.25), (1.75, 0.25),
(2.25, 0.25), (2.25, 0.75), (2.25, 1.25),
(2.25, 1.75), (2.25, 2.25), (2.25, 2.75)]
self.assertEqual(path, expected_path)
for pose, expected_position in zip(ros_path.poses, expected_ros_path):
# print(pose) # Uncomment to ease debugging
self.assertEqual(pose.pose.position.x, expected_position[0])
self.assertEqual(pose.pose.position.y, expected_position[1])
def pub_occupancy(self):
occupancy = []
for row in self.obstacle_map.log_prob_map:
for i in row:
try:
occupancy.append(math.exp(i) * 100)
except OverflowError:
rospy.logwarn('OverflowError: %s', i)
occupancy.append(100)
msg = OccupancyGrid()
msg.info.resolution = self.grid_size
msg.info.width = self.width
msg.info.height = self.height
msg.data = occupancy
self.occupancy_pub.publish(msg)
def map_callback(self,msg):
"""
receives new map info and inflates the map
"""
old_map = np.array(msg.data)
side_length = int(round(np.sqrt(old_map.shape[0])))
old_map = old_map.reshape((side_length, side_length))
print(old_map)
new_map = grey_dilation(old_map, size=(13,13))
new_map = new_map.reshape(side_length**2).tolist()
print(len(new_map))
new_msg = OccupancyGrid()
new_msg.data = new_map
new_msg.info = msg.info
self.pub.publish(new_msg)
def initialize_occ_grid(frame_id="odom",
res=1.0,
width=100,
height=100,
origin=Pose(Point(0.0, 0.0, 0.0),
Quaternion(0.0, 0.0, 0.0, 1.0))):
occ_grid = OG()
occ_grid.header.frame_id = frame_id
occ_grid.info.resolution = res
occ_grid.info.width = width
occ_grid.info.height = height
occ_grid.info.origin = origin
occ_grid.data = occ_grid.info.width * occ_grid.info.height * [50]
return occ_grid
def publish_test_map(self, points, metadata, map_header):
#Broken, about to fix.
"""
For testing purposes, publishes a map highlighting certain points.
points is a list of tuples (x, y) in the map's coordinate system.
"""
test_map = np.zeros((metadata.height, metadata.width),dtype = np.uint8)
for x, y in points:
test_map[y, x] = 100
test_map_msg = OccupancyGrid()
test_map_msg.header = map_header
test_map_msg.header.stamp = self.get_clock().now().to_msg()
test_map_msg.info = metadata
test_map_msg.data = list(np.ravel(test_map))
self.test_map_pub.publish(test_map_msg)
def configure_msg(self):
if not self.mapIsParsed:
data = self.parse_and_configure_map()
msg = OccupancyGrid()
msg.header.frame_id = self.yml['image']
msg.header.stamp = self.get_clock().now().to_msg()
msg.info.map_load_time = self.get_clock().now().to_msg()
msg.info.origin.position.x = self.yml['origin'][0]
msg.info.origin.position.y = self.yml['origin'][1]
msg.info.resolution = round(self.yml['resolution'], 4)
msg.info.height = self.map_shape[0]
msg.info.width = self.map_shape[1]
msg.data = data.tolist()
return msg
def getMapSrv(self):
map_ = OccupancyGrid()
map_.header.stamp = rospy.Time.now()
map_.header.frame_id = 'map'
map.info.resolution = self.ground_resolution
map_.info.width = self.width
map_.info.height = self.height
map_.info.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
flat_map = self.map.reshape((self.self.map.size, ))
map_.data = list(np.round(flat_map))
return GetMapResponse(map_)
