search

ethz-asl / ethzasl_msf

MSF - Modular framework for multi sensor fusion based on an Extended Kalman Filter (EKF)
Other
1.01k stars 437 forks source link

NO /msf_core/XXX topices out put #112

Closed clogord closed 9 years ago

clogord commented 9 years ago

I follwed the https://pixhawk.org/dev/ros/visual_estimation tutorial but can't get the /msf_core/pose topice message.In the rqt_graph,the rosnode look like that: rosgraph I remap the topices node name="msf_pose_sensor" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen" remap from="msf_core/imu_state_input" to="/mavros/imu/data" remap from="msf_updates/pose_with_covariance_input" to="/svo/pose" rosparam file="$(find msf_updates)/px4_svo_sensor_fix.yaml" The msf_pose_sensor haven't any msf_core topices.I don't know what's wrong.... Could you please help me? Thanks!

simonlynen commented 9 years ago

Did you initialize the filter in dynamic reconfigure?

On Sat, May 2, 2015, 18:52 clogord notifications@github.com wrote:

I follwed the https://pixhawk.org/dev/ros/visual_estimation tutorial but can't get the /msf_core/pose topice message.In the rqt_graph,the rosnode look like that: [image: rosgraph] https://cloud.githubusercontent.com/assets/10514708/7441876/f7773f7a-f12c-11e4-81e3-818ecd775b32.png I remap the topices

The msf_pose_sensor haven't any msf_core topices.I don't know what's wrong.... Could you please help me? Thanks!

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/ethzasl_msf/issues/112.

clogord commented 9 years ago

@simonlynen Yes,I use the rqt_reconfigure and hit init_filter or Call the /pose_sensor/initialize_msf_scale to 1 . It's still not work.....

clogord commented 9 years ago

I tested ethzasl_msf_sensor_fusion in the ros tutorial ,it has the same phenomenon，the msf_core topice didn't run..... rosgraph what's wrong??

simonlynen commented 9 years ago

The publishing is done from the msf_core module aren't these available if you run rostopic list? On May 3, 2015 9:08 AM, "clogord" notifications@github.com wrote:

I tested ethzasl_msf_sensor_fusion in the ros tutorial ,it has the same phenomenon，the msf_core topice didn't run..... [image: rosgraph] https://cloud.githubusercontent.com/assets/10514708/7444046/2698ead2-f1a6-11e4-8bd3-d81715265423.png what's wrong??

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/ethzasl_msf/issues/112#issuecomment-98446741 .

clogord commented 9 years ago

This module are in rostopic list ,but haven't data when I run the rostopic echo .

simonlynen commented 9 years ago

Can you take the tutorial rosbag, start the bag playback, then init the filter. Are you now getting data on the msf_core/state_out topic? On May 3, 2015 9:24 AM, "clogord" notifications@github.com wrote:

This module are in rostopic list ,but haven't data when I run the rostopic echo .

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/ethzasl_msf/issues/112#issuecomment-98448067 .

clogord commented 9 years ago

emm… I try ..but it was the same.No data output…

simonlynen commented 9 years ago

Can you post the msf terminal output here from the point that you start the launch file to after the moment that you init the filter? Thanks

On Sun, May 3, 2015, 09:29 clogord notifications@github.com wrote:

emm… I try ..but it was the same.No data output…

— Reply to this email directly or view it on GitHub https://github.com/ethz-asl/ethzasl_msf/issues/112#issuecomment-98448276 .

clogord commented 9 years ago

oh...I was wrong... when I start the bag palyback . I can see the data in rostopic echo /msf_core/pose but the data wasn't correct.... and when I inital the filter the log in the terminal seems like that [ INFO] [1430638741.372527579]: Initializing msf_core (built: May 2 2015) [ INFO] [1430638741.372737163]: Core parameters: fixed_bias: 0 fuzzythres: 0.1 noise_acc: 0.002 noise_accbias: 5e-08 noise_gyr: 0.0004 noise_gyrbias: 3e-06

[ INFO] [1430638741.373417776]: Core init with state: --------- State at time 8741.26s: --------- 0 : [0-2] : Matrix<3, 1> : [0 0 0] 1 : [3-5] : Matrix<3, 1> : [0 0 0] 2 : [6-9] : Quaternion (w,x,y,z) : [1, 0, 0, 0] 3 : [10-12] : Matrix<3, 1> : [0 0 0] 4 : [13-15] : Matrix<3, 1> : [0 0 0] 5 : [16-16] : Matrix<1, 1> : [1] 6 : [17-20] : Quaternion (w,x,y,z) : [1, 0, 0, 0] 7 : [21-23] : Matrix<3, 1> : [0 0 0] 8 : [24-27] : Quaternion (w,x,y,z) : [1, 0, 0, 0]

