def __init__(self, scan_topic, laser_ray_step, exclude_max_range_rays,
max_range_meters, map_msg, particles, weights, car_length, state_lock=None):
if state_lock is None:
self.state_lock = Lock()
else:
self.state_lock = state_lock
self.particles = particles
self.weights = weights
self.LASER_RAY_STEP = laser_ray_step # Step for downsampling laser scans
self.EXCLUDE_MAX_RANGE_RAYS = exclude_max_range_rays # Whether to exclude rays that are beyond the max range
self.MAX_RANGE_METERS = max_range_meters # The max range of the laser
self.CAR_LENGTH = car_length
oMap = range_libc.PyOMap(map_msg) # A version of the map that range_libc can understand
max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser
self.range_method = range_libc.PyCDDTCast(oMap, max_range_px, THETA_DISCRETIZATION) # The range method that will be used for ray casting
#self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting
self.range_method.set_sensor_model(self.precompute_sensor_model(max_range_px)) # Load the sensor model expressed as a table
self.queries = None
self.ranges = None
self.laser_angles = None # The angles of each ray
self.downsampled_angles = None # The angles of the downsampled rays
self.do_resample = False # Set so that outside code can know that it's time to resample
# Subscribe to laser scans
self.laser_sub = rospy.Subscriber(scan_topic, LaserScan, self.lidar_cb, queue_size=1)
# FOR KIDNAPPED ROBOT PROBLEM
self.do_confidence_update = False
self.CONF_HISTORY_SIZE = 10
self.conf_history = Queue.Queue()
self.conf_sum = 0.0
self.confidence = 1.0
def _rasterize(self, resolution):
import cv2
division = int(1.0 / resolution)
x1, x2, y1, y2 = self.map_bbox
origin = (x1, y1)
width = int((x2 - x1) / resolution)
height = int((y2 - y1) / resolution)
def grid_coord(x, y, n_division):
return int((x - origin[0]) * n_division +
0.5), int((y - origin[1]) * n_division + 0.5)
canvas = np.zeros((height, width), np.uint8)
for i in range(len(self.lines)):
x1, y1, x2, y2 = self.lines[i]
cv2.line(canvas, grid_coord(x1, y1, division),
grid_coord(x2, y2, division), 255, 2)
# print canvas.shape
# cv2.imshow('canvas', canvas)
# cv2.imwrite('canvas.jpg', canvas)
# cv2.waitKey(0)
# exit(0)
self.rasterized = np.zeros((max(height, width), max(height, width)),
np.uint8)
self.rasterized[:width, :height] = np.transpose(canvas, (1, 0))
self.omap = range_libc.PyOMap(self.rasterized)
self.range_scanner = range_libc.PyBresenhamsLine(
self.omap, int(self.range_max * division))
def update_range_method_map(self):
self.map_msg.data = self.dynamic_map.astype(int).flatten().tolist()
print "UPDATING MAP"
self.map_debug_pub.publish(self.map_msg)
oMap = range_libc.PyOMap(self.map_msg)
if self.WHICH_RM == "bl":
self.range_method = range_libc.PyBresenhamsLine(
oMap, self.MAX_RANGE_PX)
elif "cddt" in self.WHICH_RM:
self.range_method = range_libc.PyCDDTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if self.WHICH_RM == "pcddt":
print "Pruning..."
self.range_method.prune()
elif self.WHICH_RM == "rm":
self.range_method = range_libc.PyRayMarching(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == "rmgpu":
self.range_method = range_libc.PyRayMarchingGPU(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == "glt":
self.range_method = range_libc.PyGiantLUTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
self.range_method.set_sensor_model(self.sensor_model_table)
def __init__(self, map_msg, particles, weights, state_lock=None):
if state_lock is None:
self.state_lock = Lock()
else:
self.state_lock = state_lock
self.particles = particles
self.weights = weights
self.LASER_RAY_STEP = int(rospy.get_param(
"~laser_ray_step")) # Step for downsampling laser scans
self.EXCLUDE_MAX_RANGE_RAYS = bool(
rospy.get_param("~exclude_max_range_rays")
) # Whether to exclude rays that are beyond the max range
self.MAX_RANGE_METERS = float(
rospy.get_param("~max_range_meters")) # The max range of the laser
oMap = range_libc.PyOMap(
map_msg) # A version of the map that range_libc can understand
max_range_px = int(
self.MAX_RANGE_METERS /
map_msg.info.resolution) # The max range in pixels of the laser
self.range_method = range_libc.PyCDDTCast(
oMap, max_range_px, THETA_DISCRETIZATION
) # The range method that will be used for ray casting
#self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting
self.range_method.set_sensor_model(
self.precompute_sensor_model(
max_range_px)) # Load the sensor model expressed as a table
self.queries = None
self.ranges = None
self.laser_angles = None # The angles of each ray
self.downsampled_angles = None # The angles of the downsampled rays
self.do_resample = False # Set so that outside code can know that it's time to resample
def __init__(self, scan_topic, laser_ray_step, exclude_max_range_rays,
max_range_meters, map_msg, particles, weights, state_lock=None):
if state_lock is None:
self.state_lock = Lock()
else:
self.state_lock = state_lock
self.particles = particles
self.weights = weights
self.LASER_RAY_STEP = laser_ray_step # Step for downsampling laser scans
self.EXCLUDE_MAX_RANGE_RAYS = exclude_max_range_rays # Whether to exclude rays that are beyond the max range
self.MAX_RANGE_METERS = max_range_meters # The max range of the laser
oMap = range_libc.PyOMap(map_msg) # A version of the map that range_libc can understand
max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution) # The max range in pixels of the laser
self.range_method = range_libc.PyCDDTCast(oMap, max_range_px, THETA_DISCRETIZATION) # The range method that will be used for ray casting
#self.range_method = range_libc.PyRayMarchingGPU(oMap, max_range_px) # The range method that will be used for ray casting
self.range_method.set_sensor_model(self.precompute_sensor_model(max_range_px)) # Load the sensor model expressed as a table
self.queries = None # Do not modify this variable
self.ranges = None # Do not modify this variable
self.downsampled_angles = None # The angles of the downsampled rays
self.do_resample = True # Set so that outside code can know that it's time to resample
# Subscribe to laser scans
self.laser_sub = rospy.Subscriber(scan_topic, LaserScan, self.lidar_cb, queue_size=1)
def __init__(self):
self.UPDATE_RATE = float(rospy.get_param("~update_rate"))
self.THETA_DISCRETIZATION = float(rospy.get_param("~theta_discretization"))
self.RANGE_MIN_METERS = float(rospy.get_param("~range_min"))
self.RANGE_MAX_METERS = float(rospy.get_param("~range_max"))
self.RANGE_BLACKOUT_METERS = float(rospy.get_param("~range_no_obj"))
self.ANGLE_STEP = float(rospy.get_param("~angle_step"))
self.ANGLE_MIN = float(rospy.get_param("~angle_min"))
self.ANGLE_MAX = float(rospy.get_param("~angle_max"))
self.ANGLES = np.arange(
self.ANGLE_MIN, self.ANGLE_MAX, self.ANGLE_STEP, dtype=np.float32
)
self.CAR_LENGTH = float(rospy.get_param("vesc/chassis_length"))
self.Z_SHORT = float(rospy.get_param("~z_short"))
self.Z_MAX = float(rospy.get_param("~z_max"))
self.Z_BLACKOUT_MAX = float(rospy.get_param("~z_blackout_max"))
self.Z_RAND = float(rospy.get_param("~z_rand"))
self.Z_HIT = float(rospy.get_param("~z_hit"))
self.Z_SIGMA = float(rospy.get_param("~z_sigma"))
self.TF_PREFIX = str(rospy.get_param("~car_name").rstrip("/"))
if len(self.TF_PREFIX) > 0:
self.TF_PREFIX = self.TF_PREFIX + "/"
self.BASE_FRAME_ID = self.TF_PREFIX + "base_link"
self.ODOM_FRAME_ID = self.TF_PREFIX + "odom"
self.LASER_FRAME_ID = self.TF_PREFIX + "laser_link"
map_msg = self.get_map()
occ_map = range_libc.PyOMap(map_msg)
max_range_px = round(self.RANGE_MAX_METERS / map_msg.info.resolution)
self.range_method = range_libc.PyCDDTCast(
occ_map, max_range_px, self.THETA_DISCRETIZATION
)
self.tl = tf.TransformListener()
try:
self.tl.waitForTransform(
self.BASE_FRAME_ID,
self.LASER_FRAME_ID,
rospy.Time(0),
rospy.Duration(15.0),
rospy.Duration(1.0)
)
position, orientation = self.tl.lookupTransform(
self.BASE_FRAME_ID,
self.LASER_FRAME_ID,
rospy.Time(0)
)
self.x_offset = position[0]
except Exception:
rospy.logfatal("Laser: transform from {0} to {1} not found, aborting!".
format(self.LASER_FRAME_ID, self.BASE_FRAME_ID)
)
exit()
self.laser_pub = rospy.Publisher("laser/scan", LaserScan, queue_size=1)
self.update_timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.UPDATE_RATE), self.timer_cb
)
def make_wall(self):
n_ranges = 2
theta = self.start_orientation
y = self.start_loc[0] - 10 * np.sin(theta)
x = self.start_loc[1] - 10 * np.cos(theta)
max_scan_angle = 90 * (np.pi / 180)
testMap = range_libc.PyOMap(self.map_loc, 1)
bl = range_libc.PyBresenhamsLine(testMap, 500)
queries = np.zeros((n_ranges, 3), dtype=np.float32)
ranges = np.zeros(n_ranges, dtype=np.float32)
queries[:, 0] = x
queries[:, 1] = y
queries[:, 2] = theta + np.linspace(-max_scan_angle, max_scan_angle,
n_ranges)
points = bl.calc_range_many(queries, ranges)
bl.saveTrace("/home/racecar/racecar-ws/src/lab6/maps/test.png")
# print imread("./test.png")[500][600]
map_img = imread("/home/racecar/racecar-ws/src/lab6/maps/test.png")
cells_to_remove = set()
for i in range(len(map_img[0])):
for j in range(len(map_img)):
if map_img[i][j][2] == 200:
self.open_cells.discard((j, i))
# print (j,i)
self.open_cells_tuple = tuple(self.open_cells)
self.start_flag = True
print "wall made; set goal!"
def put_cylinder_obstacle(self,
x,
y,
r,
check_collision=False,
collision_inflation_radius=0.0):
xys = []
xys_collision = []
for i in range(360):
s = math.cos(np.deg2rad(i))
c = math.sin(np.deg2rad(i))
xys.append((x + r * c, y + r * s))
xys_collision.append((x + (r + collision_inflation_radius) * c,
y + (r + collision_inflation_radius) * s))
xys = np.array(xys)
xys = self.grid_coord_batch(xys, int(1.0 / self.resolution))
if check_collision:
xys_collision = np.array(xys_collision)
xys_collision = self.grid_coord_batch(xys_collision,
int(1.0 / self.resolution))
if np.any(self.occupancy_grid[xys_collision[:, 1],
xys_collision[:, 0]] == 0):
return False
# if np.sum(self.occupancy_grid[yy1:yy2, xx1:xx2]) > 0:
# return False
self.occupancy_grid[xys[:, 1], xys[:, 0]] = 0.0
self.square_omap[xys[:, 0], xys[:, 1]] = 1
self.omap = range_libc.PyOMap(self.square_omap)
self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)
return True
def build(self, map_msg, mrx, theta_disc):
"""
ros_map : Numpy array with map grid values(as float)
resolution : Resolution of the map
origin : State of car first
"""
self.omap = range_libc.PyOMap(map_msg)
self.scan_method = range_libc.PyCDDTCast(self.omap, mrx, theta_disc)
def make_wall(self):
scale = 0.0504
n_ranges = 2
theta = self.start_orientation
y = self.start_loc[0] - 10*np.sin(theta)
x = self.start_loc[1] - 10*np.cos(theta)
max_scan_angle = 90*(np.pi/180)
# map_loc =
testMap = range_libc.PyOMap(self.map_loc,1)
bl = range_libc.PyBresenhamsLine(testMap, 500)
queries = np.zeros((n_ranges, 3), dtype=np.float32)
ranges = np.zeros(n_ranges, dtype=np.float32)
queries[:,0] = x
queries[:,1] = y
queries[:,2] = theta + np.linspace(-max_scan_angle, max_scan_angle, n_ranges)
points = bl.calc_range_many(queries,ranges)
bl.saveTrace("/home/racecar/racecar-ws/src/lab6/maps/test.png")
# print imread("./test.png")[500][600]
map_img = imread("/home/racecar/racecar-ws/src/lab6/maps/test.png")
cells_to_remove = set()
wall = []
for i in range(len(map_img[0])):
for j in range(len(map_img)):
if map_img[i][j][2] == 200:
self.open_cells.discard((j,i))
y = -16.50 + i*scale
x = 25.90 - j*scale
p = Point()
p.x = x
p.y = y
p.z = 0
wall.append(p)
# print (j,i)
self.open_cells_tuple = tuple(self.open_cells)
self.start_flag = True
print "wall made; set goal!"
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wall
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 1.0
self.pub_wall.publish(marker)
def getRangeLib(mapMsg):
# Pre-compute range lib
oMap = range_libc.PyOMap(mapMsg)
rospy.loginfo("initializing range_libc...")
MAX_RANGE_METERS = 60
MAX_RANGE_PX = int(MAX_RANGE_METERS / mapMsg.info.resolution)
THETA_DISCRETIZATION = 200
rangeLib = range_libc.PyCDDTCast(oMap, MAX_RANGE_PX,
THETA_DISCRETIZATION)
rospy.loginfo("pruning...")
# rangeLib.prune()
rospy.loginfo("range_libc initialized")
return rangeLib
def __init__(self):
self.UPDATE_RATE = float(rospy.get_param("~update_rate", 10.0))
self.THETA_DISCRETIZATION = float(
rospy.get_param("~theta_discretization", 656))
self.MIN_RANGE_METERS = float(
rospy.get_param("~min_range_meters", 0.02))
self.MAX_RANGE_METERS = float(rospy.get_param("~max_range_meters",
5.6))
self.ANGLE_STEP = float(
rospy.get_param("~angle_step", 0.00613592332229))
self.ANGLE_MIN = float(rospy.get_param("~angle_min", -2.08621382713))
self.ANGLE_MAX = float(rospy.get_param("~angle_max", 2.09234976768))
self.ANGLES = np.arange(self.ANGLE_MIN,
self.ANGLE_MAX,
self.ANGLE_STEP,
dtype=np.float32)
self.CAR_LENGTH = float(rospy.get_param("~car_length", 0.33))
self.Z_SHORT = float(rospy.get_param("~z_short", 0.03))
self.Z_MAX = float(rospy.get_param("~z_max", 0.16))
self.Z_BLACKOUT_MAX = float(rospy.get_param("~z_blackout_max", 50))
self.Z_RAND = float(rospy.get_param("~z_rand", 0.01))
self.Z_HIT = float(rospy.get_param("~z_hit", 0.8))
self.Z_SIGMA = float(rospy.get_param("~z_sigma", 0.03))
self.TF_PREFIX = str(rospy.get_param("~tf_prefix", "").rstrip("/"))
if len(self.TF_PREFIX) > 0:
self.TF_PREFIX = self.TF_PREFIX + "/"
map_msg = self.get_map()
occ_map = range_libc.PyOMap(map_msg)
max_range_px = int(self.MAX_RANGE_METERS / map_msg.info.resolution)
self.range_method = range_libc.PyCDDTCast(occ_map, max_range_px,
self.THETA_DISCRETIZATION)
self.tl = tf.TransformListener()
while not self.tl.frameExists(self.TF_PREFIX + "base_link"):
pass
while not self.tl.frameExists(self.TF_PREFIX + "laser_link"):
pass
position, orientation = self.tl.lookupTransform(
self.TF_PREFIX + "base_link", self.TF_PREFIX + "laser_link",
rospy.Time(0))
self.x_offset = position[0]
self.laser_pub = rospy.Publisher("scan", LaserScan, queue_size=1)
self.update_timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.UPDATE_RATE), self.timer_cb)
def get_omap(self):
'''
Fetch the occupancy grid map from the map_server instance, and initialize the correct
RangeLibc method. Also stores a matrix which indicates the permissible region of the map
'''
# this way you could give it a different map server as a parameter
map_service_name = rospy.get_param("~static_map", "static_map")
print("getting map from service: ", map_service_name)
rospy.wait_for_service(map_service_name)
self.map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
self.dilated_map_msg = rospy.ServiceProxy(map_service_name,
GetMap)().map
self.map_info = self.map_msg.info
self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS /
self.map_info.resolution)
self.init_dynamic_map(self.MAP_BASE_PATH)
self.map_msg.data = self.dynamic_map.astype(float).flatten().tolist()
oMap = range_libc.PyOMap(self.map_msg)
# initialize range method
print "Initializing range method:", self.WHICH_RM
if self.WHICH_RM == "bl":
self.range_method = range_libc.PyBresenhamsLine(
oMap, self.MAX_RANGE_PX)
elif "cddt" in self.WHICH_RM:
self.range_method = range_libc.PyCDDTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if self.WHICH_RM == "pcddt":
print "Pruning..."
self.range_method.prune()
elif self.WHICH_RM == "rm":
self.range_method = range_libc.PyRayMarching(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == "rmgpu":
self.range_method = range_libc.PyRayMarchingGPU(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == "glt":
self.range_method = range_libc.PyGiantLUTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
print "Done loading map"
# 0: permissible, -1: unmapped, 100: blocked
array_255 = np.array(self.map_msg.data).reshape(
(self.map_msg.info.height, self.map_msg.info.width))
# 0: not permissible, 1: permissible
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255 == 0] = 1
self.map_initialized = True
def get_omap(self):
# this way you could give it a different map server as a parameter
map_service_name = rospy.get_param("~static_map", "static_map")
print("getting map from service: ", map_service_name)
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
self.map_info = map_msg.info
oMap = range_libc.PyOMap(map_msg)
# this value is the max range used internally in RangeLibc
# it also should be the size of your sensor model table
self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS /
self.map_info.resolution)
# initialize range method
print "Initializing range method:", self.WHICH_RANGE_METHOD
if self.WHICH_RANGE_METHOD == "bl":
self.range_method = range_libc.PyBresenhamsLine(
oMap, self.MAX_RANGE_PX)
elif "cddt" in self.WHICH_RANGE_METHOD:
self.range_method = range_libc.PyCDDTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if self.WHICH_RANGE_METHOD == "pcddt":
print "Pruning..."
self.range_method.prune()
elif self.WHICH_RANGE_METHOD == "rm":
self.range_method = range_libc.PyRayMarching(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RANGE_METHOD == "rmgpu":
self.range_method = range_libc.PyRayMarchingGPU(
oMap, self.MAX_RANGE_PX)
elif self.WHICH_RANGE_METHOD == "glt":
self.range_method = range_libc.PyGiantLUTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
print "Done loading map"
# 0: permissible, -1: unmapped, large value: blocked
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
# 0: not permissible, 1: permissible
# this may be useful for global particle initialization - don't initialize particles in non-permissible states
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255 == 0] = 1
self.map_initialized = True
def __init__(self, map_msg, particles, weights, state_lock=None):
if state_lock is None:
self.state_lock = Lock()
else:
self.state_lock = state_lock
self.particles = particles
self.weights = weights
self.weights_unsquashed = np.array(weights)
self.LASER_RAY_STEP = int(rospy.get_param(
"~laser_ray_step")) # Step for downsampling laser scans
self.MAX_RANGE_METERS = float(
rospy.get_param("~max_range_meters")) # The max range of the laser
print("ZZ!")
oMap = range_libc.PyOMap(
map_msg) # A version of the map that range_libc can understand
print("ZA!")
print("ZA!")
max_range_px = int(
self.MAX_RANGE_METERS /
map_msg.info.resolution) # The max range in pixels of the laser
self.range_method = range_libc.PyCDDTCast(
oMap, max_range_px, THETA_DISCRETIZATION
) # The range method that will be used for ray casting
print("ZZZZZ!")
self.range_method.set_sensor_model(
self.precompute_sensor_model(
max_range_px)) # Load the sensor model expressed as a table
print("ZB!")
self.queries = None
self.ranges = None
self.laser_angles = None # The angles of each ray
self.downsampled_angles = None # The angles of the downsampled rays
self.do_resample = False # Set so that outside code can know that it's time to resample
print("ZC!")
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.lidar_cb,
queue_size=1)
print("ZD!")
def __init__(self,
num_rays=None,
min_angle=None,
max_angle=None,
max_range=None,
range_method=None,
frame=None,
omap=None):
self.max_range = max_range
self.laser_ranges = np.zeros(num_rays, dtype=np.float32)
self.laser_angles = np.linspace(min_angle,
max_angle,
num_rays,
endpoint=True)
self.queries = np.zeros((num_rays, 3), dtype=np.float32)
map_msg = omap.map_msg
map_info = map_msg.info
oMap = range_libc.PyOMap(map_msg)
self.MAX_RANGE_PX = int(self.max_range / map_info.resolution)
self.THETA_DISCRETIZATION = 200
# initialize range method
print "Initializing range method:", range_method
if range_method == "bl":
self.range_method = range_libc.PyBresenhamsLine(
oMap, self.MAX_RANGE_PX)
elif "cddt" in range_method:
self.range_method = range_libc.PyCDDTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if range_method == "pcddt":
print "Pruning..."
self.range_method.prune()
elif range_method == "rm":
self.range_method = range_libc.PyRayMarching(
oMap, self.MAX_RANGE_PX)
elif range_method == "rmgpu":
self.range_method = range_libc.PyRayMarchingGPU(
oMap, self.MAX_RANGE_PX)
elif range_method == "glt":
self.range_method = range_libc.PyGiantLUTCast(
oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
print "Simulated Laser Scanner Initialized..."
def put_box_obstacle(self, x, y, w, h, check_collision=False):
x1, y1 = x - w * 0.5, y - h * 0.5
x2, y2 = x + w * 0.5, y + h * 0.5
xx1, yy1 = self.grid_coord(x1, y1, int(1.0 / self.resolution))
xx2, yy2 = self.grid_coord(x2, y2, int(1.0 / self.resolution))
if check_collision:
if np.any(self.occupancy_grid[yy1:yy2, xx1:xx2] == 0):
return False
# if np.sum(self.occupancy_grid[yy1:yy2, xx1:xx2]) > 0:
# return False
self.occupancy_grid[yy1:yy2, xx1:xx2] = 0.0
self.square_omap[xx1:xx2, yy1:yy2] = 1
self.omap = range_libc.PyOMap(self.square_omap)
self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)
return True
def get_omap(self):
'''
Fetch the occupancy grid map from the map_server instance, and initialize the correct
RangeLibc method. Also stores a matrix which indicates the permissible region of the map
'''
# this way you could give it a different map server as a parameter
# map_service_name = rospy.get_param('/static_map', '/static_map')
map_service_name = '/static_map'
print('getting map from service: ', map_service_name)
print('map service name: {}'.format(map_service_name))
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
self.map_info = map_msg.info
oMap = range_libc.PyOMap(map_msg)
self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS / self.map_info.resolution)
# initialize range method
print 'Initializing range method:', self.WHICH_RM
if self.WHICH_RM == 'bl':
self.range_method = range_libc.PyBresenhamsLine(oMap, self.MAX_RANGE_PX)
elif 'cddt' in self.WHICH_RM:
self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
if self.WHICH_RM == 'pcddt':
print 'Pruning...'
self.range_method.prune()
elif self.WHICH_RM == 'rm':
self.range_method = range_libc.PyRayMarching(oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == 'rmgpu':
self.range_method = range_libc.PyRayMarchingGPU(oMap, self.MAX_RANGE_PX)
elif self.WHICH_RM == 'glt':
self.range_method = range_libc.PyGiantLUTCast(oMap, self.MAX_RANGE_PX, self.THETA_DISCRETIZATION)
print 'Done loading map'
# 0: permissible, -1: unmapped, 100: blocked
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
# 0: not permissible, 1: permissible
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255==0] = 1
self.map_initialized = True
def mapCallback(self, map_msg):
"""
Map is changed, pass new map 2d-scanner,
fix this for dynamic map updates, origin is not important
if its changes to the original map
"""
ros_omap = range_libc.PyOMap(map_msg)
quat = np.array((map_msg.info.origin.orientation.x,
map_msg.info.origin.orientation.y,
map_msg.info.origin.orientation.z,
map_msg.info.origin.orientation.w))
(_, _, yaw) = euler_from_quaternion(quat)
origin = (map_msg.info.origin.position.x,
map_msg.info.origin.position.y, yaw)
# pass map info to racecar instance
self.rcs.setMap(ros_omap, map_msg.info.resolution, origin)
def as_sdf(self, raytracer=None):
NUMBA = False
RANGE_LIBC = True
occupied_points_ij = np.array(self.as_occupied_points_ij())
min_distances = np.ones(self.occupancy_.shape) * self.HUGE
if NUMBA:
compiled_sdf_math(occupied_points_ij, min_distances)
if RANGE_LIBC:
if raytracer is None:
import range_libc
pyomap = range_libc.PyOMap(np.ascontiguousarray(self.occupancy_.T >= self.thresh_occupied_))
rm = range_libc.PyRayMarching(pyomap, self.occupancy_.shape[0])
min_distances = np.ascontiguousarray(np.zeros_like(self.occupancy_, dtype=np.float32))
rm.get_dist(min_distances)
else:
min_distances = raytracer.get_dist()
# Change from i, j units to x, y units [meters]
min_distances = min_distances * self.resolution_
# Switch sign for occupied and unkown points (*signed* distance field)
min_distances[self.occupancy_ > self.thresh_free] *= -1.
return min_distances
def get_omap(self):
"""
Loads the map and initializes RangeLibc
Map server must be running
"""
print("Loading Map")
map_service_name = "static_map"
rospy.wait_for_service(map_service_name)
map_msg = rospy.ServiceProxy(map_service_name, GetMap)().map
self.map_info = map_msg.info
print(self.map_info)
oMap = range_libc.PyOMap(map_msg)
# max range used internally in RangeLibc
self.MAX_RANGE_PX = int(self.MAX_RANGE_METERS /
self.map_info.resolution)
# initialize range method
self.range_method = range_libc.PyCDDTCast(oMap, self.MAX_RANGE_PX,
self.THETA_DISCRETIZATION)
print "Done loading map"
self.map_initialized = True
from scipy.misc import imread
# particles = np.zeros((10,3))
# action = np.array([3,2,1])
# particles[:,0] += action[0]*np.cos(particles[:,2]) - action[1]*np.sin(particles[:,2])
# print particles
# n_ranges, x, y, theta = int, float, float, float
n_ranges = 2
x = 300
y = 500
theta = np.pi / 2.
max_scan_angle = 90 * (np.pi / 180)
testMap = range_libc.PyOMap("../maps/basement_fixed_more_dilated.png", 1)
bl = range_libc.PyBresenhamsLine(testMap, 500)
queries = np.zeros((n_ranges, 3), dtype=np.float32)
ranges = np.zeros(n_ranges, dtype=np.float32)
queries[:, 0] = x
queries[:, 1] = y
queries[:, 2] = theta + np.linspace(-max_scan_angle, max_scan_angle, n_ranges)
points = bl.calc_range_many(queries, ranges)
bl.saveTrace("./test.png")
print imread("./test.png")[500][600]
map_img = imread("./test.png")
cells = set()
for i in range(len(map_img[0])):
for j in range(len(map_img)):
def __init__(self):
self.GT_INDEX = 0 # Ground truth index (for sampling in K > 1 poses)
# Scan constants
self.THETA_DISCRETIZATION = 112 # Discretization of scanning angle
self.Z_SHORT = 0.01 #0.14 # Weight for short reading
self.LAMBDA_SHORT = 0.05 # Intrinsic parameter of the exponential dist.
self.Z_MAX = 0.07 #0.015 # Weight for max reading
self.Z_RAND = 0.12 #0.045 # Weight for random reading
self.SIGMA_HIT = 8.0 # Noise value for hit reading
self.Z_HIT = 0.80 #0.80 # Weight for hit reading
self.MAX_RANGE_METERS = float(rospy.get_param(
"~max_range_meters", 5.6)) # The max range of the laser
# Dynamics constants
self.MIN_VEL = -0.75 #TODO make sure these are right
self.MAX_VEL = 0.75
self.MIN_DEL = -0.34
self.MAX_DEL = 0.34
self.laser_params = {
"angle_min": -2.08621382713,
"angle_max": 2.09234976768,
"angle_increment": 0.00613592332229,
"time_increment": 9.76562732831e-05,
"scan_time": 0.10000000149,
"range_min": 0.019999999553,
"range_max": 5.59999990463
}
self.num_ranges = 682 # Number of ranges to publish (M)
# This is equivalent to int(max-min)/incr + 2
self.dtype = torch.cuda.FloatTensor
self.K = 5 # Number of particles to publish
# TODO: These values will need to be tuned
self.sigma = torch.Tensor([0.25,
0.1]).type(self.dtype).expand(self.K, 2)
# Initial position [x, y, theta]
INITIAL_POSE = torch.Tensor([0.0, 0.0, 0.0]).type(self.dtype)
dt = 0.1 # Constant dt, same as trained model
# TODO: get topic names from server
self.pose_pub_topic = "/sim/ground_truth/pose"
self.poses_pub_topic = "/sim/ground_truth/poses" # TODO get rid of poses / or create a ROI
self.scan_pub_topic = "/sim/scan"
self.set_pose_sub_topic = "/sim/set_pose"
self.control_sub_topic = "/vesc/high_level/ackermann_cmd_mux/input/nav_0"
#self.model_name = rospy.get_param("~nn_model", "/media/JetsonSSD/model3.torch")
self.model_name = rospkg.RosPack().get_path(
"environment_sim") + "/models/dynamics_model_noisy.torch"
# Initialize publishers and subscribers
self.pose_pub = rospy.Publisher(self.pose_pub_topic,
PoseStamped,
queue_size=10)
self.poses_pub = rospy.Publisher(self.poses_pub_topic,
PoseArray,
queue_size=10)
self.scan_pub = rospy.Publisher(self.scan_pub_topic,
LaserScan,
queue_size=1)
self.model = torch.load(self.model_name)
self.model.cuda() # Tell torch to run the network on the GPU
self.model.eval() # Model ideally runs faster in eval mode
print("Loading:", self.model_name)
print("Model:\n", self.model)
print("Torch Datatype:", self.dtype)
self.noise = torch.Tensor(self.K, 2).type(self.dtype) # (K,2)
self.ctrl = torch.zeros(self.K, 2).type(self.dtype) # (K,2)
self.pose = INITIAL_POSE.repeat(self.K, 1) # (K,3)
self.pose_dot = torch.zeros(self.K, 3).type(self.dtype) # (K,3)
self.nn_input = torch.zeros(self.K, 8).type(self.dtype) # (K,8)
self.nn_input[:, 4] = 1.0 # cos(0)
self.nn_input[:, 7] = dt # set dt = 0.1
# 0. Wait for map to publish / static map server
rospy.wait_for_service('static_map')
static_map = rospy.ServiceProxy('/static_map', GetMap)
map_msg = static_map().map
self.ranges = np.zeros(self.num_ranges, dtype=np.float32)
self.query = np.zeros((1, 3),
dtype=np.float32) # (num particles = 1, 3)
self.obs_angles = np.arange(self.laser_params["angle_min"],
self.laser_params["angle_max"] +
self.laser_params["angle_increment"],
self.laser_params["angle_increment"],
dtype=np.float32)
oMap = range_libc.PyOMap(
map_msg) # A version of the map that range_libc can understand
max_range_px = int(
self.MAX_RANGE_METERS /
map_msg.info.resolution) # The max range in pixels of the laser
self.range_method = range_libc.PyCDDTCast(
oMap, max_range_px, self.THETA_DISCRETIZATION
) # The range method that will be used for ray casting
# TODO: do we need this to create a range
self.range_method.set_sensor_model(
self.precompute_sensor_model(
max_range_px)) # Load the sensor model expressed as a table
self.control_sub = rospy.Subscriber(self.control_sub_topic,
AckermannDriveStamped,
self.control_cb,
queue_size=10)
self.set_pose_sub = rospy.Subscriber(self.set_pose_sub_topic,
PoseStamped,
self.set_pose_cb,
queue_size=1)
# left handed coordinate space
#
#
####################################################################################################
# print range_libc.USE_CACHED_TRIG
# print range_libc.USE_CACHED_TRIG
# print range_libc.USE_ALTERNATE_MOD
# print range_libc.USE_CACHED_CONSTANTS
# print range_libc.USE_FAST_ROUND
# print range_libc.NO_INLINE
# print range_libc.USE_LRU_CACHE
# print range_libc.LRU_CACHE_SIZE
# testMap = range_libc.PyOMap("../maps/basement_hallways_5cm.png",1)
testMap = range_libc.PyOMap("../maps/synthetic.map.png", 1)
# testMap = range_libc.PyOMap("/home/racecar/racecar-ws/src/TA_examples/lab5/maps/basement.png",1)
if testMap.error():
exit()
# testMap.save("./test.png")
num_vals = 100000
# vals = np.zeros((3,num_vals), dtype=np.float32)
# vals[0,:] = testMap.width()/2.0
# vals[1,:] = testMap.height()/2.0
# vals[2,:] = np.linspace(0,2.0*np.pi, num=num_vals)
# def make_scan(x,y,theta,n_ranges):
# MAX_SCAN_ANGLE = (0.75 * np.pi)
# left handed coordinate space
#
#
####################################################################################################
# print(range_libc.USE_CACHED_TRIG)
# print(range_libc.USE_CACHED_TRIG)
# print(range_libc.USE_ALTERNATE_MOD)
# print(range_libc.USE_CACHED_CONSTANT)S
# print(range_libc.USE_FAST_ROUND)
# print(range_libc.NO_INLINE)
# print(range_libc.USE_LRU_CACHE)
# print(range_libc.LRU_CACHE_SIZE)
testMapFileName = "../maps/test_medium.png".encode('utf8')
testMap = range_libc.PyOMap(testMapFileName, 1)
if testMap.error():
exit()
max_range = 30
theta_discretization = 1000
print("Initializing: bl (max range = {0})".format(max_range))
bl = range_libc.PyBresenhamsLine(testMap, max_range)
print("Initializing: rm (max range = {0})".format(max_range))
rm = range_libc.PyRayMarching(testMap, max_range)
print("Initializing: cddt (max range = {0}, theta_discretization = {1})".
format(max_range, theta_discretization)
)
cddt = range_libc.PyCDDTCast(testMap, max_range, theta_discretization)
cddt.prune()
def set_map(self, occupancy_grid):
self.map = range_libc.PyOMap(occupancy_grid)
self.max_range_px = int(self.max_range_meters / occupancy_grid.info.resolution)
self.range_method = range_libc.PyCDDTCast(self.map, self.max_range_px, self.theta_discretization)
def __init__(self,
occupancy_grid,
resolution,
origin,
initial_pos=None,
path_map=None,
path_map_division=16,
path_map_dilation=2,
path_map_weighted_dilation=False,
reachable_area_dilation=3,
destination_map=None,
background_traversable=True,
name=None):
"""
path_map encodes the allowed space for path planning
reachable_area is path_map shrinked by reachable_area_dilation. This is useful for selecting
starting point and goal points, which should be on the path map but also should not be too
close to obstacles.
:param occupancy_grid: a grayscale image where 255 means free space.
:param resolution: real width of a pixel in the occupancy map in meters.
:param destination_map: a colored path map where each color represents a destination.
:param path_map_dilation: higher value will shrink the path map further
:param path_map_weighted_dilation: create the path map from a weighted combination between
the original path map and the dilated path map. This prevents tight openings from
being closed due to the dilation process and also allows the agent to start at
positions anywhere from the original path maps.
:param reachable_area_dilation: higher value will shrink the reachable area further
:param background_traversable: this affects map bounding box calculation.
"""
self.g = {
'path_map_dilation': path_map_dilation,
'reachable_area_dilation': reachable_area_dilation,
'path_map_weighted_dilation': path_map_weighted_dilation
}
self.occupancy_grid = occupancy_grid
self.occupancy_grid_copy = np.array(self.occupancy_grid, copy=True)
self.resolution = resolution
self.origin = origin
self.name = name
free_space = (self.occupancy_grid >= 254).astype(np.uint8)
# hard_obstacles = (self.occupancy_grid == 0).astype(np.uint8)
self.binary_occupancy = (1 - free_space) * 255 # 0 means no obstacle
# Compute distance transform of the map
start_time = time.time()
self.map_dist_transform, _ = cv2.distanceTransformWithLabels(
255 - self.binary_occupancy, cv2.DIST_L2, cv2.DIST_MASK_PRECISE)
print('distance transform time: %.2f sec' % (time.time() - start_time))
self.visible_map_bbox = self._compute_visible_map_bbox(
background_traversable)
self.path_map_division = path_map_division
if path_map is None:
# Automatically generate path map. This is usually the case if we use a real
# map generated by laser scan.
path_map_scale = self.resolution / (1.0 / self.path_map_division)
m = cv2.resize(self.binary_occupancy,
dsize=(0, 0),
fx=path_map_scale,
fy=path_map_scale,
interpolation=cv2.INTER_NEAREST)
self.path_map = m
else:
self.path_map = (path_map > 0).astype(np.uint8) * 255
# cv2.imshow('path_map1', cv2.resize(self.path_map, (1024, 1024)))
# The path map may contain multiple disconnected areas. Only the area where
# the initial position falls will be considered.
# However, if initial_pos is None, we use the occupancy grid itself as the path map.
if initial_pos is not None:
init_pos_grid = self.grid_coord(initial_pos[0], initial_pos[1],
self.path_map_division)
self.path_map = 255 - self.find_reachable_area(
self.path_map, init_pos_grid)
# Shrink the path map (i.e., dilate the occupied area)
path_map_dilated = self.dilate(self.path_map, path_map_dilation)
self.path_map_dilation = path_map_dilation
self.reachable_area = 255 - self.dilate(path_map_dilated,
reachable_area_dilation)
self.reachable_area_dilation = reachable_area_dilation
self.reachable_locs = list(zip(*np.nonzero(self.reachable_area)[::-1]))
self.reachable_locs_per_destination = {}
if path_map_weighted_dilation:
self.path_map = (self.path_map * 0.2 +
path_map_dilated * 0.8).astype(np.uint8)
else:
self.path_map = path_map_dilated
self.path_map_weighted_dilation = path_map_weighted_dilation
print('path map area', self._compute_path_map_area())
# self.path_map = path_map_dilated
# print self.path_map.max()
# exit(0)
# cv2.imshow('reachable_area', cv2.resize(self.reachable_area, (1024, 1024)))
# cv2.imshow('path_map2', cv2.resize(self.path_map, (1600, 1600)))
# cv2.imwrite('/tmp/path_map.png', self.path_map)
# cv2.waitKey(0)
# exit(0)
if destination_map is None:
self.destination_map = None
else:
assert destination_map.shape[:2] == self.path_map.shape[:2]
self.destination_map = destination_map
self._gen_per_goal_reachable_locations()
print('%d destinations' % len(self.reachable_locs_per_destination))
self.destination_centroids = self._precompute_destination_centroids(
)
print('precompute destination paths...')
start_time = time.time()
self.destination_paths = self._precompute_destination_paths()
print('precompute destination paths time %.2f sec' %
(time.time() - start_time))
# pad occupancy map to make it a square because it seems range_libc will crash
# on non-square map.
h, w = self.binary_occupancy.shape
self.square_omap = np.zeros((max(h, w), max(h, w)), np.uint8)
self.square_omap[:w, :h] = np.transpose(self.binary_occupancy, (1, 0))
self.square_omap_copy = np.array(self.square_omap, copy=True)
self.omap = range_libc.PyOMap(self.square_omap)
self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)
def clear_obstacles(self):
self.occupancy_grid = np.array(self.occupancy_grid_copy, copy=True)
self.square_omap = np.array(self.square_omap_copy, copy=True)
self.omap = range_libc.PyOMap(self.square_omap)
self.range_scanner = range_libc.PyBresenhamsLine(self.omap, 1000)