I am using Rover-3.2.0-rc3 on an Aion rover with an attached stereo camera. I am also running SLAM on an onboard processor, and feeding the estimated visual pose to mavros via the ROS topic “/mavros/vision_pose/pose”. I have instrumented the mavros code, so I am certain that these pose estimates are being received, and the appropriate MAVLink message (VISION_POSITION_ESTIMATE) is being sent. However, this message does not appear in QGroundControl’s MAVLink inspector, and the expected “/mavros/local_position/pose” topic is not being published.
I have successfully done this on a different platform using PX4, so the issue must be in APM or my use of it. Has anyone successfully used external vision-based pose estimates with Rover?
EDIT: I have also implemented a Python program, using pymavlink, to listen to all MAVLINK messages, and the VISION_POSITION_ESTIMATE messages do not show up there, either. So it is not a QGroundControl problem.
I have (mostly) figured this out, and thought I should share for people who come across this page in the future. There were several parts to getting this working:
Ensure that vision* and local* are whitelisted in mavros’ plugins file
Launch mavros with fcu_protocol:=“v2.0”
Ensure that the SERIAL*_PROTOCOL firmware parameters are set to “MAVLink2” (value=2)
MAVROS needs several changes to its code to get it working, so you’ll need to download and edit the code.
In mavros/mavros_extras/src/plugins/vision_pose_estimate.cpp, in the function ‘vision_position_estimate’, send VISION_POSITION_DELTA messages instead of VISION_POSITION_ESTIMATE. APM doesn’t listen for the latter message.
Before you start sending these messages, send a ‘mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN’ Mavlink message, to set the origin for the EKF. You will have to hard-code the lat/long coordinates you wish to send (or maybe make them a mavros parameter if you’re feeling ambitious)
I was thinking (for now only thinking) to use a visual odometry system on a quadcopter with arducopter and a companion computer running ROS for indoor autonomous flight. I had the same doubt about the message to use, thanks for the explanation.
I would like to know if the last point (2.) is necessary also for an indoor system, and in case can I use 0,0,0 for home position?
I have vision pose estimates feed working fine, but with PX4, on a quadcopter for a mocap setup. Just out of curiosity what VO framework are you using, @anbello ?
For now I am only thinking about, I would like to try SVO2, but I think as first setup I will use pose estimate from fiducial markers and aruco (simpler), to see how to do all the information exchange between flight controller and companion computer with mavros using the right messages.
Point 2 (send a ‘mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN’) is required although it can also be done through the mission planner after each reboot of the flight controller… http://ardupilot.org/copter/_images/zed-set-ekf-origin.png
@jeff_null, thanks very much for sharing your experiences.
Re Non-GPS navigation with Rover, I’ve recently found this issue which stops the vehicle from going into autonomous and semi-autonomous modes (Auto, Guided, RTL, SmartRTL, Steering) unless a GPS is connected.
I’m working on a fix as a high priority. I hope to release a Rover-3.2.1-rc1 beta version this week or next.
@anbello: You can pass any value you’d like, but I’d recommend passing your real lat/long coordinates. ArduPilot will use these values to determine the magnetic declination (via a simple look-up table), so if you use a location that is distant from where you are, you will have inaccurate compass headings. Here is the code I use:
As Randy mentioned, this can also be done in Mission Planner, but it has to be done every time your controller (in my case, a PixHawk2) is powered on. So I find it more convenient to do it on code.
This message only needs to be sent once, and is only used to initialize the EKF. All of our control is local; we never use or query the global position estimates.
@jeff_null VISION_POSITION_ESTIMATE message get seven parameters: t, x, y, z, r, p, y whereas VISION_POSITION_DELTA get nine parameters t, delta_t, delta_x, delta_y, delta_z, delta_r, delta_p, delta_y, confidence. I would like to know how did you manage this.
@rmackay9 I would like to know if VISION_POSITION_DELTA will be the only message for visual odometry in Ardupilot or do you think that VISION_POSITION_ESTIMATE will be implemented, it could be more simple to use because it doesn’t need any modification in mavros code.
@anbello I cannot agree more with you, not having the standard VISION_POSITION_ESTIMATE message implemented is problematic because it prevent usage of standard Visual Odometry systems.
Yes, in fact, it’s already on @priseborough’s list. I think it’s 2nd on his list so hopefully it won’t be too long. My guess is it’s several weeks away still but not months and months.
@anbello As you note, VISION_POSITION_DELTA requires seven parameters: the position delta, the rotation delta, and a confidence value. The confidence value is a measurement of your visual odometry quality and accuracy, that should range from 0 to 100. I pass in ‘90,’ because my visual odometry system is pretty good, but I haven’t noticed different results if I pass in ‘50’ or ‘70.’
The position delta is easy: it’s the vector from the previous position to the current position in the frame of reference of the previous position. Thus, moving in straight line always gives you +X (we’re in a NED frame). Moving laterally to the right would give us +Y, etc.
Rotation is trickier, and I’m still not certain I’m doing it correctly. It should be just the rotation delta in the same frame of reference as the position; i.e. a triplet of Euler angles about the local X, Y and Z axes. However, in MAVROS the rotation is flipped 180 degrees around the X (forward) axis before the absolute visual pose is sent. (There is no documentation as to why this is.) So, should the vision delta also be computed in this flipped (NWU) frame of reference? I don’t know.
Thanks @jeff_null for detailed explanation, now I have to pass from theory to practice
Thanks @rmackay9 when for the vision-position-estimate message will be supported in ardupilot this stuff will be easier, also for Jeff.
Something else to be aware of: if your visual odometry system provides updates slower than 10 Hz, I suspect the EKF sensor fusion will not work properly. There’s a hard cutoff, hardcoded within the ring buffer’s definition of ‘recall’, that will ignore visual odometry (or any body odometry) poses that are older than 100ms (since the last EKF update).
