def __init__(self, name): # Get config self.config = girona500.get_config() # Main SLAM worker self.slam_worker = self.init_slam() # Structure to store vehicle pose from sensors self.vehicle = PARAMETERS() # roll, pitch and yaw rates - equivalent to odom.twist.twist.angular self.vehicle.twist_angular = np.zeros(3, dtype=float) self.vehicle.twist_linear = np.zeros(3, dtype=float) # position x, y, z - the state space estimated by the filter. self.vehicle.pose_position = np.zeros(3, dtype=float) # orientation quaternion x,y,z,w - same as odom.pose.pose.orientation self.vehicle.pose_orientation = np.zeros(4, dtype=float) # Altitude from dvl self.vehicle.altitude = 0.0 # Initialise ROS stuff self.ros = PARAMETERS() self.ros.name = name self.ros.last_update_time = rospy.Time.now() self.ros.NO_LOCK_ACQUIRE = 0 self.init_ros() self.__LOCK__ = threading.Lock() # Set as true to run without gps/imu initialisation self.config.init = False
def __init__(self, name): """ Merge different navigation sensor values """ self.name = name self.config = girona500.get_config() config = self.config #Init Kalman filter self.filter = PFG500(config.model_covariance, config.gps_covariance, config.dvl_bottom_covariance, config.dvl_water_covariance) self.init_ros() self.LOCK = threading.RLock()