No output in msf_core/state_out and fixed_covariance error about ethzasl_msf HOT 18 OPEN

Fixed the message about the covariance. For anybody having the same problem...
In file viconpos_sensor_fix.yaml you had among other things:
--> pose_sensor/pose_sensor/pose_use_fixed_covariance: true
But after some research, at least when you launch:
roslaunch msf_updates viconpos_sensor.launch, hpp header pose_sensorhandler.hpp is handling node pnh, which has namespace msf_viconpos_sensor, so it's actually looking in another place for that parameter, and setting use_fixed_covariance_ to the default which is false.

Solution: Change the .yaml file first part to:
/msf_viconpos_sensor/pose_sensor/pose_use_fixed_covariance: true

All the same, now I'm getting in my launch the Warning:
[ WARN] [1423883421.388011281]: Fixed Covariance OK!

But still no output in any channel.
As you can see I remapped:

But I'm still getting no output nowhere. Either by subscribing to /msf_core/state_out or by running the fixed plot_relevant which now has to read:
rqt_plot msf_core/state_out/data[0]:data[1]:data[2] msf_core/state_out/data[3]:data[4]:data[5]
rqt_plot msf_core/state_out/data[13]:data[14]:data[15] msf_core/state_out/data[16]

As rxplot does not work anymore.

I hope this helps some other lost souls like me.
I would appreciate some input by @simonlynen @cfo @mhkabir, or anybody that feels sorry for me. haha.

Thanks!
Francisco.

from ethzasl_msf.

You need to init the filter.
Run rqt_reconfigure and hit init_filter

from ethzasl_msf.

you can also initialize with a service call:

~/pose_sensor/initialize_msf_scale to set the scale

or

~/pose_sensor/initialize_msf_height to compute the initial scale based on the current z-measurement and the height that you set in the service call

https://github.com/ethz-asl/ethzasl_msf/tree/master/sensor_fusion_comm/srv

from ethzasl_msf.

Thanks a lot, I have it streaming data out, and for most of the part it looks ok.
I have already started building a guide as how to use the new version of the code.

I'm having a small concern right now... At the end of my data stream the y position goes REALLY up, and when I plot my velocities I see kind of the same behavior. This sounds really weird as the 3D path plot of the data that you put in there is only less than 2 m.

For the rest, it seems I'm correctly being able to input IMU and vicon data into the system and getting some result out at least.

Do you want me to upload a new guide through here or what should I do??

I'll update on my final result when I'm sure I got everything ok.
Thanks a lot,

Francisco.

from ethzasl_msf.

Ok that sounds good. Can you try your setup with the provided bagfile from
the old framework using the vicon launchfile + parameters we provide?

Yes if you could just start writing wiki pages this would be very much
appreciated.

On Thu, Feb 19, 2015, 20:42 Francisco de Villalobos <
[email protected]> wrote:

Thanks a lot, I have it streaming data out, and for most of the part it
looks ok.
I have already started building a guide as how to use the new version of
the code.

I'm having a small concern right now... At the end of my data stream the y
position goes REALLY up, and when I plot my velocities I see kind of the
same behavior. This sounds really weird as the 3D path plot of the data
that you put in there is only less than 2 m.

For the rest, it seems I'm correctly being able to input IMU and vicon
data into the system and getting some result out at least.

Do you want me to upload a new guide through here or what should I do??

I'll update on my final result when I'm sure I got everything ok.
Thanks a lot,

Francisco.

—
Reply to this email directly or view it on GitHub
#101 (comment)
.

from ethzasl_msf.

@simonlynen , if I get you correctly that is what I have been doing. I downloaded the rosbag that was published for the ssf tutorial, and that is why I needed to write a topic converter, as the topic required now for /vicon/auk/auk was not of the type TransformStamped but PoseStamped.

I still could not plot in 3D the x, y, z position of the quad, but I'm getting two small issues I'm running into. At one point the viconpos_sensor looses a message and it starts dumping messages because it says "Expected message with sequence: 4567 but got seq 4568, dumping message". So, from that point on, obviously, everything goes to hell. haha.

