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