9 : [28-30] : Matrix<3, 1> : [0 0 0]

[ WARN] [1430638741.417103276]: large time-gap re-initializing to last state

[ WARN] [1430638742.308250112]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638744.354336466]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638746.404418059]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638748.405535093]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638750.453908577]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638752.454100341]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638754.503935889]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message. [ WARN] [1430638756.553247459]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message.

when I start the launch file the log look like:

SUMMARY

CLEAR PARAMETERS

PARAMETERS

NODES / msf_viconpos_sensor (msf_updates/pose_sensor)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found process[msf_viconpos_sensor-1]: started with pid [3660]

subscribed to topics:
advertised topics:
    /rosout
    /tf
    /msf_viconpos_sensor/core/parameter_descriptions
    /msf_viconpos_sensor/core/parameter_updates
    /msf_core/state_out
    /fcu/ekf_state_in
    /msf_core/pose
    /msf_core/odometry
    /msf_core/pose_after_update
    /msf_core/ext_state
    /msf_core/cov_core
    /msf_core/cov_aux
    /msf_core/cov_core_aux

[ INFO] [1430639193.440305742]: Loading parameters for pose sensor from namespace: /msf_viconpos_sensor/pose_sensor [ INFO] [1430639193.447837256]: Pose sensor is interpreting measurement as sensor w.r.t. world [ INFO] [1430639193.448023572]: Pose sensor is using covariance from sensor [ INFO] [1430639193.448216551]: Pose sensor is handling measurements as absolute values

Thank you!

simonlynen commented 9 years ago

Ok yes it looks like once you pass in the correct measurements the filter will process them too upon which you will get output.

This message says it all: :) "[WARN] [1430638750.453908577]: Provided message type without covariance but set fixed_covariance == false at the same time. Discarding message."

Is your problem resolved? Could you then close this issue? Thanks

clogord commented 9 years ago

Thank for your help!
emm......I try to modify the viconpos_sensor_fix.yaml and launch the launch file and palyblack the bag.Now,when I inital the filter.ONLY ONE WARN display: [ WARN] [1430642238.053535264]: large time-gap re-initializing to last state But in the rqt_graph,I haven't see any /msf_core module too...and /msf_core/state_out can't read the data. Can you tell me what's wrong ? Thanks!

simonlynen commented 9 years ago

So let me get an overview of the situation:

clogord commented 9 years ago

Thanks for you help! I haved already successfully running the MSF tutorial. So,following the Pixhawk Visual Position Estimation tutorial,I changes these thing in the msf_sensor_fusion:

  1. multi-sensor message input have changed remap from="msf_core/imu_state_input" to="/mavros/imu/data" /> remap from="msf_updates/pose_with_covariance_input" to="/svo/pose" /> #(message type changed to geometry_msgs/PoseWithCovarianceStamped )
  2. In the parameter (.fix) file some important changed are: pose_sensor/pose_fixed_scale set to false pose_measurement_world_sensor set to false pose_sensor/pose_fixed_p_wv set to false pose_sensor/pose_fixed_q_wv set to false

Then, I can't get the /msf_core/pose topice data correctly........ the msf output data didn't followed the input,it was increased with the time.... What's wrong ? Thanks !

simonlynen commented 9 years ago

So,following the Pixhawk Visual Position Estimation tutorial,I changes these thing in the msf_sensor_fusion:

ok. It would have been good if you would have pasted this information earlier. It is impossible for me to guess what your problem is if you don't tell me that you changed central parts of the system.

  1. multi-sensor message input have changed remap from="msf_core/imu_state_input" to="/mavros/imu/data" />

Do you have data on the topic? What does rostopic echo /mavros/imu/data say? Also what is the message type, please verify that the subscriber type in the msf matches with what you are publishing.

remap from="msf_updates/pose_with_covariance_input" to="/svo/pose" /> #(message type changed to geometry_msgs/PoseWithCovarianceStamped )

Are you sure your covariance is set in svo? Did you check if the MSF outputs error messages? Did you check if the msf prints "got first pose message"?

  1. In the parameter (.fix) file some important changed are: pose_sensor/pose_fixed_scale set to false pose_measurement_world_sensor set to false pose_sensor/pose_fixed_p_wv set to false pose_sensor/pose_fixed_q_wv set to false

These parameters don't have an influence on the above issue.

Simon

clogord commented 9 years ago

Thanks Simon!

