Servers by jDrones

VISION_POSITION_ESTIMATE not appearing in QGroundControl

(Jeff Smith) #1

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.

2 Likes
How can I correct the LOCAL_POSITION_NED from an external vision system
(Jeff Smith) #2

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:

  1. Ensure that vision* and local* are whitelisted in mavros’ plugins file
  2. Launch mavros with fcu_protocol:=“v2.0”
  3. 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.

  1. 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.
  2. 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)
1 Like
Position Hold with VISION_POSITION_ESTIMATE and ATT_POS_MOCAP for indoor localization
(Andrea Belloni) #3

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?

(mzahana) #4

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 ?

(Andrea Belloni) #5

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.

(rmackay9) #6

Andrea,

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.

(rmackay9) #7

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.

2 Likes
(Andrea Belloni) #8

Thanks a lot for your explanation, I remain with a doubt, when indoor which values I have to pass to SET_GPS_GLOBAL_ORIGIN?

(Jeff Smith) #9

@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:

mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo;
gpo.target_system = m_uas->get_tgt_system();
// gpo.time_boot_ms = stamp.toNSec() / 1000;    #TODO: requires Mavlink msg update
gpo.latitude = 37.2343 * 1E7;
gpo.longitude = -115.8067 * 1E7;
gpo.altitude = 610.0 * 1E3; // roughly 610m above sea level

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.

(ppoirier) #10

LOL Love this place … It smells like OuterSpace and Skunk !!! :wink:

1 Like
(Andrea Belloni) #11

@jeff_null thanks a lot now is all clear

(Andrea Belloni) #12

@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.

Thanks
Andrea

1 Like
(ppoirier) #13

@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.

@rmackay9 I think that you plan to push in that direction with next GSOC
http://ardupilot.org/dev/docs/gsoc-ideas-list.html

  • Object Avoidance improvements for Multicopters and Rovers including adding occupancy grid using OctoMap or ROS.
  • Improve ROS integration and documentation

@fnoop, I guess you agree with this too :wink:

3 Likes
(rmackay9) #14

@anbello, @ppoirier,

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.

2 Likes
(rmackay9) #15

I chatted with Paul and we’re going to bump up the priority on getting support for the vision-position-estimate message into ardupilot.

2 Likes
(ppoirier) #16

@rmackay9 that is great news !!

(Jeff Smith) #17

@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.

(Andrea Belloni) #18

Thanks @jeff_null for detailed explanation, now I have to pass from theory to practice :slight_smile:
Thanks @rmackay9 when for the vision-position-estimate message will be supported in ardupilot this stuff will be easier, also for Jeff.

(Jeff Smith) #19

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

(Subodh Mishra) #20

I am doing the same, were you able to do this with apm firmware. I did it with px4 but I need to do it with apm.