VISION_POSITION_ESTIMATE not appearing in QGroundControl

rmackay9 · May 14, 2018, 11:02pm

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_null · May 15, 2018, 6:27pm

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 · May 15, 2018, 6:35pm

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_null · May 15, 2018, 6:52pm

@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 · May 15, 2018, 11:51pm

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 · June 12, 2018, 5:26am

Thanks for posting the config!!

Subodh_Mishra · June 12, 2018, 5:45am

@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 · June 12, 2018, 10:11am

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 · June 12, 2018, 4:49pm

@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 · June 13, 2018, 2:23am

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

anbello · July 12, 2018, 7:32am

@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 · July 12, 2018, 4:59pm

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 · July 13, 2018, 12:51am

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 · July 13, 2018, 2:47am

@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

anbello · July 13, 2018, 7:28am

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.

anbello · July 13, 2018, 2:56pm

@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 · July 14, 2018, 1:02am

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.

anbello · July 14, 2018, 7:50am

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

github.com

mavlink/mavros/blob/master/mavros/src/plugins/sys_time.cpp#L308


			time_unix->time_ref = time_ref;
			time_unix->source = time_ref_source;


			time_ref_pub.publish(time_unix);
		}
		else {
			ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time.");
		}
	}


	void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
	{
		uint64_t now_ns = ros::Time::now().toNSec();


		if (tsync.tc1 == 0) {
			send_timesync_msg(now_ns, tsync.ts1);
			return;
		}
		else if (tsync.tc1 > 0) {
			// Time offset between this system and the remote system is calculated assuming RTT for
			// the timesync packet is roughly equal both ways.

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:

github.com

ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/GCS_Common.cpp#L1897


{
   // we add in our own system id try to ensure we only consider
   // responses to our own timesync request messages
   return AP_HAL::micros64()*1000LL + mavlink_system.sysid;
}


/*
 return a timesync request
 Sends back ts1 as received, and tc1 is the local timestamp in usec
*/
void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
{
   // decode incoming timesync message
   mavlink_timesync_t tsync;
   mavlink_msg_timesync_decode(msg, &tsync);


   if (tsync.tc1 != 0) {
       // this is a response to a timesync request
       if (tsync.ts1 != _timesync_request.sent_ts1) {
           // we didn't actually send the request.... or it's a
           // response to an ancient request...

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

Subodh_Mishra · September 4, 2018, 3:36pm

Hey guys… I am using the following param set:

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

to do vision based state estimation. The pipeline is complete, ie. I am sending data in /mavros/vision_pose/pose and getting an output in /mavros/local_position/pose but these topics are not same at all. So, I think there is an issue with the frame of reference of both these topics. For my setup the /mavros/vision_pose/pose comes from the tracking a single aruco markers and the result in the topic is the position of the drones camera in the marker’s frame. How can I get similar results in both the aforementioned topics, in other words , how can I get input topic = output topic.

When I say the topics aren’t same, I mean that the data contained in them is not same.

anbello · September 5, 2018, 7:15am

From what I have understood VISO_TYPE 1 is only necessary if you use the VISION_POSITION_DELTA message with EKF3, to use the VISION_POSITION_ESTIMATE message (with EKF2) should be necessary EK2_GPS_TYPE 3 and COMPASS_USE 0 in addition to the parameters to choose EKF2 and GPS_TYPE 0