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 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):
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 __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 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 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,
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 __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 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 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
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)
# 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)
# bl.calc_range_many(queries,ranges)
# bl.saveTrace("./test.png")
# make_scan(510,520,np.pi/2.0,61)
print("Init: bl")
bl = range_libc.PyBresenhamsLine(testMap, 500)
print("Init: rm")
rm = range_libc.PyRayMarching(testMap, 500)
print("Init: cddt")
cddt = range_libc.PyCDDTCast(testMap, 500, 108)
cddt.prune()
print("Init: glt")
glt = range_libc.PyGiantLUTCast(testMap, 500, 108)
# this is for testing the amount of raw functional call overhead, does not compute ranges
# null = range_libc.PyNull(testMap, 500, 108)
for x in range(10):
vals = np.random.random((3, num_vals)).astype(np.float32)
vals[0, :] *= (testMap.width() - 2.0)
vals[1, :] *= (testMap.height() - 2.0)
vals[0, :] += 1.0
vals[1, :] += 1.0
vals[2, :] *= np.pi * 2.0
ranges = np.zeros(num_vals, dtype=np.float32)
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)
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()
print("Initializing: glt (max range = {0}, theta_discretization = {1})".
format(max_range, theta_discretization)
)
glt = range_libc.PyGiantLUTCast(testMap, max_range, theta_discretization)
print()
# For testing / debugging
def fixed_scan(num_ranges, print_sample=True):
ranges_np = np.zeros(num_ranges, dtype=np.float32)
queries = np.zeros((num_ranges, 3), dtype=np.float32)
queries[:,0] = int(testMap.width() / 2.0)
queries[:,1] = int(testMap.height() / 2.0)
queries[:,2] = np.linspace(0, 2.0 * np.pi, num_ranges)
queries_deg = np.copy(queries)
