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)
Example #2
0
 def initialize_particle_filter(self, preset_features):
     '''
     Create an instance of FastSLAM algorithm
     '''
     self.core = FastSLAM(preset_features)
Example #3
0
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()))
Example #4
0
 def initialize_particle_filter(self, preset_features):
     '''
     Create an instance of FastSLAM algorithm
     '''
     self.core = FastSLAM(preset_features)
Example #5
0
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)