The other issue that may or may not be part of the framework is the graph output for a signal. There is sometimes a los more noise than I would expect. I attach here an Image of my Y position output (data[2], against the robs vicon pose position y.

Here you can see how the error goes to like over half a meter, and then it JUMPS back to the "correct" Vicon value. This seems wrong to me for the filter output, so I don't know if I'm doing something wrong or if the filter is just behaving that way.

I will write the Wiki Page as soon as I have everything figured out. I already wrote kind of a tutorial for myself. I'm also going to record a new rosbag that outputs the correct topic messages for better integration.

Let me know if you have any insight on my problem,

Thanks,
Francisco.

from ethzasl_msf.

@fdevillalobos mhh, several things seem to need changes in your setup:

1. first don't reprocess the data using a TF->Pose node, you should be able to directly subscribe to TFStamped: See this this topic.
2. I can't see any code on master that would drop pose messages. Which branch are you using?
3. If you discard all messages then it is expected that your pose error increases as the estimator performs solely IMU integration with only a few updates (large corrections).

Let me know what you get, once you fixed the above.

from ethzasl_msf.

Thanks @simonlynen !! Very helpful.
So, I'm going to quickly mention what I've achieved and the small questions that I have about the results (Though they look pretty good at first sight).

1.- Changed my remap from publishing in pose_input to doing it in transform_input and using the robs that you guys provide and not the one I converted to. So:

2.- It's now not dropping messages so I don't know where that issue came from, but it's not a problem any more.

3.- Tutorial should be ready in the next few days as I think I have it pretty much running. Will create a wikipage and publish all that I've done in there. This is my result now with the original rosbag, and my modifications to some parts:

4.- With the things you told me about. So... when should I use pose_input and when transform_input? What is the framework internally doing to each of them? This could be the extras of the tutorial, or the initial parts for a second one.
5.- Also in the same context: When should I use hl_state_input and imu_state_input_asctec? What type of sensor and data are they expecting?

6.- Finally, what I really wanted to try in this framework and the reason in my experiment, is what happens when you have data publishing at different rates. So I retained the imbue data at a 100Hz as it was, but published only one out of 10 messages from the vicon system, getting a vicon rate of 10Hz (As an example). This are the results of the filter when subscribed to the 10Hz topic instead of the 100Hz vicon.

As you can see the 10Hz and the 100Hz signal from Vicon go pretty well together, but the state_out data is just terrible. I thought it should still calculate at 100Hz with the IMU data, and correct every Hz to the vicon truth, but that is not what I'm observing here.
This should be the answer as how the system handles different rate sensors into the same filter and gets correct data. Could I get some insight into why is this happening?

I'll let you know as soon as I have the wikipage ready! Thanks!
Francisco.

from ethzasl_msf.

@fdevillalobos

4.- With the things you told me about. So... when should I use pose_input and when transform_input? What is the framework internally doing to each of them? This could be the extras of the tutorial, or the initial parts for a second one.

You should use the input type that is the same as what your sensor provides. If you have a sensor that provides TFs then use the tf-input of the msf. Internally tf or pose are handled the exact same way.

5.- Also in the same context: When should I use hl_state_input and imu_state_input_asctec? What type of sensor and data are they expecting?

hl_state_input is a message that is generated by Asctec FCUs and transports the integrated state as computed by the on-board estimator that runs on the Highlevel processor of the Asctec FCU.
imu_state_input_asctec is an imu message in a special Asctec format. You can ignore both inputs and instead use the "normal" IMU topic to feed IMU messages to the MSF.

6.- Finally, what I really wanted to try in this framework and the reason in my experiment, is what happens when you have data publishing at different rates. So I retained the imbue data at a 100Hz as it was, but published only one out of 10 messages from the vicon system, getting a vicon rate of 10Hz (As an example). This are the results of the filter when subscribed to the 10Hz topic instead of the 100Hz vicon.

It should not be a problem even running the MSF with only 1 Hz Vicon update as long as you are providing the full IMU rate.

As you can see the 10Hz and the 100Hz signal from Vicon go pretty well together, but the state_out data is just terrible. I thought it should still calculate at 100Hz with the IMU data, and correct every Hz to the vicon truth, but that is not what I'm observing here.
This should be the answer as how the system handles different rate sensors into the same filter and gets correct data. Could I get some insight into why is this happening?

You need to subscribe to a topic that is actually published after the IMU propagation such as "pose". The "state_out" message is an internal message meant for communicating state corrections only after updates to the Asctec FCU. So it is expected that you get low frequent updates there. If you subscribe to the "pose" topic, you will get what you expect.

Thanks a lot for putting up the tutorial! This is really great!

from ethzasl_msf.

Ok. Nice! I subscribed to pose and got it nice and running. When I later tried to run the vicon data at 10Hz... the system was only correcting every 2Hz. Once every 5 Vicon readings. Like this:

So I looked into the code and in /msf_updates/include/msf_updates/pose_sensor_handler/implementation/pose_sensorehandler.hpp in line 193, I found what I thought was something slowing down the vicon... but weirdly enough I was not getting the message inside of it...

if (msg->header.seq % 5 != 0) { // Slow down vicon.
MSF_WARN_STREAM_THROTTLE(30, "Measurement throttling is on, dropping every "
"but the 5th message");
return;
}

I just commented those lines out, re-built, and run the filter again. Voila!

At least it looks like it's updating @10hz so that's fine.
I guess my question is why are those lines in there, and why can't I not have a Vicon "as quick" as the IMU readings? I know it's stupid because WHY would you want to do IMU data processing if you are getting Vicon readings at the same rate... yeah, I know... but just wondering...
Is it safe to comment this lines out? I just have to be careful that my Vicon is slower than IMU?
And also... why did it slow down my reading even at 10Hz. It seems like this lines ALWAYS kick in, and ALWAYS slow down your Vicon no matter at what speed you are sending the data.

If you tell me now that this graph looks exactly as it should for a 10Hz Vicon and a 100Hz IMU from the rosbag... then I believe I will be ready to upload the tutorial pretty soon.

Thanks again,
Francisco.

from ethzasl_msf.

@fdevillalobos yes, this is just some legacy code that we had in there for our Vicon which runs at 200Hz here. Having a higher update than propagation rate is not a problem technically, but obviously makes not much sense since you will be interpolating your IMU measurements in the end.

We should make this an option that can be switched off if people want to run their Vicon updates at a low frequency like 10Hz. Did you just do this for testing or what was your intention to feed only 10Hz Vicon into the estimator?

Are you listening to the ros-warning messages at all? The warning about the throttling should be published there.

from ethzasl_msf.

@simonlynen Ok. Nice... I commented it out then.
My intention was to test the framework specifically for sensors with different time-stamps, and update rates so that I could have GPS, then no-GPS and just SLAM and IMU.
But overall yes, I tried that on purpose but I don't yet have a real system going into it.

I already published a pre-version of the tutorial, with all the stuff you wrote also adapted for the multi-sensor part... but I'll keep on working in it and adding more of the issues I run into while coding. If you can have a look at it and edit or tell me whatever you think is wrong... that would be useful. The first part until (6) is mainly copied from the first tutorial changing a word or phrase here and there to make if part of the MSF, but from 6 on I state all the issues and steps you should go to make it work.

Thanks!
Francisco.

from ethzasl_msf.

@fdevillalobos sure, I am happy to take a look. Where can I find this version?

from ethzasl_msf.

@simonlynen
http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/Introductory%20Tutorial%20for%20Multi-Sensor%20Fusion%20Framework
It seems it's already up with the rest of the tutorials...

from ethzasl_msf.

Also @simonlynen , just found out about some other strange thing. As you saw in the previous images... the position estimate from msg_core/pose (x,y,z), was coming out pretty neat... but I just started looking at the pose estimate of the same data and this is what's coming out:

It's like it jumps every other time going completely haywire and then comes back to where it's supposed to track.
Could you give me some insight as to why this could be happening?
Thanks,
Francisco.

from ethzasl_msf.

This looks like sth is very wrong with the plotting or on the data
transport. Can you check what the values look like when you echo the
message directly to the terminal? And also perhaps plot the state values
here:
https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_core/include/msf_core/msf_sensormanagerROS.h#L207

On Fri, Feb 27, 2015 at 1:31 AM Francisco de Villalobos <
[email protected]> wrote:

Also @simonlynen https://github.com/simonlynen , just found out about
some other strange thing. As you saw in the previous images... the position
estimate from msg_core/pose (x,y,z), was coming out pretty neat... but I
just started looking at the pose estimate of the same data and this is
what's coming out:

[image: pose]
https://cloud.githubusercontent.com/assets/6305634/6405149/cb4c3e36-bdd4-11e4-8aac-3553113440ad.png

It's like it jumps every other time going completely haywire and then
comes back to where it's supposed to track.
Could you give me some insight as to why this could be happening?
Thanks,
Francisco.

—
Reply to this email directly or view it on GitHub
#101 (comment)
.

from ethzasl_msf.

Just look at the data... Here is the weird part... the one that's jumping all over the graph is the VICON dataset.bag data. I'm just providing a 10Hz sample (Do discarding 9 and taking 1 into the system), but the lines that are jumping are from /vicon/auk/auk/10Hz topic. Here are the two continuos messages that we can see here:
header:

seq: 272
stamp:
secs: 1349442778
nsecs: 319749282
frame_id: /world
child_frame_id: ''
transform:
translation:
x: 0.297124668196
y: -0.0154649289755
z: 1.46997330544
rotation:
x: -0.194870754254
y: -0.0664521404711
z: 0.864270314362
w: -0.458962227068

header:
seq: 273
stamp:
secs: 1349442778
nsecs: 419675057
frame_id: /world
child_frame_id: ''
transform:
translation:
x: 0.231332529938
y: -0.0289755954332
z: 1.554242101
rotation:
x: 0.243429321789
y: 0.128878206821
z: -0.806572416822
w: 0.523042550393

So... I then got the data back to original instead of 10Hz... and...

It's like the VICON provided data is the MIRROR of the filter output pose quaternion around 0. All of them. The blue and the green, the brown and the red, the pink and the deep green, and the black and the light blue. They go mirroring for some time... and then the suddenly jump back in track together. But the ones doing the jump, are the quaternion coords.

But in better thinking... this is the z component of the QUATERNION for the rotation, and is not real roll pitch yaw for the quad. So... this may be normal as it is a normalized vector. My question comes in why a mirror of the data??
Do you have a service function or something in the system that converts or gives out roll, pitch, yaw instead of the quaternion notation? I can code it if not... but I mean, it's a lot more useful to have roll pitch yaw than the quaternion in terms to graph. I also put this data into rviz, and it runs smoothly as the pose filter output (I'm using the filter output in rviz).

So... after some time thinking that I was doing everything ok... now I don't know what's going on.
Thanks,
Francisco.

from ethzasl_msf.

@fdevillalobos I am not sure I understand what you are doing here. From the plot above it looks like the estimator is outputting the estimate as expected but your reference signal is wrong. Can you describe what exactly you do to the measurements we provided in the bagfile? You seem to be doing sth wrong there.

Yes you can of course convert a Quaternion to RPY for plotting, the question is rather why you want to convert to a representation which has singularities?

Btw, if you are interested in simulating different delays, dropouts, frequency limiters I suggest you use the msf_distort package which has a dynamic-reconfigure GUI to set parameters of such influences on the measurements. This will probably also fix whatever the issue is with your current downsampling method.

from ethzasl_msf.