def __init__(self, particles, state_lock):
print("Initalized")
self.odom = OdometryMotionModel(particles, state_lock)
self.kine = KinematicMotionModel(particles, state_lock)
self.odomMessage = []
self.kineMessage = []
#self.odomParticles = [particles]
self.odomParticles = np.array([particles])
self.kineParticles = np.array([particles])
class OpenLoopRollout():
def __init__(self, particles, state_lock):
#print("Initalized")
self.particle_list= np.array(particles)
self.particle_list_kin = np.array(particles)
self.true_pose_list = None
self.motionModel = OdometryMotionModel(particles, state_lock)
#self.kinematic = KinematicMotionModel(particles, state_lock)
def apply_motion_model(self, msg):
#print(msg)
self.motionModel.motion_cb(msg)
#print("Motion model particle: ",self.motionModel.inner.particles)
self.particle_list = np.concatenate((self.particle_list, self.motionModel.inner.particles))
def apply_kin_model(self, msg, topic):
if topic == "/vesc/sensors/core":
self.kinematic.motion_cb(msg)
self.particle_list_kin = np.concatenate((self.particle_list_kin, self.kinematic.inner.particles))
elif topic == "/vesc/sensors/servo_position_command":
self.kinematic.servo_cb(msg)
def add_true_pose(self, particle):
if self.true_pose_list is None:
self.true_pose_list = np.array(particle)
else:
self.true_pose_list = np.concatenate((self.true_pose_list, particle))
def plot_particles(self):
#print("First Particle: " ,self.particle_list)
x = self.particle_list[:,0]
y = self.particle_list[:,1]
x_kin = self.particle_list_kin[:,0]
y_kin = self.particle_list_kin[:,1]
x_true = self.true_pose_list[:,0]
y_true = self.true_pose_list[:,1]
#print("X: ", x)
#print("Y: ", y)
plt.plot(x,y)
#plt.plot(x_kin, y_kin)
plt.plot(x_true, y_true)
plt.show()
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) # The maximum number of particles to visualize
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES,3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Numpy matrix containig weight for each particle
self.state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
self.tfl = tf.TransformListener()
# Use the 'static_map' service (launched by MapServer.launch) to get the map
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 # The map, will get passed to init of sensor model
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[array_255==0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
# Globally initialize the particles
self.initialize_global()
# Publish particle filter state
self.pose_pub = rospy.Publisher("/pf/ta/viz/inferred_pose", PoseStamped, queue_size = 1) # Publishes the expected pose
self.particle_pub = rospy.Publisher("/pf/ta/viz/particles", PoseArray, queue_size = 1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster() # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher("/pf/ta/viz/scan", LaserScan, queue_size = 1) # Publishes the most recent laser scan
self.pub_odom = rospy.Publisher("/pf/ta/viz/odom", Odometry, queue_size = 1) # Publishes the path of the car
''' HACK VIEW INITIAL DISTRIBUTION'''
'''
self.MAX_VIZ_PARTICLES = 1000
while not rospy.is_shutdown():
self.visualize()
rospy.sleep(0.2)
'''
self.RESAMPLE_TYPE = rospy.get_param("~resample_type", "naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(self.particles, self.weights, self.state_lock) # An object used for resampling
self.sensor_model = SensorModel(map_msg, self.particles, self.weights, self.state_lock) # An object used for applying sensor model
# Subscribe to laser scans. For the callback, use self.sensor_model's lidar_cb function
self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.sensor_model.lidar_cb, queue_size=1)
self.MOTION_MODEL_TYPE = rospy.get_param("~motion_model", "kinematic") # Whether to use the odometry or kinematics based motion model
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(self.particles, self.state_lock) # An object used for applying kinematic motion model
# Subscribe to the state of the vesc (topic: /vesc/sensors/core). For the callback, use self.motion_model's motion_cb function
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(self.particles, self.state_lock)# An object used for applying odometry motion model
# Subscribe to the vesc odometry (topic: /vesc/odom). For the callback, use self.motion_model's motion_cb function
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/odom"), Odometry, self.motion_model.motion_cb, queue_size=1)
else:
print "Unrecognized motion model: "+ self.MOTION_MODEL_TYPE
assert(False)
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
#self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose_cb, queue_size=1)
print('Initialization complete')
def __init__(self, particles, state_lock):
#print("Initalized")
self.particle_list= np.array(particles)
self.particle_list_kin = np.array(particles)
self.true_pose_list = None
self.motionModel = OdometryMotionModel(particles, state_lock)
def __init__(self, n_particles, n_viz_particles, motion_model,
odometry_topic, motor_state_topic, servo_state_topic,
scan_topic, laser_ray_step, exclude_max_range_rays,
max_range_meters, resample_type, speed_to_erpm_offset,
speed_to_erpm_gain, steering_angle_to_servo_offset,
steering_angle_to_servo_gain, car_length):
self.N_PARTICLES = n_particles # The number of particles
# In this implementation, the total number of
# particles is constant
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
self.particle_indices = np.arange(
self.N_PARTICLES) # Cached list of particle indices
self.particles = np.zeros(
(self.N_PARTICLES, 3)) # Numpy matrix of dimension N_PARTICLES x 3
self.weights = np.ones(self.N_PARTICLES) / float(
self.N_PARTICLES
) # Numpy matrix containig weight for each particle
self.state_lock = Lock(
) # A lock used to prevent concurrency issues. You do not need to worry about this
self.tfl = tf.TransformListener(
) # Transforms points between coordinate frames
# Get the map
print("Getting map from service: ", MAP_TOPIC)
rospy.wait_for_service(MAP_TOPIC)
map_msg = rospy.ServiceProxy(
MAP_TOPIC,
GetMap)().map # The map, will get passed to init of sensor model
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
self.permissible_region[
array_255 ==
0] = 1 # Numpy array of dimension (map_msg.info.height, map_msg.info.width),
# With values 0: not permissible, 1: permissible
# Globally initialize the particles
self.initialize_global()
# Publish particle filter state
self.pub_tf = tf.TransformBroadcaster(
) # Used to create a tf between the map and the laser for visualization
self.pose_pub = rospy.Publisher(
PUBLISH_PREFIX + "/inferred_pose", PoseStamped,
queue_size=1) # Publishes the expected pose
self.particle_pub = rospy.Publisher(
PUBLISH_PREFIX + "/particles", PoseArray,
queue_size=1) # Publishes a subsample of the particles
self.pub_laser = rospy.Publisher(
PUBLISH_PREFIX + "/scan", LaserScan,
queue_size=1) # Publishes the most recent laser scan
self.pub_odom = rospy.Publisher(
PUBLISH_PREFIX + "/odom", Odometry,
queue_size=1) # Publishes the path of the car
self.RESAMPLE_TYPE = resample_type # Whether to use naiive or low variance sampling
self.resampler = ReSampler(
self.particles, self.weights,
self.state_lock) # An object used for resampling
# An object used for applying sensor model
self.sensor_model = SensorModel(scan_topic, laser_ray_step,
exclude_max_range_rays,
max_range_meters, map_msg,
self.particles, self.weights,
self.state_lock)
self.MOTION_MODEL_TYPE = motion_model # Whether to use the odometry or kinematics based motion model
if self.MOTION_MODEL_TYPE == "kinematic":
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(
motor_state_topic, servo_state_topic, speed_to_erpm_offset,
speed_to_erpm_gain, steering_angle_to_servo_offset,
steering_angle_to_servo_gain, car_length, self.particles,
self.state_lock)
elif self.MOTION_MODEL_TYPE == "odometry":
# An object used for applying odometry motion model
self.motion_model = OdometryMotionModel(odometry_topic,
self.particles,
self.state_lock)
else:
print "Unrecognized motion model: " + self.MOTION_MODEL_TYPE
assert (False)
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1)
print('Initialization complete')
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param("~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) # The maximum number of particles to visualize
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES,3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES, dtype=np.float64) / float(self.MAX_PARTICLES) # Numpy matrix containig weight for each particle
self.state_lock = Lock() # A lock used to prevent concurrency issues. You do not need to worry about this
# Use the 'static_map' service (launched by MapServer.launch) to get the map
# Will be used to initialize particles and SensorModel
# Store map in variable called 'map_msg'
# YOUR CODE HERE
rospy.wait_for_service('/static_map')
get_map = rospy.ServiceProxy('/static_map', GetMap)
map_msg = get_map().map
# Use dir to show msg fields
# print(dir(map_msg))
# print(dir(map_msg))
# Globally initialize the particles
self.map_free_space = []
self.initialize_global(map_msg)
print("Z!")
# Publish particle filter state
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size = 1) # Publishes the expected pose
self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size = 1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster() # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher("/pf/viz/scan", LaserScan, queue_size = 1) # Publishes the most recent laser scan
print("Y!")
self.RESAMPLE_TYPE = rospy.get_param("~resample_type", "naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(self.particles, self.weights, self.state_lock, self.map_free_space) # An object used for resampling
print("X!")
self.sensor_model = SensorModel(map_msg, self.particles, self.weights, self.state_lock) # An object used for applying sensor model
self.laser_sub = rospy.Subscriber(rospy.get_param("~scan_topic", "/scan"), LaserScan, self.sensor_model.lidar_cb, queue_size=1)
print("XZ!")
self.MOTION_MODEL_TYPE = rospy.get_param("~motion_model", "kinematic") # Whether to use the odometry or kinematics based motion model
print("!~!@#@! MOTION MODEL TYPE", self.MOTION_MODEL_TYPE)
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(self.particles, self.state_lock) # An object used for applying kinematic motion model
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(self.particles, self.state_lock)# An object used for applying odometry motion model
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/odom"), Odometry, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "learned":
from TorchInclude import torch
learned_motion_model = InternalLearnedMotionModel(self.particles, torch.load("../../lab3/src/tanh50k.pt"))
self.motion_model = KinematicLikeMotionModel(self.particles, self.state_lock, learned_motion_model)
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
elif self.MOTION_MODEL_TYPE == "learned_kinematic":
from TorchInclude import torch
kinematic_motion_model = InternalKinematicMotionModel(particles)
learned_kinematic_motion_model = InternalLearnedKinematicMotionModel(self.particles, kinematic_motion_model, torch.load("../../lab1/src/KinematicDropout0.13Itr5k.pt"))
self.motion_model = KinematicLikeMotionModel(self.particles, self.state_lock, learned_kinematic_motion_model)
self.motion_sub = rospy.Subscriber(rospy.get_param("~motion_topic", "/vesc/sensors/core"), VescStateStamped, self.motion_model.motion_cb, queue_size=1)
else:
print "Unrecognized motion model: "+ self.MOTION_MODEL_TYPE
assert(False)
print("A!")
# Use to initialize through rviz. Check clicked_pose_cb for more info
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose_cb, queue_size=1)
def __init__(self):
self.MAX_PARTICLES = int(rospy.get_param(
"~max_particles")) # The maximum number of particles
self.MAX_VIZ_PARTICLES = int(
rospy.get_param("~max_viz_particles"
)) # The maximum number of particles to visualize
self.noise = bool(rospy.get_param("~noise"))
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros(
(self.MAX_PARTICLES,
3)) # Numpy matrix of dimension MAX_PARTICLES x 3
self.weights = np.ones(self.MAX_PARTICLES) / float(
self.MAX_PARTICLES
) # Numpy matrix containig weight for each particle
self.viz_seq = 0
self.INIT_NOISE_POS_SIGMA = 0.2
self.INIT_NOISE_THETA_SIGMA = np.pi / 12
self.state_lock = Lock(
) # A lock used to prevent concurrency issues. You do not need to worry about this
# Use the 'static_map' service (launched by MapServer.launch) to get the map
# Will be used to initialize particles and SensorModel
# Store map in variable called 'map_msg'
# YOUR CODE HERE
rospy.wait_for_service('static_map')
static_map = rospy.ServiceProxy('/static_map', GetMap)
map_msg = static_map().map
# Globally initialize the particles
self.initialize_global(map_msg)
# Publish particle filter state
self.pose_pub = rospy.Publisher(
"/pf/viz/inferred_pose", PoseStamped,
queue_size=1) # Publishes the expected pose
self.particle_pub = rospy.Publisher(
"/pf/viz/particles", PoseArray,
queue_size=1) # Publishes a subsample of the particles
self.pub_tf = tf.TransformBroadcaster(
) # Used to create a tf between the map and the laser for visualization
self.pub_laser = rospy.Publisher(
"/pf/viz/scan", LaserScan,
queue_size=1) # Publishes the most recent laser scan
self.RESAMPLE_TYPE = rospy.get_param(
"~resample_type",
"naiive") # Whether to use naiive or low variance sampling
self.resampler = ReSampler(
self.particles, self.weights,
self.state_lock) # An object used for resampling
self.sensor_model = SensorModel(
map_msg, self.particles, self.weights,
self.state_lock) # An object used for applying sensor model
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.sensor_model.lidar_cb,
queue_size=1)
self.MOTION_MODEL_TYPE = rospy.get_param(
"~motion_model", "kinematic"
) # Whether to use the odometry or kinematics based motion model
if self.MOTION_MODEL_TYPE == "kinematic":
self.motion_model = KinematicMotionModel(
self.particles, self.state_lock, noise=self.noise
) # An object used for applying kinematic motion model
self.motion_sub = rospy.Subscriber(rospy.get_param(
"~motion_topic", "/vesc/sensors/core"),
VescStateStamped,
self.motion_model.motion_cb,
queue_size=1)
elif self.MOTION_MODEL_TYPE == "odometry":
self.motion_model = OdometryMotionModel(
self.particles, self.state_lock, noise=self.noise
) # An object used for applying odometry motion model
self.motion_sub = rospy.Subscriber(rospy.get_param(
"~motion_topic", "/vesc/odom"),
Odometry,
self.motion_model.motion_cb,
queue_size=1)
else:
print "Unrecognized motion model: " + self.MOTION_MODEL_TYPE
assert (False)
# Use to initialize through rviz. Check clicked_pose_cb for more info
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1)
class NoisePropagation():
def __init__(self, particles, state_lock):
print("Initalized")
self.odom = OdometryMotionModel(particles, state_lock)
self.kine = KinematicMotionModel(particles, state_lock)
self.odomMessage = []
self.kineMessage = []
#self.odomParticles = [particles]
self.odomParticles = np.array([particles])
self.kineParticles = np.array([particles])
def addOdomMessage(self, msg):
#self.odomMessage.append(msg)
self.odom.motion_cb(msg)
#self.odomParticles.append(np.copy(self.odom.inner.particles))
self.odomParticles = np.concatenate(
(self.odomParticles, [np.copy(self.odom.inner.particles)]), axis=0)
def addKineVelMessage(self, msg):
#self.kindVelMessage.append(msg)
self.kine.motion_cb(msg)
#self.kineParticles.append(self.kine.inner.particles)
self.kineParticles = np.concatenate(
(self.kineParticles, [np.copy(self.kine.inner.particles)]), axis=0)
def addKineServMessage(self, msg):
#self.kindServMessage.append(msg)
self.kine.servo_cb(msg)
def plotOdomParticles(self):
x_total = []
y_total = []
c = iter(cm.rainbow(np.linspace(0, 1, len(self.odomParticles) + 1)))
#assert len(self.odomParticles) == 20
for i in range(len(self.odomParticles)):
current_particle = self.odomParticles[i]
#print(current_particle.shape)
x = current_particle[:, 0]
#x = column(current_particle, 0)
if x_total == []:
x_total = x
else:
x_total += x
print("X: ", x)
assert len(x) == 500
y = current_particle[:, 1]
#y = column(current_particle, 1)
if y_total == []:
y_total = y
else:
y_total += y
#print("X", x)
#plt.scatter(x, y)
plt.scatter(x, y, color=next(c))
plt.show()
def plotKineParticles(self):
c = iter(cm.rainbow(np.linspace(0, 1, len(self.kineParticles) + 1)))
#assert len(self.odomParticles) == 20
for i in range(len(self.kineParticles)):
current_particle = self.kineParticles[i]
#print(current_particle.shape)
x = current_particle[:, 0]
#x = column(current_particle, 0)
assert len(x) == 500
y = current_particle[:, 1]
#y = column(current_particle, 1)
#print("X", x)
#plt.scatter(x, y)
plt.scatter(x, y, color=next(c))
plt.show()