MicroRTPS Vehicle_visual_odometry, EKF2 not working

1

Hi all,

I want to get local position messages after EKF2 filtering. I Publish the vehicle_visual_odometry messages but nothing changes in the vehicle_odometry and vehicle_local_position messages.

My Env:
Ubuntu 18.04
ROS Melodic
FastDDS-2.0.2
Fast-RTPS-Gen
QGroundControl v4.2.0
PX4 - V 1.12.3 / make px4_fmu-v5_rtps

hardware: Pixhawk 4 Mini

vehicle setup:
EKF2_AID_MASK = 24 (vision position fusion and vision yaw fusion were activated)
MAV_ODOM_LP = 1 or MAV_ODOM_LP = 0

create an bridge app with "fastrtpsgen -example x64Linux2.6gcc …/micrortps_agent/idl/vehicle_visual_odom.idl " command

start the client with: micrortps_client start -d /dev/ttyS1 -t UART -b 460800
start the agent with: ./micrortps_agent -t UART -d /dev/ttyUSB0 -b 460800

PS: Other baud rates do not work I could not figure it out.

In my publisher node fill the vehicle_visual_odometry object and publish it. The agent sent the messages to the client and I see the vehicle_visual_odometry messages in QGroundControl.
But I could not see any changes in the vehicle_odometry messages in QGroundControl.
You could see the changes in the image below.

visual_odometry
visual_odometry1843×1049 174 KB

In QGroundControl I see only Odometry messages in Mavlink Inspector->ODOMETRY you could see in the image below.

odometry
odometry1838×1024 130 KB

Do you have any idea or suggestion for getting the local position & odometry messages (filtered via ekf2)?

2

Hi there!
I had something similar, where I found out that I didn’t assign the timestamp property of the vehicle_visual_odometry correctly. If these are not correct, the EKF2 will not take them into account.

3

got the timestamp from Timesync subscriber node (px4_msgs Timesync message-timestamp variable). But nothing changed.
(don’t know what is the timestamp_sample_ )

    vehicle_visual_odometry st;
    st.timestamp_(timestamp);
    st.timestamp_sample_(timestamp);

    st.x_(position.x());
    st.y_(position.y());
    st.z_(position.z());

    std::array<float, 4> q = {rpy.x(), rpy.y(), rpy.z(), 1};
    st.q(q);

    mp_publisher->write(&st);
4

So also with adding the timestamp_sample it doesn’t work?
I think this value represents the time at which the visual_odometry measurement was gathered, so it is important that this one is also properly set. If you look at the images you sent before, you could see that the timestamp_sample there was 0 (6989… us before timestamp), so the EKF disregards it as it is too old.

5

Yes, I set the time_stamp from the Timesync from micrortps_client and it does not affect the visual_odom. Actually, I expect visual_local_position at the QGroundControl → Mavlink Inspector tab.

The timestamp/timestamp_sample 0 and (6989) are different because I set the timestamp_sample after my post.

Also, I created a vehicle_local_position listener app via RTPS code gen.
I got the messages but only the Z index was changed.

Another Note: I publish dummy positions (incremental +0.5 x,y&z ), Is this scenario affects the EKF2?

    transform_stamped = *msg;
    // This block from mavros source code --- start  
    Eigen::Affine3d tr;
    tf::transformMsgToEigen(transform_stamped.transform, tr);

    auto position = mavros::ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
    auto quat = mavros::ftf::transform_orientation_enu_ned(
        mavros::ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
   // --- end  

    ROS_INFO("timestamp: [%lli] ", timestamp);
    st.timestamp_(timestamp);
    st.timestamp_sample_(timestamp);

    st.x_(position.x());
    st.y_(position.y());
    st.z_(position.z());

    std::array<float, 4> q = {quat.x(), quat.y(), quat.z(), quat.w()};
    st.q(q);