Robot_localization seems to ignore some sensors

edit

Hello everyone,

I try do to a sensor fusion and I decided to use the robot_localization package. I have done a simulation with few sensor inside (2 "depth sensors", 2 IMUs, and a "DVL". And I made it worked ! But now I received some real sensors and I try to implement them in r_l with a posewithcovariancestamped and a twistwithcovariancestamped msgs but it doesn't work. It seems to me that r_l just ignore the data or doesn't manage to include them in the ekf. However, when I look to the debug file I can see them but I doesn't know where it goes wrong.

The main part of the code that publish my sensor data (I set use_sim_time to true when I run the node):

def __init__(self):
        super().__init__('dvl_pub')
        self.publisher_pose_nucleo1000 = self.create_publisher(PoseWithCovarianceStamped, 'pose_nucleo1000', 10)
        self.publisher_twist_nucleo1000 = self.create_publisher(TwistWithCovarianceStamped, 'twist_nucleo1000', 10)
        self.publisher_real_imu_nucleo1000 = self.create_publisher(Imu, 'real_imu_nucleo1000', 10)
        timer_period = 0.01  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.driver = NucleusDriver()
        self.driver.start_measurement()
        self.real_imu_nucleo1000=Imu()
        self.pose_nucleo1000 = PoseWithCovarianceStamped()
        self.twist_nucleo1000 = TwistWithCovarianceStamped()
        self.pose_nucleo1000.pose.covariance=[0.0]*36
        self.pose_nucleo1000.pose.covariance[14]=0.9
        self.pose_nucleo1000.header.frame_id="pose_nucleo1000"
        self.twist_nucleo1000.header.frame_id="twist_bt_nucleo1000"
        self.real_imu_nucleo1000.header.frame_id="real_imu_nucleo1000"
        self.pose_nucleo1000.pose.covariance=[0.0000001]*36
        self.tf_broadcaster = TransformBroadcaster(self)
  def timer_callback(self):
                packet = self.driver.read_packet()
              self.pose_nucleo1000.header.stamp=self.get_clock().now().to_msg()
                self.real_imu_nucleo1000.header.stamp=self.get_clock().now().to_msg()
                ned_quat = np.array([float(packet["ahrsData.quaternionW"]), float(packet["ahrsData.quaternionX"]),float(packet["ahrsData.quaternionY"]), float(packet["ahrsData.quaternionZ"])])
                ned_rot_matrix = quaternions.quat2mat(ned_quat)
                # Transform the rotation matrix to the ENU frame
                enu_rot_matrix = np.matmul(ned_to_enu, ned_rot_matrix)
                # Convert the rotation matrix back to a quaternion
                enu_quat = quaternions.mat2quat(enu_rot_matrix)
                self.pose_nucleo1000.pose.pose.orientation.w= enu_quat[0]
                self.pose_nucleo1000.pose.pose.orientation.x= enu_quat[1]
                self.pose_nucleo1000.pose.pose.orientation.y= enu_quat[2]
                self.pose_nucleo1000.pose.pose.orientation.z= enu_quat[3]
                self.real_imu_nucleo1000.orientation=self.pose_nucleo1000.pose.pose.orientation
                self.twist_nucleo1000.twist.twist.angular.x= float(packet["turnRateX"])
                self.twist_nucleo1000.twist.twist.angular.y= float(packet["turnRateY"])
                self.twist_nucleo1000.twist.twist.angular.z= float(packet["turnRateZ"])
                self.pose_nucleo1000.pose.pose.position.z=packet["depth"]


self.publisher_pose_nucleo1000.publish(self.pose_nucleo1000)
self.publisher_twist_nucleo1000.publish(self.twist_nucleo1000)
self.publisher_real_imu_nucleo1000.publish(self.real_imu_nucleo1000)

I have checked some quaternions and they are normalized.

My ekf yaml that works with only the simulated sensor :

ekf config file
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 2.0
        two_d_mode: false
        print_diagnostics: true
        smooth_lagged_data: true
        debug: true
        publish_tf: true

    odom_frame: odom            
    base_link_frame: main_buddy_link  
    world_frame: odom        


    pose0 : /buddy_sim/depth_Nucleo1000
    pose0_config: [false, false,  false,
                   true, true, true,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    pose0_queue_size: 10
    pose0_differential: false 

    twist0: /buddy_sim/vel_dvl
    twist0_config: [false, false, false,
                    false, false, false,
                    true,  true,  true,
                    false, false, false,
                    false, false, false]
    twist0_queue_size: 2

    imu0: /buddy_sim/imu_XsensMti3Click
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_differential: true
    imu0_relative: false
    imu0_queue_size: 10

    acceleration_limits: [3.0 ...