Servers by jDrones

VISION_POSITION_ESTIMATE not appearing in QGroundControl


(mike) #59

@rmackay9 Thank you very much for the firmware. I will test it with my copter soon.
I’ll just joined the gitter.

A rosbag is a recorded data set, and which can be replayed with the exact timing at any time


(Jeff Smith) #60

@rmackay9 Thanks for following up on this. I tried the new firmware with our existing system and – alas – it does not solve the EKF problems we are seeing. Specifically:

  • If I send VISION_DELTA_ESTIMATE messages, I still see the same poor fused results of our visual odometry and the IMU signals.
  • If I send VISION_POSE_ESTIMATE messages, the EKF never produces a fused result.

It’s possible there’s an unrelated problem on our end; it’s been a few months since I last dug into this. I can email you dataflash logs if you’re curious.


(rmackay9) #61

Jeff,
Thanks for giving it a try. If you have the dataflash logs from the 2nd one using “VISION_POSITION_ESTIMATE” that would be great.


(Jeff Smith) #62

Big thanks to @rmackay9 for looking at my dataflash log. It turns out, I was still using the EKF3; but the fixed vision-position-estimate work has been done in the EKF2. Switching to this EKF has fixed my problems:

In this image, the red trace is the output from my visual odometry system. The blue trace is the fused visual-inertial position estimate. (Units are in meters). I’ve run a few of these tests to make sure I wasn’t just getting lucky, but the results are consistent.

Thanks again for all the great help from the APM developers.


(ppoirier) #63

Cool, could you describe a little more what is your setup configuration ?
What is you camera system
What is the SLAM system running , on what type of Companion Computer
What are your launch files
How you communicate with the Flight Controler
What parameters have you set
And how did you run the test above.
I think that would really helps to set a base test models so others can refer and add to this
Regards,


(Jeff Smith) #64

@ppoirier Absolutely. This is work I’m doing for my employer, and I’ll be publishing full details (including code and launch files) on our github project page soon. But for now, some details of our system. We’re using:

  • Aion Robotics R1 platform
  • Stereolab ZED camera, mounted on a 15cm mast attached to the top of the rover
  • Nvidia Tegra TX2 for on-board computation
  • an internal (yet-to-be-released) visual odometry system that runs at about 30 fps
  • ROS

ArduRover parameters:

  • AHRS_EKF_TYPE 2
  • EKF2_ENABLE 1
  • EKF3_ENABLE 0
  • GPS_TYPE 0
  • AHRS_GPS_USE 0
  • VISO_TYPE 1

When you launch mavros, make sure to specify fcu_protocol:="v2.0" and ensure that the vision* plugins are whitelisted.

The results I plotted above (in Matlab) are the output of the /mavros/vision_pose/pose and /mavros/local_position/pose topics (which I recorded into a bag file).


(rmackay9) #65

Jeff,

Fantastic! thanks so much for your perseverance! :-).

P.S. we’ve actually got some more timing improvements coming and we will port this to the EKF3 as well.


(Subodh Mishra) #66

Thanks for posting the config!!


(Subodh Mishra) #67

@rmackay9 and @jeff_null thanks for all the info you guys shared here, it works for my skyviper drone aswell. I see more or less the same output at /mavros/local_position/pose as my input at /mavros/vision_pose/pose.

Now I plan to use this data for hovering/controlling the quadrotor. Any lead on how I can do that?


(ppoirier) #68

That is a good question;-)
Most publications are using RVIZ or MoveIt! but I have not found any references to actual GCS implementations.
Technically, tracking an Aruco tag is visual servoing and it should be controlled accordingly


(Subodh Mishra) #69

@ppoirier its more like vision based control where I want the FCU to use the state estimated by the vision system, i.e. use the data in /mavros/local_position/pose for controlling its pose.


(rmackay9) #70

@Subodh_Mishra, that’s really great progress! I don’t really know what you’ve got on the ground station side… perhaps something you’re developing yourself? In any case, whatever it is, you’ll want to send arm/disarm commands (within a COMMAND_LONG) to arm/disarm the vehicle. Also set the mode to Guided and then send a takeoff command (again within a COMMAND_LONG message).


(Andrea Belloni) #71

