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 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 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 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 publishOccupancyGrid(self):
grid = self.senseOccupancyGrid()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'odom'
map_meta_data = MapMetaData()
map_meta_data.map_load_time = rospy.Time.now()
map_meta_data.resolution = grid_resolution # each cell is 15cm
map_meta_data.width = int(grid_size / grid_resolution)
map_meta_data.height = int(grid_size / grid_resolution)
map_meta_data.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
grid_msg = OccupancyGrid()
grid_msg.header = header
grid_msg.info = map_meta_data
grid_msg.data = list(grid)
self.occupancyGridPublisher.publish(grid_msg)
def __init__(self, image_path=None):
height = int(rospy.get_param("~grid_height", 800))
width = int(rospy.get_param("~grid_width", 800))
resolution = rospy.get_param("~grid_resolution", .3)
ogrid_topic = rospy.get_param("~grid_topic", "/ogrid")
self.grid_drawer = DrawGrid(height, width, image_path)
self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)
m = MapMetaData()
m.resolution = resolution
m.width = width
m.height = height
pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
quat = np.array([0, 0, 0, 1])
m.origin = Pose()
m.origin.position.x, m.origin.position.y = pos[:2]
self.map_meta_data = m
rospy.Timer(rospy.Duration(1), self.pub_grid)
def __init__(self, image_path=None):
height = int(rospy.get_param("~grid_height", 800))
width = int(rospy.get_param("~grid_width", 800))
resolution = rospy.get_param("~grid_resolution", .3)
ogrid_topic = rospy.get_param("~grid_topic", "/ogrid")
self.grid_drawer = DrawGrid(height, width, image_path)
self.ogrid_pub = rospy.Publisher(ogrid_topic,
OccupancyGrid,
queue_size=1)
m = MapMetaData()
m.resolution = resolution
m.width = width
m.height = height
pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
m.origin = Pose()
m.origin.position.x, m.origin.position.y = pos[:2]
self.map_meta_data = m
rospy.Timer(rospy.Duration(1), self.pub_grid)
def receive_point_cloud(self, msg):
'''
Processes the pointcloud into a costmap
'''
xyz_arr = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(msg)
# Flatten the points onto a grid
obs_grid = self.project_point_cloud_onto_plane(xyz_arr)
obs_grid[obs_grid != 0] = 1
header = Header()
header.stamp = msg.header.stamp
header.frame_id = 'base_link' # local grid is in base_link frame
map_meta_data = MapMetaData()
map_meta_data.map_load_time = header.stamp
map_meta_data.resolution = self.resolution
map_meta_data.width = self.grid_size
map_meta_data.height = self.grid_size
map_meta_data.origin = Pose(
Point(self.camera_x_offset,
self.grid_size * self.resolution / 2 + self.camera_y_offset,
0), Quaternion(0, 0, -sqrt(2) / 2,
sqrt(2) / 2)) # 90 degree rotation
grid_msg = OccupancyGrid()
grid_msg.header = header
grid_msg.info = map_meta_data
grid_msg.data = list(
np.int8(obs_grid.flatten() *
100)) # occupany grid message requires values 0-100
try:
self.grid_pub.publish(grid_msg)
except rospy.ROSInterruptException as e:
print(e.getMessage())
pass
def 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
height_offset = 58
captured_width = 70
captured_height = 50
frame_rate = 14 # Hz
preview_pub = rospy.Publisher("/igvc/preview", Image, queue_size=1)
image_pub = rospy.Publisher("/igvc/lane_map", OccupancyGrid, queue_size=1)
header = Header()
header.frame_id = "base_link"
map_info = MapMetaData()
map_info.width = occupancy_grid_size
map_info.height = occupancy_grid_size
map_info.resolution = 0.1
map_info.origin = Pose()
map_info.origin.position.x = -10
map_info.origin.position.y = -10
cam = None
class PerspectiveTransform:
def __init__(self, camera_angle):
self.top_trim_ratio = 0.0
# Camera angle in degrees, where 0 is when camera is facing perpendicular to the ground
self.camera_angle = camera_angle
# Ratio of number of pixels between top points of the trapezoid and their nearest vertical border
#!/usr/bin/env python
import rospy
from nav_msgs.msg import MapMetaData
from sdpp_modules.sdpp_agents import WalkingPerson
if __name__ == '__main__':
rospy.init_node("grid_node")
MapMeta = MapMetaData()
MapMeta.resolution = .1
MapMeta.width = 200 #in cells
MapMeta.height = 200 #in cells
MapMeta.origin.position.x = -10 #in what?
MapMeta.origin.position.y = -10 #in what?
MapMeta.origin.position.z = 0
agent_1 = WalkingPerson(MapMeta, "person_1")
agent_1.build_heading_graph((3, 4))
agent_1.set_update_rate(20)
try:
while not rospy.is_shutdown():
agent_1.update_step()
except rospy.ROSInterruptException:
pass
def talker():
odom_pub = rospy.Publisher("odom1", Odometry, queue_size=50)
map_pub = rospy.Publisher("map1", OccupancyGrid, queue_size=10)
rospy.init_node('talker', anonymous=True)
odom_broadcaster = tf.TransformBroadcaster()
#x = 0.0
#y = 0.0
th = 0.0
#vx = 0.1
#vy = -0.1
vth = 0.1
current_time = rospy.Time.now()
last_time = rospy.Time.now()
prefix = "out_turtle_map_"
ext = ".owl"
times = "1713" ## update init owl file
file_input = prefix + times + ext
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
g = Graph()
'''
#to Cloud
print "file to download "+ file_input
myCmd = 'gsutil -m cp gs://slam-up-bucket/'+file_input+' ./input'
os.system(myCmd)
local_file_input = 'input/'+file_input
print "file que se va parsear "+local_file_input
'''
local_file_input = 'out/' + file_input ## INFO:: en el local
if os.path.exists(local_file_input):
print "exist path " + local_file_input
g.parse(local_file_input, format="turtle")
print("graph has %s statements." % len(g))
px, py, pz = get_position(g)
ox, oy, oz, ow = get_orientation(g)
matrix = get_covar_matrix(g)
covar = np.array([0.0] * 36).reshape(6, 6)
for cell in matrix:
row = int(cell[0])
col = int(cell[1])
value = float(cell[2])
covar[row, col] = value
covar_list = tuple(covar.ravel().tolist())
#Creacion de nav_msg/Odometry
# compute odometry in a typical way given the velocities of the robot
'''dt = (current_time - last_time).to_sec()
delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th '''
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform((px, py, pz), odom_quat,
current_time, "base_link1", "odom1")
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom1"
# set the position
odom.pose.pose = Pose(Point(px, py, pz),
Quaternion(ox, oy, oz, ow))
odom.pose.covariance = covar_list
# set the velocity
vx = vy = 0
odom.child_frame_id = "base_link1"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
odom_pub.publish(odom)
##map
width = height = resolution = 0
mapa_info = get_map_info(g)
if len(mapa_info) > 0:
flagMap = True
if flagMap is not True and var_m is not None and var_grid_list is not None:
print "using saved"
m = var_m
grid_list = var_grid_list
else:
print "reading map"
for row in mapa_info:
p = row[1]
o = p[p.find('#') + 1:len(p)]
if o == "Width":
width = int(row[2])
if o == "Height":
height = int(row[2])
resolution = get_map_resolution(g)
#Creacion de nav_msg/occupancyGrid
len_grid = width * height
grid_map = np.array([-1] * len_grid).reshape(width, height)
objects = get_map_objects(g)
print("objects detected")
print(len(objects))
for row in objects:
p = row[0]
obj_prefix = "http://github.com/Alex23013/slam-up/individual/obj/"
obj_pos = p[len(obj_prefix):len(p)]
parts = obj_pos.split('_')
grid_map[int(parts[0]), int(parts[1])] = int(row[1])
grid_list = tuple(grid_map.ravel().tolist())
map_broadcaster = tf.TransformBroadcaster()
map_broadcaster.sendTransform(
(0, 0, 0), odom_quat, current_time, "base_link1", "map1")
m = MapMetaData()
m.resolution = resolution
m.width = width
m.height = height
pos = np.array(
[-width * resolution / 2, -height * resolution / 2, 0])
quat = np.array([0, 0, 0, 1])
m.origin = Pose()
m.origin.position.x, m.origin.position.y = pos[:2]
ogrid = OccupancyGrid()
ogrid.header.frame_id = 'map1'
ogrid.header.stamp = rospy.Time.now()
ogrid.info = m
ogrid.data = grid_list
var_m = m
var_grid_list = grid_list
map_pub.publish(ogrid)
rospy.loginfo("publishing map:")
rospy.loginfo(times)
last_time = current_time
else:
print local_file_input, "file not found"
rospy.loginfo("publishing odometry:")
rospy.loginfo(times)
times = str(1 + int(times))
file_input = prefix + times + ext
print "antes sleep"
r.sleep()
print "fin_loop"
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()
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 create_local_map(self, event):
if self.odom is None or self.scan is None:
return
occupancy_grid = OccupancyGrid()
occupancy_grid.header.stamp = rospy.Time.now()
occupancy_grid.header.frame_id = self.frame_id
# Map data information
map_data_info = MapMetaData()
map_data_info.resolution = self.cell_size # [m/cell]
map_data_info.width = self.cell_length # [cells]
map_data_info.height = self.cell_length
q = (self.odom.pose.pose.orientation.x,
self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.z,
self.odom.pose.pose.orientation.w)
self.vehicle_yaw = tf.transformations.euler_from_quaternion(q)[2]
'''
x_orig = -self.map_length*cos(self.vehicle_yaw) + self.map_length*sin(self.vehicle_yaw)
y_orig = -self.map_length*sin(self.vehicle_yaw) - self.map_length*cos(self.vehicle_yaw)
map_data_info.origin.position.x = int( x_orig + self.odom.pose.pose.position.x )
map_data_info.origin.position.y = int( y_orig + self.odom.pose.pose.position.y )
map_data_info.origin.position.z = int(self.odom.pose.pose.position.z)
map_data_info.origin.orientation = self.odom.pose.pose.orientation
'''
map_data_info.origin.position.x = self.odom.pose.pose.position.x - self.map_length
map_data_info.origin.position.y = self.odom.pose.pose.position.y - self.map_length
map_data_info.origin.position.z = 0
map_data_info.origin.orientation.x = 0
map_data_info.origin.orientation.y = 0
map_data_info.origin.orientation.z = 0
map_data_info.origin.orientation.w = 1.0
occupancy_grid.info = map_data_info
'''
print (self.vehicle_yaw/3.14*180)
print(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)
print(x_orig, y_orig)
print(map_data_info.origin.position.x, map_data_info.origin.position.y)
'''
# initial cell
data = np.zeros(map_data_info.width * map_data_info.height)
# read laser scan to grid map
for laser in range(0, len(self.scan.ranges)):
scan_range = self.scan.ranges[laser]
rad = self.scan.angle_min + laser * self.scan.angle_increment + self.vehicle_yaw
# remove points out of range
if scan_range is None or scan_range < self.scan.range_min or scan_range > self.scan.range_max or scan_range > self.map_length - 1: #
continue
cell_num = self.get_cell_number(scan_range, rad, map_data_info)
for value in cell_num:
data[value] = 100
#for i in range(0, 200):
# data[i] = -1
#print("==========================")
occupancy_grid.data = data
self.pub_grid_map.publish(occupancy_grid)
occ_grid.info.width = 512
occ_grid.info.height = 480
occ_grid.info.origin.position.x = 0.0
occ_grid.info.origin.position.y = 0.0
occ_grid.info.origin.position.z = 0.0
occ_grid.info.origin.orientation.x = 0.0
occ_grid.info.origin.orientation.y = 0.0
occ_grid.info.origin.orientation.z = 1.0
occ_grid.info.origin.orientation.w = 0.0
occ_grid.data = [0 for i in range(512 * 480)]
metadata = MapMetaData()
metadata.map_load_time.secs = 1381949468
metadata.map_load_time.nsecs = 318325596
metadata.resolution = 0.05
metadata.width = 512
metadata.height = 480
metadata.origin.position.x = 0.0
metadata.origin.position.y = 0.0
metadata.origin.position.z = 0.0
metadata.origin.orientation.x = 0.0
metadata.origin.orientation.y = 0.0
metadata.origin.orientation.z = 1.0
metadata.origin.orientation.w = 0.0
pub_mapserver.publish(occ_grid)
while not rospy.is_shutdown():
pub_mapserver_metadata.publish(metadata)