def test_motion_model(self): fs = FastSLAM() fpold = FilterParticle() fpold.state.pose.pose.position.y = 2.0 twist = Twist() twist.linear.x = 1 dt = rospy.Duration.from_sec(.1) fpnew = fs.motion_model(fpold, twist, dt) dy_expected = twist.linear.x * dt.secs dy_measured = fpnew.state.pose.pose.position.y - fpold.state.pose.pose.position.y error = abs(dy_measured - dy_expected) self.assertTrue(error < .01) twist.linear.x = 2 dt = rospy.Duration.from_sec(.1) fpnew = fs.motion_model(fpold, twist, dt) dy_expected = twist.linear.x * dt.secs dy_measured = fpnew.state.pose.pose.position.y - fpold.state.pose.pose.position.y error = abs(dy_measured - dy_expected) self.assertTrue(error < .02)
def initialize_particle_filter(self, preset_features): ''' Create an instance of FastSLAM algorithm ''' self.core = FastSLAM(preset_features)
class CamSlam360(object): ''' Maintains a state for the robot, as well as for features Robot state: v1: x, y, heading, v (linear, x), w (angular, z) Feature state: v1: x, y, r, g, b ''' def __init__(self): rospy.init_node('CAMSLAM360') # prepare filter self.core = None self.last_sensor_reading = None preset_covariance = Matrix([[0.25, 0, 0, 0, 0], [0, 0.25, 0, 0, 0], [0, 0, 0.25, 0, 0], [0, 0, 0, 0.25, 0], [0, 0, 0, 0, 0.25]]) # purple, origin feature1 = Feature(mean=Matrix([0, 25, 161, 77, 137]), covar=preset_covariance) feature1.__immutable__ = True # blue feature2 = Feature(mean=Matrix([10, 25, 75, 55, 230]), covar=preset_covariance) feature2.__immutable__ = True # green feature3 = Feature(mean=Matrix([0, 15, 82, 120, 68]), covar=preset_covariance) feature3.__immutable__ = True # pink feature4 = Feature(mean=Matrix([10, 15, 224, 37, 192]), covar=preset_covariance) feature4.__immutable__ = True features = [feature1, feature2, feature3, feature4] self.initialize_particle_filter(features) # begin ros updating self.cam_sub = rospy.Subscriber('/camera/features', VizScan, self.measurement_update) self.twist_sub = rospy.Subscriber('/cmd_vel', Twist, self.motion_update) self.odom_pub = rospy.Publisher('/slam_estimate', Odometry, queue_size=1) def run(self): joke_rate = rospy.Rate(10) if self.core is not None: rospy.loginfo('Running!') self.print_summary() while (not rospy.is_shutdown()) and (self.last_sensor_reading is None): rospy.loginfo('waiting on the first sensor data') if (rospy.Time.now().to_sec() > 40): return 10 joke_rate.sleep() while not rospy.is_shutdown(): self.loop_over_particles() rospy.loginfo('joke rate stop?') if (rospy.Time.now().to_sec() > 40): # rospy.loginfo('... yes') return 10 joke_rate.sleep() rospy.loginfo('exited main loop. Done!') def loop_over_particles(self): rospy.loginfo('main loop passing of sensor data to SLAM') self.core.cam_cb(self) self.odom_pub.publish(self.easy_odom()) def initialize_particle_filter(self, preset_features): ''' Create an instance of FastSLAM algorithm ''' self.core = FastSLAM(preset_features) def easy_odom(self): x, y, heading = self.core.summary() otto = Odometry() otto.header.frame_id = 'odom' otto.header.stamp = rospy.Time.now() otto.pose.pose.position.x = x otto.pose.pose.position.y = y otto.pose.pose.orientation = heading_to_quaternion(heading) return otto def measurement_update(self, msg): ''' Pass along a VizScan message to the main running loop ''' # self.core.cam_cb(msg) # rospy.loginfo('new sensor data...') self.last_sensor_reading = msg odom = self.easy_odom() self.odom_pub.publish(odom) def motion_update(self, msg): ''' Pass along a Twist message ''' # rospy.loginfo('motion_update('+str(msg)+')') self.core.motion_update(msg) # self.print_summary() odom = self.easy_odom() self.odom_pub.publish(odom) def print_summary(self): ''' average x, y, heading ''' rospy.loginfo('prkt_summary: ' + str(self.core.summary()))
class CamSlam360(object): ''' Maintains a state for the robot, as well as for features Robot state: v1: x, y, heading, v (linear, x), w (angular, z) Feature state: v1: x, y, r, g, b ''' def __init__(self): rospy.init_node('CAMSLAM360') # prepare filter self.core = None self.last_sensor_reading = None preset_covariance = Matrix([[0.25,0,0,0,0], [0,0.25,0,0,0], [0,0,0.25,0,0], [0,0,0,0.25,0], [0,0,0,0,0.25]]) # purple, origin feature1 = Feature(mean=Matrix([0,25,161,77,137]), covar=preset_covariance) feature1.__immutable__ = True # blue feature2 = Feature(mean=Matrix([10,25,75,55,230]), covar=preset_covariance) feature2.__immutable__ = True # green feature3 = Feature(mean=Matrix([0,15,82,120,68]), covar=preset_covariance) feature3.__immutable__ = True # pink feature4 = Feature(mean=Matrix([10,15,224,37,192]), covar=preset_covariance) feature4.__immutable__ = True features = [feature1, feature2, feature3, feature4] self.initialize_particle_filter(features) # begin ros updating self.cam_sub = rospy.Subscriber('/camera/features', VizScan, self.measurement_update) self.twist_sub = rospy.Subscriber('/cmd_vel', Twist, self.motion_update) self.odom_pub = rospy.Publisher('/slam_estimate', Odometry, queue_size=1) def run(self): joke_rate = rospy.Rate(10) if self.core is not None: rospy.loginfo('Running!') self.print_summary() while (not rospy.is_shutdown()) and (self.last_sensor_reading is None): rospy.loginfo('waiting on the first sensor data') if (rospy.Time.now().to_sec() > 40): return 10 joke_rate.sleep() while not rospy.is_shutdown(): self.loop_over_particles() rospy.loginfo('joke rate stop?') if (rospy.Time.now().to_sec() > 40): # rospy.loginfo('... yes') return 10 joke_rate.sleep() rospy.loginfo('exited main loop. Done!') def loop_over_particles(self): rospy.loginfo('main loop passing of sensor data to SLAM') self.core.cam_cb(self) self.odom_pub.publish(self.easy_odom()) def initialize_particle_filter(self, preset_features): ''' Create an instance of FastSLAM algorithm ''' self.core = FastSLAM(preset_features) def easy_odom(self): x, y, heading = self.core.summary() otto = Odometry() otto.header.frame_id = 'odom' otto.header.stamp = rospy.Time.now() otto.pose.pose.position.x = x otto.pose.pose.position.y = y otto.pose.pose.orientation = heading_to_quaternion(heading) return otto def measurement_update(self, msg): ''' Pass along a VizScan message to the main running loop ''' # self.core.cam_cb(msg) # rospy.loginfo('new sensor data...') self.last_sensor_reading = msg odom = self.easy_odom() self.odom_pub.publish(odom) def motion_update(self, msg): ''' Pass along a Twist message ''' # rospy.loginfo('motion_update('+str(msg)+')') self.core.motion_update(msg) # self.print_summary() odom = self.easy_odom() self.odom_pub.publish(odom) def print_summary(self): ''' average x, y, heading ''' rospy.loginfo('prkt_summary: '+str(self.core.summary()))
def test_initilization(self): fs = FastSLAM() self.assertIsInstance(fs.last_control, Twist) self.assertIsInstance(fs.last_update, rospy.Time) self.assertIsInstance(fs.particles, list) self.assertIsInstance(fs.Qt, np.ndarray)