@jeff_null and @Subodh_Mishra I would like to know something more of your setup.
I am doing something similar to what @Subodh_Mishra did. I have a little quad with a down-facing camera looking at Aruco marker, I have the pose estimation using ros package aruco_mapping and I publish this to mavros/vision_pose/pose. I see that the pose from /mavros/local_position/pose follow that from vision_pose but there is a drift and if I try to switch to a mode that require a 3d fix (Loiter, Guided) I got EKF Compass error.

In the Flight Controler (RevoMIni) I have ArduCopter V3.6.0-rc4, with params:

AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
AHRS_GPS_USE 0
VISO_TYPE 1

I would like to know if is important to set this other params and with which value
EK2_GPS_TYPE ?
COMPASS_USE ?

in this post Indoor flight with external navigation data @chobitsfan use:
EK2_GPS_TYPE 3
COMPASS_USE 0

On mavros side I have local? and vision* plugin whitelisted, I would like to know if global* should be whitelisted or blacklisted.

I start mavros using a modified version of the launch file coming with the package:
apm.launch, apm_config.yaml, apm_pluginlists.yaml

I would like to see your version of this files to see if I forgot to change something.

Thanks in advance and sorry for so much questions, if I can make it work I will do a post with all the steps necessary to replicate my work.

Ciao
Andrea

P.S:
When I start mavros I see (besides other things):

[ INFO] [1531379625.929736640]: FCU: EKF2 IMU0 is using external nav data
[ INFO] [1531379625.931855170]: FCU: EKF2 IMU0 initial pos NED = 0.0,0.0,0.0 (m)
[ INFO] [1531379625.937697776]: FCU: EKF2 IMU0 ext nav yaw alignment complete

I think this is OK, but then I see (repeated):

[ WARN] [1531380481.439952513]: TM: Wrong FCU time.
[ WARN] [1531380489.054766092]: GP: No GPS fix

and as I said if I try to switch to Loiter or Guided I see EKF Compass error.


(Subodh Mishra) #72

Sorry, I can only get back to you on this after 2 months and I guess you must have figured it out by then.
But why are you switching to a mode that needs 3d fix?

I think
[ WARN] [1531380481.439952513]: TM: Wrong FCU time.
[ WARN] [1531380489.054766092]: GP: No GPS fix
is normal because I presume that you are indoor and there is no GPS there. And even if you are outdoor, it may display these warnings because you have set AHRS_GPS_USE to 0 and I don’t think you can switch to GPS dependent modes with this parameter set to zero.


(rmackay9) #73

I think VISO_TYPE can be left at “0” although it probably doesn’t hurt to be “1”. The VISO_ feature is for visual odometry which is different from the external position estimates…

I think that @chobitsfan used the mocap message which sends in a yaw attitude as well and so he was able to disable the compasses and perhaps that’s how he avoided seeing the EKF Compass error message.


(Chobitsfan) #74

@rmackay9 is correct. I disable all compass because of magnetic interference in my environment. (but leave MAG_ENABLE = 1, see this issue). Both position and yaw are feed from motion capture system


(Andrea Belloni) #75

Thanks @rmackay9 and @chobitsfan for your answer, I will try with VISO_TYPE=0 and COMPASS_USE=1 even if using mavros/vision_pose/pose topic I send both position and orientation and so even yaw attitude.

I would like to see mavros launch and config files from @jeff_null to understand if I miss something on that side.


(Andrea Belloni) #76

@rmackay9 I read that one of the key piece that gave us a good working vision_position_estimate handling was the PR “for handling the system clock differences between the companion-computer/GCS and the flight controller”.

So I was thinking that the warning “TM: Wrong FCU time” could be the cause of my problem, maybe I have to send a synchronization message before all the other stuff.
What do you think about this?


(rmackay9) #77

Andrea,

I’ve also seen that message on a colleague’s PC when using ROS and I think it should go away if the TIMESYNC and/or SYSTEM_TIME messages are being transferred between AP and ROS.

I don’t know for sure and I think it should work even with that message appearing but I hope we can make it go away eventually. Maybe @peterbarker has some more input.


(Andrea Belloni) #78

In the mavros sys_time plugin there is handle for mavlink TIMESYNC message:

On the relative wiki they say that this is only for PX4:
http://wiki.ros.org/mavros#mavros.2BAC8-Plugins.sys_time

But I see that also Ardupilot has handle for mavlink TIMESYNC message:

Anyway I am not able to go further on this front so for now I will continue with my Aruco vision_position_estimate tests ignoring the warning “TM: Wrong FCU time”.

Thanks
Andrea