Position Hold with VISION_POSITION_ESTIMATE and ATT_POS_MOCAP for indoor localization

anbello · September 24, 2018, 4:28pm

If you are talking about this as you can see has been merged

If you see delay it coud be due to what I wrote above:

Subodh_Mishra · September 24, 2018, 4:31pm

What exactly do you mean by this? Do you say that the time stamp of the image and the pose associated with it must be same?

I also wanted to know is you are using the arducopter 3.6 rc7 from github’s master or from the website where you can directly download the binaries?

anbello · September 24, 2018, 4:46pm

I have problems with english language, anyway I will try.
Any message has a timestamp, when you form the message for /mavros/vision_pose/pose topic you should use as timestamp the time at witch the frames are captured by the cam on the quadcopter.
I don’t know how you can do this. In my system I use gstreamer both to capture frames (on quadcopter), send it over UDP and receive it on desktop PC. In this way I can know the time at witch the frames are captured and use it as timestamp so I keep in the count the WiFi latency.

In the last test I used 3.6.0-rc10 from the website

Subodh_Mishra · September 24, 2018, 4:51pm

Thanks. It is clear now.

Subodh_Mishra · September 25, 2018, 12:34am

Hey @rmackay9 @vkurtz , Is this vision_lag parameter being used somewhere in the code which is in master now? There is a lot of lag b/w my vision position estimates and EKF’s output. Where can I fix this?

This video shows the lag between /mavros/mocap/pose(base_link) and /mavros/local_position/pose(local_pose): https://youtu.be/cOFGx1HAGJs

anbello · October 8, 2018, 1:41pm

Another test with my system for indoor localization (more info some post above):

Here I don’t used the RC, all the flight is controlled by a python script, a modified version of this.
Takeoff in guided mode, 4 setpoint in a square, land.
As can be seen the setpoint are reached with instabilities in x and y axis, the z axis is always stable.
I don’t know how much of this instabilities are due to some Arducopter parameters that I have to tune better and how much are due to suboptimal position estimation from the aruco marker.
I will investigate further.

chobitsfan · October 13, 2018, 12:28pm

Hi @rmackay9
I create a lag parameter for vision system, too. I place it in AP_NavEKF2 because I think it may be similar to EK2_FLOW_DELAY and EK2_HGT_DELAY. My changes are here. I have tested it with skyviper v2450 and motion capture system (delay = 10ms). Maybe one of us could create a PR? Because it would be helpful to have a vision delay parameter. Thank you very much

rmackay9 · October 14, 2018, 3:19pm

@chobitsfan, definitely sounds like something we need. I suspect we may need to put the delay parameter at a higher level but I’m just guessing. I’m at the HEX conference but I think we should try and get this into master next week somehow.

anbello · October 18, 2018, 10:11am

@chobitsfan and @rmackay9 why do you think that is necessary to have a lag parameter for vision system? I understood that this is automatically calculated by this method:

github.com

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




  1) the data did not come from the future in our time-domain
  2) the data is not older than max_lag_ms in our time-domain


 It works by estimating the transport lag by looking for the incoming
 packet that had the least lag, and converging on the offset that is
 associated with that lag


 Return a value in milliseconds since boot (for use by the EKF)
