Add ability to navigate using MAVLink vision messages #7980

peterbarker · 2018-03-21T06:11:31Z

We accept messages of various sorts, and feed the data into EKF2, which can fuse this data and allow us to navigate.

Preliminary instructions on testing this are here:
https://github.com/peterbarker/ardupilot_wiki/blob/vicon/dev/source/docs/using-sitl-for-ardupilot-testing.rst#testing-visual-positioning

Notes:

arming in loiter is not possible unless GPS checks are disabled

Edit: the earlier issue, in which the vehicle could not climb in Loiter mode while using external position estimates, has been resolved.

rmackay9 · 2018-03-21T07:34:09Z

This is a really important feature. This will enable the ROS users to feed in the SLAM position estimates to ArduPilot.

khancyr · 2018-03-21T09:03:27Z

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -191,6 +191,19 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
    }
 }

+#define streq(a, b) (!strcmp(a, b))


can we remove this define ?

it is used only once !

khancyr · 2018-03-21T09:08:51Z

libraries/SITL/SIM_Vicon.cpp

+
+    // push a reading into the socket...
+
+    bool should_send_heartbeat = 0;


this look weird

Yep, this means it never sends an heartbeat... (and it should be false in any case).

khancyr · 2018-03-21T09:15:12Z

libraries/SITL/SIM_Vicon.h

+
+private:
+
+    const uint8_t system_id = 17;


maybe add a todo for a param table to handle that

peterbarker · 2018-03-21T10:01:31Z

On Wed, 21 Mar 2018, Pierre Kancir wrote: +#define streq(a, b) (!strcmp(a, b)) can we remove this define ?

Only if we can get from somewhere else :-P

+ + // push a reading into the socket... + + bool should_send_heartbeat = 0; this look weird

Will fix.

+ const uint8_t system_id = 17; maybe add a todo for a param table to handle that

Done! Force-pushed.

peterbarker · 2018-03-21T10:01:59Z

On Wed, 21 Mar 2018, Pierre Kancir wrote: +#define streq(a, b) (!strcmp(a, b)) it is used only once !

So far :-) But my sds021 branch would use it once rebased on this.

rmackay9 · 2018-03-24T00:57:33Z

@OXINARF, if you get a chance to review this that would be really really appreciated!

vincekurtz · 2018-03-24T01:47:30Z

As requested in #7997 by @peterbarker, I'm trying to test this functionality but running into some trouble.

I'm able to send VISION_POSITION_ESTIMATE and ATT_POS_MOCAP messages generated by an Optitrack mocap system via mavros, and can view them in QGroundControl's MavLink inspector. I'm also able to run the preliminary tests without any problem.

But on the actual system (pixhawk 2.1, custom quad frame, mavlink connection on Telem2), I never get the APM: EKF2 IMU0 is using external nav data ... messages. I'm guessing my setup is not quite right: any guidance would be much appreciated!

Parameters: mocap_message_tests.txt
Logs: logs.zip

OXINARF

Besides inline comments:

why isn't this in EKF3? Looks like that could lead to a regression when we change to using it
I have no issue with how SITL Vicon simulation is done, but it's different from other simulated devices
SITL Vicon seems to have an old idea of keeping an observation history that is unused

OXINARF · 2018-03-24T18:27:34Z

libraries/AP_NavEKF2/AP_NavEKF2.cpp

