VISION_POSITION_ESTIMATE not appearing in QGroundControl

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

Subodh_Mishra · September 5, 2018, 9:47pm

Hey @anbello thanks for your response. But can you be a little more clear and specific? Can you please tell me the parameters you have set?

anbello · September 6, 2018, 12:55pm

Hi @Subodh_Mishra
I have the following parameters on my quad:
AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
EK2_GPS_TYPE 3
COMPASS_USE 0
VISO_TYPE 0

my quad stream the video from a camera via WiFi like your, only it is DIY:

little quad (130mm) with RevoMini FC with Arducopter 3.6 rc7
on the quad there is a Raspberry Pi 0 with Raspberry Cam streaming video to Desktop PC
on Desktop PC there is ROS Kinetic with mavros and aruco_mapping
the video is streamed with gstreamer and on the PC the node gscam get this stream and publish camera/image_raw and camera/camera_info topics
on the PC aruco_mapping (slightly modified by me) subscribe to above topics and publish a camera_pose message to the mavros/vision_pose/pose topic.

[Edit]
I forget to say that i send a SET_GPS_GLOBAL_ORIGIN and a SET_HOME_POSITION messages (similar to this) before sending vision position estimate.

When all this is running and I have the quad on my hand (disarmed) with camera pointing to a marker I can see /mavros/vision_pose/pose and /mavros/local_position/pose that moves in a coherent manner.

When I try to arm and fly over the marker if I switch to Loiter mode the quad drift and is not stable.

One problem I see is a not so good video quality and when I fly over the marker the marker itself is detected intermittently and not in a smooth an continuous manner as needed. I have to work to improve the quality of the streamed video and perhaps to redo a camera calibration procedure.

Another thing that I did not understand well is if I’m using the correct transformation when I send the message /mavros/vision_pose/pose, I am talking about the ENU NED stuff as explained here under " Asserting on Reference Frames".

Subodh_Mishra · September 6, 2018, 2:35pm

Thanks @anbello, I am going to try this and get back to you in a day or two.

Subodh_Mishra · September 6, 2018, 2:45pm

Hey @anbello, Do you arm your quad using the controller/transmitter or in using ros commands for mavros?

Subodh_Mishra · September 6, 2018, 3:04pm

Don’t you think this should be set to 3, which means no GPS?

anbello · September 6, 2018, 3:21pm

Yes it’s a typo I mean 3
now I edit the previous post with the right value, thanks

anbello · September 6, 2018, 3:23pm

I arm with the controller

Subodh_Mishra · September 6, 2018, 7:08pm

What mode are you in when you arm your quadrotor?