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()