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 publishOccupancyGrid(self):
grid = self.senseOccupancyGrid()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'odom'
map_meta_data = MapMetaData()
map_meta_data.map_load_time = rospy.Time.now()
map_meta_data.resolution = grid_resolution # each cell is 15cm
map_meta_data.width = int(grid_size / grid_resolution)
map_meta_data.height = int(grid_size / grid_resolution)
map_meta_data.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
grid_msg = OccupancyGrid()
grid_msg.header = header
grid_msg.info = map_meta_data
grid_msg.data = list(grid)
self.occupancyGridPublisher.publish(grid_msg)
def 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 local_grid_callback(self, msg):
grid_size = msg.info.width
self.local_grid = np.flipud(
np.reshape(msg.data, (grid_size, grid_size)))
# Transform center of grid in robot coordinate frame to map frame
grid_origin = PointStamped(
msg.header,
Point(self.camera_offset[0] + grid_size * self.resolution / 2,
self.camera_offset[1], 0))
try:
grid_origin = self.tf_buffer.transform(grid_origin, "map",
rospy.Duration(0.1))
rot = self.tf_buffer.lookup_transform(
"base_link", "map", msg.header.stamp,
rospy.Duration(0.1)).transform.rotation
except Exception as e:
rospy.logwarn(e)
return
grid_origin = np.array([grid_origin.point.x, grid_origin.point.y])
robot_angle = R.from_quat([rot.x, rot.y, rot.z,
rot.w]).as_euler('zyx')[0]
if self.save_data:
print('Saving...')
np.save(
'%s/%d.npy' % (self.data_dir, len(os.listdir(self.data_dir))),
self.local_grid)
np.save(
'%s/%d.npy' %
(self.data_dir + 'localization',
len(os.listdir(self.data_dir + 'localization'))),
np.array(
[self.robot_x[-1], self.robot_y[-1],
self.robot_pitch[-1]]))
indices = np.indices(self.local_grid.shape)
mask = abs(indices[1] - self.local_grid.shape[1] / 2) < (
self.local_grid.shape[0] - indices[0])
mask = np.reshape(mask, (grid_size, grid_size))
self.local_grid[~mask] = -1
# Rotate image using degrees for some dumb reason
rotated_grid = ndimage.rotate(
self.local_grid,
(-robot_angle * 180 / np.pi + self.camera_offset[2]),
mode='constant',
cval=-1,
reshape=True) # rotate grid by camera + robot angle
# convert to grid indices:
grid_origin = grid_origin / self.resolution
# flip due to coordinate system of matrix being top left
# Switch local origin from center of image to top left based on new width of rotated image
grid_origin = [
int(
np.round(self.global_grid_shape[0] - grid_origin[1] -
rotated_grid.shape[0] / 2)),
int(np.round(grid_origin[0] - rotated_grid.shape[1] / 2))
]
# Account for map origin not being the 0,0 cell
grid_origin = [
grid_origin[0] - self.map_buffer, grid_origin[1] + self.map_buffer
]
counts = np.ones_like(
rotated_grid) # Count how many times each cell was measured
counts[rotated_grid == -1] = -1
counts = self.paste(self.global_counts, counts, grid_origin)
roi = (counts != -1)
counts[~roi] = 0
global_additions = self.paste(self.global_totals, rotated_grid,
grid_origin)
window_size = 100
self.global_totals[roi] = (self.global_totals[roi] * np.minimum(self.global_counts[roi], window_size) + global_additions[roi]) \
/ (np.minimum(self.global_counts[roi], window_size) + 1)
self.global_counts += counts
# self.global_grid = np.divide(self.global_totals, self.global_counts, out=np.zeros_like(self.global_totals), where=self.global_counts!=0)
# self.global_grid = np.nan_to_num(self.global_grid, copy=False)
# self.global_grid /= np.max(self.global_grid) # ensure values are 0-1
self.global_grid = ndimage.maximum_filter(self.global_totals,
size=6,
mode='nearest')
self.global_grid[self.global_grid >= 50] = 100
self.global_grid = ndimage.gaussian_filter(self.global_grid, sigma=1.5)
self.global_grid[self.global_grid >= 50] = 100
self.global_grid[self.global_totals >= 50] = 150 + self.global_totals[
self.global_totals >= 50]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
map_meta_data = MapMetaData()
map_meta_data.map_load_time = rospy.Time.now()
map_meta_data.resolution = self.resolution
map_meta_data.width = self.global_grid_shape[1]
map_meta_data.height = self.global_grid_shape[0]
map_meta_data.origin = Pose(
Point(-self.map_buffer * self.resolution,
-self.map_buffer * self.resolution, 0),
Quaternion(0, 0, 0, 1))
grid_msg = OccupancyGrid()
grid_msg.header = header
grid_msg.info = map_meta_data
grid_msg.data = list((np.int8(np.flipud(self.global_grid).flatten())))
try:
pub = rospy.Publisher("global_occupancy_grid",
OccupancyGrid,
queue_size=1)
pub.publish(grid_msg)
except rospy.ROSInterruptException as e:
print(e.getMessage())
pass
if self.save_imgs:
fig = plt.figure(figsize=(15, 5))
ax = plt.subplot(131)
ax.imshow(self.local_grid, cmap='Reds', vmin=0, vmax=100)
ax.set_title('Local Occupancy Grid')
ax = plt.subplot(132)
ax.imshow(rotated_grid, cmap='Reds', vmin=0, vmax=100)
ax.set_title('Rotated Occupancy Grid')
ax = plt.subplot(133)
ax.imshow(self.global_grid, cmap='Reds', vmin=0, vmax=100)
ax.set_title('Global Occupancy Grid')
fig.savefig('%s/%d' %
(self.viz_dir, len(os.listdir(self.viz_dir))))
plt.close()
def getMap(self, wall_objects):
if len(wall_objects) == 0:
return None
now = rospy.Time.now()
half_wall_length = self.wall_size.x / 2.0
#TODO: add/subtract half the length from each object as go along
min_x = min([wall['pose'].position.x for wall in wall_objects])
min_y = min([wall['pose'].position.y for wall in wall_objects])
max_x = max([wall['pose'].position.x for wall in wall_objects])
max_y = max([wall['pose'].position.y for wall in wall_objects])
min_x -= half_wall_length
min_y -= half_wall_length
max_x += half_wall_length
max_y += half_wall_length
map_origin = Pose()
map_origin.position.x = min_x
map_origin.position.y = min_y #
map_origin.orientation.w = 1 # .505
map_origin.orientation.z = 0 #.505
map_width = int((max_x - min_x) / self.resolution)
map_height = int((max_y - min_y) / self.resolution)
empty_map = self.OCC_UNKNOWN * np.ones(shape=(map_width, map_height),
dtype=np.int8)
map_image = empty_map
#pts = np.array(shape=(len(wall_poses),2),dtype=np.int32)
#pose_ind = 0
for wall in wall_objects:
pose = wall['pose']
x_im = pose.position.y - min_y
y_im = pose.position.x - min_x
quat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
angle = transformations.euler_from_quaternion(quat)[2]
wall_size = wall['size']
half_wall_length = wall_size.x / 2.0
wall_thickness = int(wall_size.y / self.resolution)
p1x = x_im + math.sin(angle) * half_wall_length
p2x = x_im - math.sin(angle) * half_wall_length
p1y = y_im + math.cos(angle) * half_wall_length
p2y = y_im - math.cos(angle) * half_wall_length
p1x = int(p1x / self.resolution)
p2x = int(p2x / self.resolution)
p1y = int(p1y / self.resolution)
p2y = int(p2y / self.resolution)
cv2.line(img=map_image,
pt1=(p1x, p1y),
pt2=(p2x, p2y),
color=self.OCC_OBSTACLE,
thickness=wall_thickness)
#pts.append(((p1x,p1y),(p2x,p2y)))
#cv2.imshow(winname="map",mat=map_image)
#cv2.waitKey(10)
#cv2.polylines(img=map_image,pts=pts,isClosed=False,color=self.OCC_OBSTACLE,thickness=wall_thickness)
#cv2.imshow(winname="map",mat=map_image)
#cv2.waitKey(1)
map_msg = OccupancyGrid()
map_msg.header.frame_id = self.gazebo_frame_id
map_msg.header.stamp = now
map_msg.data = np.reshape(a=map_image, newshape=-1, order='F').tolist(
) #First flatten the array, then convert it to a list
metadata = MapMetaData()
metadata.resolution = self.resolution
metadata.map_load_time = now
metadata.width = map_width
metadata.height = map_height
metadata.origin = map_origin
map_msg.info = metadata
return map_msg
def gridmap_publisher():
gridmap_pub = rospy.Publisher('gridmap', OccupancyGrid, queue_size=1)
pose_pub = rospy.Publisher('pose',
geometry_msgs.msg.PoseStamped,
queue_size=10)
scan_pub = rospy.Publisher('scan', LaserScan, queue_size=10)
rospy.init_node('gridmap_publisher', sys.argv)
rospy.loginfo('Initialized gridmap_publisher')
# Gridmap
laser_data = LaserDataMatlab(sys.argv[1])
gridmap = GridMap(grid_size=0.1)
gridmap.init_from_laserdata(laser_data)
# Setup ros message
timestep_list = laser_data.get_timestep_list()
timesteps = timestep_gen(timestep_list)
timestep_len = len(timestep_list)
map_meta = MapMetaData()
# map_meta.map_load_time = rospy.Time().now()
map_meta.resolution = gridmap._grid_size
grid_map = gridmap.get_prob_map() * 100
grid_map = grid_map.astype(np.int8)
# Width = cols
map_meta.width = grid_map.shape[1]
# Height = rows
map_meta.height = grid_map.shape[0]
map_meta.origin.position.x = 0
map_meta.origin.position.y = 0
map_meta.origin.position.z = 0
# Broadcast robot transform
pose_tf_br = tf2_ros.TransformBroadcaster()
rbase_tf = geometry_msgs.msg.TransformStamped()
rbase_tf.header.stamp = rospy.Time.now()
rbase_tf.header.frame_id = 'map'
rbase_tf.child_frame_id = 'robot_base'
rate = rospy.Rate(10)
rx = 0 - gridmap._offset[0]
ry = 0 - gridmap._offset[1]
rtheta = 0
pose_msg = geometry_msgs.msg.PoseStamped()
pose_msg.header.frame_id = 'map'
scan_msg = LaserScan()
scan_msg.header.frame_id = 'robot_base'
# Get info about laser scan
laser_scan_temp = laser_data.get_range_scan(0)
# This info doesn't change
start_angle = laser_scan_temp['start_angle']
angular_resolution = laser_scan_temp['angular_resolution']
scan_msg.angle_min = start_angle
scan_msg.angle_increment = angular_resolution
scan_msg.range_min = 0
scan_msg.time_increment = 0
robot_pose = None
range_scan = None
while not rospy.is_shutdown():
try:
t = next(timesteps)
robot_pose = laser_data.get_pose(t)
range_scan = laser_data.get_range_scan(t)
gridmap.update(robot_pose, range_scan)
except StopIteration:
robot_pose = laser_data.get_pose(timestep_len - 1)
range_scan = laser_data.get_range_scan(timestep_len - 1)
grid_map = gridmap.get_prob_map() * 100
grid_map = grid_map.astype(np.int8)
rx = robot_pose.item(0) - gridmap._offset[0]
ry = robot_pose.item(1) - gridmap._offset[1]
rtheta = robot_pose.item(2)
# Robot_base transform
rbase_tf.transform.translation.x = rx
rbase_tf.transform.translation.y = ry
rbase_tf.transform.translation.z = 0
quat = tf.transformations.quaternion_from_euler(0, 0, rtheta)
rbase_tf.transform.rotation.x = quat[0]
rbase_tf.transform.rotation.y = quat[1]
rbase_tf.transform.rotation.z = quat[2]
rbase_tf.transform.rotation.w = quat[3]
# Robot pose
pose_msg.pose.position.x = rx
pose_msg.pose.position.y = ry
pose_msg.pose.position.z = 0
pose_quat = tf.transformations.quaternion_from_euler(0, 0, rtheta)
pose_msg.pose.orientation.x = pose_quat[0]
pose_msg.pose.orientation.y = pose_quat[1]
pose_msg.pose.orientation.z = pose_quat[2]
pose_msg.pose.orientation.w = pose_quat[3]
# Laser scan
# Get changing info from current scan
num_beams = len(range_scan['ranges'])
max_range = range_scan['maximum_range']
laser_ranges = range_scan['ranges']
valid_endpoints = (laser_ranges < max_range) & (laser_ranges > 0)
laser_ranges = laser_ranges[valid_endpoints]
scan_msg.angle_max = start_angle + num_beams * angular_resolution
scan_msg.range_max = max_range
scan_msg.ranges = laser_ranges
scan_msg.time_increment = (1 / 50) / num_beams
scan_msg.scan_time = rospy.Time.now().nsecs - scan_msg.scan_time
# Publish everything
rbase_tf.header.stamp = rospy.Time.now()
pose_msg.header.stamp = rospy.Time.now()
scan_msg.header.stamp = rospy.Time.now()
pose_tf_br.sendTransform(rbase_tf)
pose_pub.publish(pose_msg)
scan_pub.publish(scan_msg)
h = Header()
h.stamp = rospy.Time.now()
map_meta.map_load_time = rospy.Time().now()
gridmap_pub.publish(header=h, info=map_meta, data=grid_map.flatten())
rate.sleep()