*/
uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size)
{
   const uint32_t max_lag_us = 500*1000UL;
   uint64_t local_us;
   // if the HAL supports it then constrain the latest possible time
   // the packet could have been sent by the uart receive time and
   // the baudrate and packet size.
   uint64_t uart_receive_time = _port->receive_time_constraint_us(payload_size);
   if (uart_receive_time != 0) {
       local_us = uart_receive_time;
   } else {

The linked method calculate the transport lag to correct the timestamp, maybe this new lag parameter has another purpose?

fnoop · October 18, 2018, 10:26am

@anbello This only compensates for link lag. The sensor lag can be many times this (the time it takes to capture the image, process it, analyse it, calculate the required data, and then send it to the link).

anbello · October 18, 2018, 10:35am

Thank you @fnoop now I understand better. But if I use the timestamp of captured image in the header of the vision position estimate message I should have all the lag calculated by the aforementioned method. Am I wrong?

fnoop · October 18, 2018, 10:55am

No, it only deals with the transport/link lag which will typically be a physical link latency of a few ms. A sensor/vision sensor lag could be anything from 10ms to 50ms to 100ms to much higher. If you’re using a fast onboard computer with a fast camera frame rate you may get a sensor lag as low as 10-20ms. If you were using eg. a raspberry pi+raspicam it may well be as high as 200ms, and it could vary from frame to frame from anything between 50ms and 200ms depending on a variety of factors at any particular millisecond of time.

Also the link lag converges on a sample of latencies over a period of time, I would have thought if you have a vision based estimation system that you would want to take into account individual frame latencies rather than trying to smooth out jitter. The higher the lag the higher the jitter is likely to be.

Your system looks really cool, I’m looking forward to more details as it’s something I’ve had on my todo list to try for a long time

anbello · October 18, 2018, 12:14pm

Thanks, on post #32 above there is a link to some details of my system, I am waiting to have enough time to put together all the necessary info and source (only little diff vs aruco_mapping package) to put it on github and here as a post. I have very little time to do this kind of work but I will do for sure.
And in any case I have yet some problems to solve on my system as you can see from the videos.

Ciao

Mohamed_Ehab · November 29, 2018, 2:10am

You are using fake GPS msg, right?
because I am doing the same thing, but the position hold doesnot work fine with me.
Can you please share with the steps that you have made?
I created my ROS node to get the position and publish it as the HIL_GPS msg.
what parameters should I change, should something with mavros??
Your help really matters, thank you.

ppoirier · December 9, 2018, 2:55pm

Hello Andrea,
I finally have started to experiment with aruco/ vision_position_estimate
I am using an Intel based Companion Computer with a calibrated USB Camera
Launch:
roslaunch usb_cam.launch
roslaunch aruco_gridboard detection.launch
roslaunch mavros apm.launch fcu_url:=/dev/ttyUSB0:115200
rosrun mavros mavsys rate --all 100

On my glight controler BBBMINI
sudo ./arducopter -C udp:192.168.7.1:14550 -D /dev/ttyO4

So the pipeline is camera-CC-Serial to BBBMINI - udp to - Mission Planner
I connect to mission planner, set these:
AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
EK2_GPS_TYPE 3
COMPASS_USE 0
VISO_TYPE 0 == or 1?!? , I tried both

I set the Home/EKF home and get the vehicle displayed
I can get good movement correlation on display when moving my quad-camera

But I cannot read the mavros local pose in RVIZ
I have WORLD as reference so I guess I am missing a couple of TRANSFORM here…
OK found it here
/launch/ detection_rpi.launch

Have you made the complete TF for your frame ?

anbello · December 9, 2018, 3:32pm

No, to be honest I did not understand well all the TF stuff in ROS, I have to study more.
Anyway I added offset parameter for the camera but I did not push it yet, I will do asap.

[EDIT]
VISO_TYPE 0

ppoirier · December 9, 2018, 3:43pm

ok good, I might make one for my frame and will update here.

BTW Andrea I added the missing debug logic for the display
parameter in detection.launch

modified node.cpp line 408
if(debug_display_) {
cv::imshow(OPENCV_WINDOW, imageCopy);
cv::waitKey(2);
}

Making it easier to switch display on/off

ppoirier · December 9, 2018, 11:45pm

Well, that was a nice sunday experiment
Here is the Launch config
…lol it does not read xml !!!

Local origin to world
node pkg=“tf” type=“static_transform_publisher” name=“tf_local_origin”
args="0 0 0 0 0 0 world local_origin 100

Rotate the IMU in reference to camera
arg name=“pi/2” value="1.57079632
arg name=“imu_rotate” value=“0.05 0 -0.08 $(arg pi/2) 3.1415926
node pkg=“tf” type=“static_transform_publisher” name=“camera_fcu”
args=”$(arg imu_rotate) camera fcu 100

according to this:

ppoirier · December 9, 2018, 11:50pm

please note that I have a forward camera setup (not down-looking) this is why there is a rotation about the red axis == still need some tweaking but I have the general setup working

anbello · December 10, 2018, 9:55am

This is not true, I already pushed the offset parameter commit, but only for the offset along the back-front axes (and I have forgotten to have done it )