About "/mavros/imu/data": The message type of the "/mavros/imu/data" is "sensor_msgs/Imu" ,same as the msf subscriber. And the data of "/mavros/imu/data" have publishing correctly .Here is the data:

header: seq: 13412 stamp: secs: 721 nsecs: 7297008 frame_id: fcu orientation: x: 0.0147992696495 y: 0.0762160299954 z: 0.971319592125 w: 0.224745074125 orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] angular_velocity: x: -0.00163393351249 y: 0.00627219444141 z: 0.023651978001 angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07] linear_acceleration: x: 0.074472039938 y: 1.57893514633 z: 9.79785346985 linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

About "/svo/pose" : The data include the correct covariance. Here is the data publish by the "/svo/pose" topice.

header: seq: 143 stamp: secs: 1430757658 nsecs: 955060005 frame_id: /cam pose: pose: position: x: 0.0261334090984 y: 0.0657043718026 z: -0.0254704167218 orientation: x: -0.0388214673448 y: 0.0180642740452 z: 0.0147265451155 w: 0.998974326269 covariance: [2.137702449043848e-05, -4.799296488304068e-06, -3.917796946997961e-06, 3.5474243463630228e-06, 1.933800742753466e-05, -3.7769025527193674e-06, -5.417568387493764e-06, 5.486719874854015e-05, -9.282973604624339e-06, -5.256909986846095e-05, -6.284813279873599e-06, -6.846517369346083e-06, -4.092617991484252e-06, -1.2976726425887829e-05, 4.669033803479326e-06, 1.2994856804747634e-05, -3.3784634016182236e-06, 2.7930652444179863e-06, 3.7344726256602017e-06, -5.353283684087096e-05, 9.782954325066295e-06, 5.150478262636739e-05, 4.735066295894495e-06, 7.065035497545395e-06, 1.916173480612831e-05, -5.876694927811638e-06, -3.2699977203506566e-06, 4.687168639168799e-06, 1.746225607240713e-05, -2.9265583372410326e-06, -5.212474445284425e-06, -5.350894518602579e-06, 2.2671948168199346e-06, 5.449011469757523e-06, -4.384600528995505e-06, 2.895583184455871e-06]

And when the data haved published to the MSF,only two message have show:

[ WARN] [1430757983.488080426]: large time-gap re-initializing to last state [ INFO] [1430757986.922536114]: * pose sensor got first measurement from topic //svo/pose *

It's seems the msf is running normally..... when I stop publish the "/svo/pose"message, the MSF still have opuput in"/msf_core/pose" ,but the data still un-right.It's increased with the time.... when I stop the "/mavros/imu/data" ,then the "/msf_core/pose" stop the output. ONLY the "svo/pose" data input can make MSF output ? This phenomenon was really strange. Thanks!

simonlynen commented 9 years ago

And when the data haved published to the MSF,only two message have show: [ WARN] [1430757983.488080426]: large time-gap re-initializing to last state [ INFO] [1430757986.922536114]: * pose sensor got first measurement from topic //svo/pose *

ok this looks good.

It's seems the msf is running normally..... when I stop publish the "/svo/pose"message, the MSF still have opuput in"/msf_core/pose" ,but the data still un-right.It's increased with the time....

This is correct and expected behaviour of the estimator. If you stop providing measurements the filter will integrate the IMU measurements. Bias as well as noise on the IMU signals cause the integrated position to accumulate errors.

when I stop the "/mavros/imu/data" ,then the "/msf_core/pose" stop the output. ONLY the "svo/pose" data input can make MSF output ?

Yes if you don't provide IMU measurements no state propagation happens and therefore no pose estimates are published. Again this is expected and correct behaviour.

This phenomenon was really strange.

I would suggest you take the time to read some literature to get a feeling about how an indirect EKF works. Good starting points are these:

clogord commented 9 years ago

Yes,I really need this paper to learn how ekf work.Thanks! But, the important things, I still couldn't get the correct data on the MSF output when I publish the /svo/pose and /mavros/imu/data ,The MSF output /msf_core/pose same as No /svo/pos measurements. I want to konw why ? Thank you very much!

wang-chen commented 9 years ago

Hi Clogord, Have you solved the question: But, the important things, I still couldn't get the correct data on the MSF output when I publish the /svo/pose and /mavros/imu/data ,The MSF output /msf_core/pose same as No /svo/pos measurements. I want to konw why ?

I meet the same problem with you. The output of /msf_core/pose diverge, but both /svo/pose and /mavros/imu/data are good. Could you tell me how you solved this problem? Thanks very much!