+/*
+ * Write position and quaternion data from an external navigation system
+ *
+ * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)


There is no frameIsNED argument.

OXINARF · 2018-03-24T18:29:17Z

libraries/AP_NavEKF2/AP_NavEKF2.cpp

+ * Write position and quaternion data from an external navigation system
+ *
+ * pos        : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m)
+ * quat       : quaternion desribing the the rotation from navigation frame to body frame


desribing -> describing

OXINARF · 2018-03-24T19:01:44Z

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

+                // handle yaw reset as special case
+                extNavYawResetRequest = true;
+                controlMagYawReset();
+                // handle height reset as sepcial case


sepcial -> special

OXINARF · 2018-03-24T19:05:27Z

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -169,7 +169,8 @@ void NavEKF2_core::setAidingMode()
        // GPS aiding is the preferred option unless excluded by the user
        bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
        bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
-        if(canUseGPS || canUseRangeBeacon) {
+        bool canUseExtNav = readyToUseExtNav() && tiltAlignComplete;


tiltAlignComplete is already checked in readyToUseExtNav.

OXINARF · 2018-03-24T19:07:37Z

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -500,7 +520,7 @@ void  NavEKF2_core::updateFilterStatus(void)
    filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
    filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
    filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
-    filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
+    filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // The GPS is glitching


This doesn't make much sense to me, just because we are using external navigation, the GPS might be glitching anyway. If anything, it seems to make more sense in the line above, using_gps.

The problem is how that flag is used. With copter it causes a failsafe reversion into alt hold mode and will prevent takeoff. The EKF2 will never use external nav and GPS at the same time, so when external nav is being used, GPS status is irrelevant.

The better way to describe the gps_glitching flag is being 'true when GPS glitching is affecting navigation accuracy'. I will update comments to make this clearer.

OXINARF · 2018-03-24T20:20:32Z

libraries/SITL/SIM_Vicon.h

+    struct {
+        const uint16_t observation_interval_ms = 20;
+        // delay results by some multiplier of the observation_interval:
+        const uint8_t observation_history_length = 0;


If this is const, how does it change? It looks unused...

OXINARF · 2018-03-24T20:20:51Z

libraries/SITL/SIM_Vicon.h

+        const uint16_t observation_interval_ms = 20;
+        // delay results by some multiplier of the observation_interval:
+        const uint8_t observation_history_length = 0;
+        ObjectArray<obs_elements> observation_history{observation_history_length};


An array of 0?

OXINARF · 2018-03-24T20:21:09Z

libraries/SITL/SIM_Vicon.h

+        Quaternion  attitude;
+    };
+
+    bool observation_history_initialised;


OXINARF · 2018-03-24T20:22:33Z

libraries/SITL/SIM_Vicon.cpp

+        observation = new_obs;
+    } else {
+        vicon.observation_history.push(new_obs);
+        vicon.last_observation_ms = now;


This should always be updated, not just when history would be used.

OXINARF · 2018-03-24T20:24:02Z

libraries/SITL/SIM_Vicon.h

+    // we share channels with the ArduPilot binary!
+    const uint8_t mavlink_ch = 17;
+
+    uint32_t last_update_ms;


peterbarker · 2018-03-26T02:56:32Z

On Sat, 24 Mar 2018, Francisco Ferreira wrote: * I have no issue with how SITL Vicon simulation is done, but it's different from other simulated devices

I'm hoping to have other things act similarly. Notably the GPS stuff.

* SITL Vicon seems to have an old idea of keeping an observation history that is unused

It does work, but is a compile-time change. I've pushed up a patch on top which makes it a parameter instead ("reboot" required ;-) )

I was expecting this would go through the AP_VisualOdom library which already have those parameters.

My understanding is that VisualOdometry (by its name if nothing else) will work with deltas, not position estimates.

+ bool should_send_heartbeat = 0; Yep, this means it never sends an heartbeat... (and it should be false in any case).

This was a just-in-case-it's needed thing. I've commented it back in, so it now sends heartbeats.

+ const uint8_t observation_history_length = 0; If this is const, how does it change? It looks unused...

As noted, this was a compile-time option for testing.... it's now a parameter.

+ bool observation_history_initialised; Unused

Removed.

+ vicon.last_observation_ms = now; This should always be updated, not just when history would be used.

Will fix.

+ uint32_t last_update_ms; Unused

Will remove.

priseborough · 2018-03-26T03:05:10Z

There was a bigger change under development for EKF3 that adds an additional state for scale factor estimation of monocular SLAM and VO. Adding changes from this PR to EKF3 would cause pain in rebasing.

rmackay9 · 2018-03-27T11:37:52Z

Merged after removing squashing the last commit and removing the, "DataFlash: Add debug loggin for EKF external nav fusion". Thanks @priseborough and @peterbarker!

peterbarker · 2018-03-30T06:44:40Z

On Sat, 24 Mar 2018, vincekurtz wrote: As requested in #7997 by @peterbarker, I'm trying to test this functionality but running into some trouble.

Thanks for testing!

I'm able to send VISION_POSITION_ESTIMATE and ATT_POS_MOCAP messages generated by an Optitrack mocap system via mavros, and can view them in QGroundControl's MavLink inspector. I'm also able to run the preliminary tests without any problem.

Would be nice if we can get those fed into SITL!

But on the actual system (pixhawk 2.1, custom quad frame, mavlink connection on Telem2), I never get the APM: EKF2 IMU0 is using external nav data ... messages. I'm guessing my setup is not quite right: any guidance would be much appreciated!

Are you convinced that mavlink connection was solid? I assume there were two mavlink connections involved here? The tlog you sent through didn't seem to be carrying vision data of any sort - and given the lack of target ids I think it would have to. Also missing is the XKCF message in the dataflash log - not one, and this is probably same root cause as not getting the magic EKF-is-using messages.

vincekurtz · 2018-04-04T17:25:55Z

@peterbarker - I'm fairly certain the mavlink connection is solid, but I'm still unable to use ATT_POS_MOCAP or VISION_POSITION_ESTIMATE messages. I've set up a minimal working example using mavros, mavros_extras, python, and SITL as follows:

Test Example

Set up SITL:

sim_vehicle.py -v ArduCopter -w
STABILIZE> param set GPS_TYPE 0
STABILIZE> param set EK2_GPS_TYPE 3
sim_vehicle.py -v ArduCopter

Connect via mavros (using the px4 launch file because it enables the mocap_pose_estimate plugin by default - using a modified apm launch file gives the same results)

roslaunch mavros px4.launch fcu_url:=udp://127.0.0.1:14551@14555

Run a python script that publishes to the /mavros/mocap/pose topic: mavros will use this to generate and send a ATT_POS_MOCAP message.

test_pose_publisher.py:

import rospy
from geometry_msgs.msg import PoseStamped
import time

topic = "/mavros/mocap/pose"
pose_data = PoseStamped()

try:
    rospy.init_node("test_pose_publisher")
    pose_pub = rospy.Publisher(topic, PoseStamped, queue_size=10)

    rate = rospy.Rate(30)  # in Hz
    pose_data.header.frame_id = "map"
    pose_data.pose.position.x = 2

    start_time = rospy.get_rostime()
    while not rospy.is_shutdown():
        now = rospy.get_rostime() - start_time
        pose_data.header.stamp.secs = now.secs
        pose_data.header.stamp.nsecs = now.nsecs
        
        pose_pub.publish(pose_data)
        
        rate.sleep()

except rospy.ROSInterruptException:
    pass

Results

By putting gcs().send_text(MAV_SEVERITY_INFO, "..."); statements at various points in the code, I can tell that the handle_att_pos_mocap() function of GCS_MAVLink/GCS_Common.cpp is being called and that the data matches what is being published on the ROS topic.

I'm able to trace the progress of the (fake) mocap information as far as the SelectVelPosFusion() function of AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp. At this point, it appears that extNavDataToFuse is never set to 1.

rmackay9 · 2018-04-05T06:29:12Z

@vincekurtz, it's really great that you're testing this, thanks!

We think the issue may be the timestamps in the messages. They need to be synced up with the system time of the flight controller/ArduPilot. If the times are very far off the EKF will simply not consume the data.

If you're using a Rover or Copter on a Pixhawk (or compatible) could you try this firmware?

It's just master with two patches on top to ignore the time in the message and instead uses the system time. This won't work really well but it should basically work. https://github.com/rmackay9/rmackay9-ardupilot/commits/visp-log

vincekurtz · 2018-04-05T20:08:58Z

Logs from two flights using the patched firmware are attached. I was able hold altitude and position, but performance was relatively poor considering the accuracy of the mocap system. Altitude tended to drift up and down about a meter, and position hold was plagued by circular oscillations (especially evident in the second test flight).

I'm not sure how much the poor performance is due to my setup (I didn't disable baro or mess with any of the EKF noise parameters) and how much is due to delays between measurement and use in the EKF that aren't taken into account.

extNavTest-ignore_time.zip

rmackay9 · 2018-04-06T02:04:17Z

@vincekurtz, I guess you used a different firmware from the one I linked above? sorry, just having trouble finding the VISP message. I've merged the VISP message to master so you could just use master if you like. The system time isn't used in master though so using master requires ensuring that somehow the timestamps match the flight controller's time.

vincekurtz · 2018-04-06T16:41:01Z

@rmackay9, it looks like the VISP message doesn't get logged for the ATT_POS_MOCAP message type, which is what I was using. Specifically, the handle_att_pos_mocap() function doesn't call _handle_common_vision_position_estimate_data(), presumably because ATT_POS_MOCAP gives quaternion attitude information from the start.

That should be an easy fix, but for now I'll try another test using the VICON_POSITION_ESTIMATE message, which does use _handle_common_vision_position_estimate_data().

vincekurtz · 2018-04-06T17:42:49Z

Flight log using VICON_POSITION_ESTIMATE is attached. Performance was similar to using ATT_POS_MOCAP: there were large (but generally bounded) oscillations in position hold mode.

extNav-vision_position_estimate.zip

rmackay9 · 2018-04-18T04:45:17Z

@vincekurtz, we've got this PR which we hope will allow syncing the clocks between the two computers. It's being reviewed now and I hope it will go into master later this week. Txs for your patience and testing!

rmackay9 requested a review from OXINARF March 21, 2018 07:32

rmackay9 added Enhancement EKF AllVehicles labels Mar 21, 2018

khancyr approved these changes Mar 21, 2018

View reviewed changes

peterbarker force-pushed the ekfExtNav-wip branch from 62773a6 to a703171 Compare March 21, 2018 10:01

peterbarker force-pushed the ekfExtNav-wip branch from a703171 to 3467e5b Compare March 23, 2018 00:35

peterbarker mentioned this pull request Mar 23, 2018

Unable to set EK3_GPS_TYPE = 0 when using mavlink HIL_GPS message #7997

Closed

peterbarker force-pushed the ekfExtNav-wip branch from 3467e5b to 41887c1 Compare March 23, 2018 03:14

OXINARF reviewed Mar 24, 2018

View reviewed changes

peterbarker and others added 9 commits March 26, 2018 13:27

AP_Math: quaternion: add contructor from float[4]

ad21bb2

SITL: method to return current vehicle location

d4838f1

DataFlash: Add debug logging for EKF external nav fusion

ec0183f

AP_AHRS: accept external position estimates into AHRS

42865b5

AP_AHRS_NavEKF: accept external position estimates into AHRS

edbd929

AP_NavEKF2: Enable fusion of external nav position data

3eb4eb2

GCS_MAVLink: feed vision position data into AHRS

585e535

AP_HAL_SITL: sim: namespace for specifying SITL devices from cmdline

5bbacc3

AP_HAL_SITL: add vicon-over-mavlink support

a46e22e

peterbarker force-pushed the ekfExtNav-wip branch from 41887c1 to 0b90865 Compare March 26, 2018 02:39

peterbarker added 2 commits March 26, 2018 13:53

SITL: add vicon-over-mavlink support

07a6342

SITL: add parameter for Vicon observation delay

7bf1a39

peterbarker force-pushed the ekfExtNav-wip branch from 0b90865 to 7bf1a39 Compare March 26, 2018 02:55

priseborough added 6 commits March 26, 2018 20:16

AP_NavEKF: Clarify definition for gps_glitching flag

49a0f08

Copter: Clarify definition for gps_glitching flag

96e7726

AP_Notify: Clarify definition for gps_glitching flag

b378528

AP_NavEKF3: Clarify definition for gps_glitching flag

8d21098

AP_NavEKF2: Clarify definition for gps_glitching flag

655e5b0

AP_NavEKF2: Address minor review comments

ba96ab0

rmackay9 closed this Mar 27, 2018

vincekurtz mentioned this pull request Apr 10, 2018

GCS_MAVLink: VISP data not logged with ATT_POS_MOCAP message #8132

Closed

chobitsfan mentioned this pull request Jun 19, 2018

Copter: support MAV_CMD_NAV_TAKEOFF_LOCAL #8683

Closed

peterbarker deleted the ekfExtNav-wip branch January 21, 2021 11:21


		// push a reading into the socket...

		bool should_send_heartbeat = 0;

Add ability to navigate using MAVLink vision messages #7980

Add ability to navigate using MAVLink vision messages #7980

Conversation

peterbarker commented Mar 21, 2018 • edited by rmackay9

rmackay9 commented Mar 21, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

peterbarker commented Mar 21, 2018 via email

peterbarker commented Mar 21, 2018 via email

rmackay9 commented Mar 24, 2018

vincekurtz commented Mar 24, 2018

OXINARF left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

peterbarker commented Mar 26, 2018 via email

priseborough commented Mar 26, 2018

rmackay9 commented Mar 27, 2018

peterbarker commented Mar 30, 2018 via email

vincekurtz commented Apr 4, 2018

Test Example

Results

rmackay9 commented Apr 5, 2018

vincekurtz commented Apr 5, 2018

rmackay9 commented Apr 6, 2018 • edited

vincekurtz commented Apr 6, 2018

vincekurtz commented Apr 6, 2018

rmackay9 commented Apr 18, 2018

peterbarker commented Mar 21, 2018 •

edited by rmackay9

rmackay9 commented Apr 6, 2018 •

edited