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
