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 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 __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 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
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
self.horizontal_corner_cut_ratio = 0.3
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"