search

ethz-asl / ethzasl_msf

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

No outputs on the msf_core/pose topic or msf_core/state_out #118

Open zaidtas opened 9 years ago

zaidtas commented 9 years ago

I know there were two issues related to this issue and they have been closed. I was trying to follow them but I couldn't get what they did to correct the issue. specially the mhkabir one!

I follwed the https://pixhawk.org/dev/ros/visual_estimation tutorial. Even though i get msf_core/pose when i do "rostopic list" but when I do "rostopic echo msf_core/pose I get nothing. Nothing is getting published in that topic. I have modified the files as mentioned in the pixhawk tutorial. and runnung pose_sensor.launch.

With the dataset which is available, it was workinng fine. but it doesn't work with live data.

Here are the messages from the pose_sensor.launch file window: [ INFO] [1435151175.900373143]: Loading parameters for pose sensor from namespace: /msf_pose_sensor/pose_sensor [ INFO] [1435151175.912892397]: Pose sensor is interpreting measurement as world w.r.t. sensor (e.g. ethzasl_ptam) [ INFO] [1435151175.912963788]: Pose sensor is using fixed covariance [ INFO] [1435151175.913016908]: Pose sensor is handling measurements as absolute values [ INFO] [1435151178.835199892]: * pose sensor got first measurement from topic //svo/pose * [ INFO] [1435151215.059098908]: initial measurement pos:[ 0.0141186 -0.0972923 0.0369226] orientation: [0.995, -0.0824, 0.011, 0.0462] [ WARN] [1435151215.179596490]: Using simulated core plus fixed diag initial error state covariance. [ INFO] [1435151215.189833058]: Initializing msf_core (built: Jun 22 2015) [ INFO] [1435151215.189985234]: Core parameters: fixed_bias: 0 fuzzythres: 0.1 noise_acc: 0.013 noise_accbias: 0.000108 noise_gyr: 0.00065 noise_gyrbias: 2.12e-06

[ INFO] [1435151215.190219889]: Core init with state: --------- State at time 1215.08s: --------- 0 : [0-2] : Matrix<3, 1> : [ 0.0141186 -0.0972923 0.0369226] 1 : [3-5] : Matrix<3, 1> : [0 0 0] 2 : [6-9] : Quaternion (w,x,y,z) : [0.995, -0.0824, 0.011, 0.0462] 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] [1435151215.227439870]: large time-gap re-initializing to last state

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

.....goes on till I stop.

Need help!

simonlynen commented 9 years ago

This looks like your IMU is either not connected or the timestamps of your IMU measurements are not set/random or corrupted. Please take a look at the imu-measurements with rqt_plot and rostopic echo.

zaidtas commented 9 years ago

Sorry, I was working on some other aspects of my project so I couldn't reply earlier.

Well, I am getting data from Imu. These is what I get when I do "rostopic list" while mavros is working. /diagnostics /mavlink/from /mavlink/to /mavros/battery /mavros/global_position/compass_hdg /mavros/global_position/global /mavros/global_position/gp_vel /mavros/global_position/local /mavros/global_position/rel_alt /mavros/gps/fix /mavros/gps/gps_vel /mavros/imu/atm_pressure /mavros/imu/data /mavros/imu/data_raw /mavros/imu/mag /mavros/imu/temperature /mavros/mission/waypoints /mavros/radio_status /mavros/rc/in /mavros/rc/out /mavros/rc/override /mavros/setpoint_accel/accel /mavros/setpoint_position/local /mavros/setpoint_velocity/cmd_vel /mavros/state /mavros/time_reference /mavros/vfr_hud /mavros/wind_estimation /rosout /rosout_agg /tf

on doing "rostopic echo mavros/imu/data" i get the following sample data:

header: seq: 1659 stamp: secs: 432 nsecs: 718000003 frame_id: fcu orientation: x: -0.274636914139 y: 0.0851455340552 z: -0.591603200804 w: 0.753213420102 orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] angular_velocity: x: -0.00126681244001 y: -0.000328944995999 z: 0.00209719594568 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: 1.8828768 y: -4.95235825 z: 7.56092715

linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

I am not sure how to check for timestamps. :/

zaidtas commented 9 years ago

ohh and I am getting this error while running mavros apm.launch is this the problem? I will look into it but if you find something, let me know!

zaidtas commented 9 years ago

I was running the apm on my raspberry pi+Navio( raspbian wheezy) while I was using svo on my laptop. I am still getting the same error!

simonlynen commented 9 years ago

Can you run everything on a single machine to debug this. I doubt it has sth to do with the estimator.

zaidtas commented 9 years ago

I will try to do that...I had a hard time running svo on raspbian because of compiler tags! Now, I am facing problem with catkin_make with glog_catkin library!

Built target msf_timing [ 7%] Performing configure step for 'glog_src' /bin/sh: 1: ./configure: not found glog_catkin/CMakeFiles/glog_src.dir/build.make:100: recipe for target 'glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-configure' failed

simonlynen commented 9 years ago

Update glog_catkin sources and delete the build/glog_catkin folder. We had a wrong commit there yesterday.

On Fri, Jul 3, 2015 at 4:58 PM zuzu837 notifications@github.com wrote:

I will try to do that...I had a hard time running svo on raspbian because of compiler tags! Now, I am facing problem with catkin_make with glog_catkin library!

Built target msf_timing [ 7%] Performing configure step for 'glog_src' /bin/sh: 1: ./configure: not found glog_catkin/CMakeFiles/glog_src.dir/build.make[image: :100:] recipe for target 'glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-configure' failed

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

zaidtas commented 9 years ago

y u do dis :'( Sorry, I was busy this weekend and couldn't keep you updated. I did download the updated folder. It compiling fine till my raspberry pi ran out of virtual memory( that's the error it gives ). Now, I am looking at how to solve it.

I will keep you updated on the progress but do let me know if you know some solution to the virtual memory problem

zaidtas commented 9 years ago

catkin_make gives the following error and then it stops.

[ 97%] Built target pose_sensor [ 98%] [100%] Built target position_sensor Built target pose_pressure_sensor [100%] Building CXX object ethzasl_msf/msf_updates/src/position_pose_msf/CMakeFiles/position_pose_sensor.dir/main.cpp.o In file included from /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/implementation/msf_state_inl.h:25:0, from /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/msf_state.h:347, from /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/msf_core.h:42, from /home/pi/msf_ws/src/ethzasl_msf/msf_updates/src/position_pose_msf/position_pose_sensormanager.h:22, from /home/pi/msf_ws/src/ethzasl_msf/msf_updates/src/position_pose_msf/main.cpp:17: /home/pi/msf_ws/devel/include/glog/logging.h: In function 'std::string* google::Check_EQImpl(const T1&, const T2&, const char*) [with T1 = unsigned int, T2 = int, std::string = std::basic_string]': /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/implementation/msf_state_inl.h:359:3: instantiated from 'void msf_core::GenericState_T<StateVector_T, StateDefinition_T>::GetPoseCovariance(geometry_msgs::PoseWithCovariance_std::allocator::_covariance_type&) [with StateSeq_T = boost::fusion::vector<msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 0, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 1, 0>, msf_core::StateVar_TEigen::Quaternion<double, 2, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 3, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 4, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, 5>, msf_core::StateVar_TEigen::Quaternion<double, 6, 3>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 7>, msf_core::StateVar_TEigen::Quaternion<double, 8>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 9>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 10> >, StateDef_T = msf_updates::StateDefinition, geometry_msgs::PoseWithCovariance_std::allocator::_covariance_type = boost::array<double, 36u>]' /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/implementation/msf_state_inl.h:120:3: instantiated from 'void msf_core::GenericState_T<StateVector_T, StateDefinition_T>::ToPoseMsg(geometry_msgs::PoseWithCovarianceStamped&) [with StateSeq_T = boost::fusion::vector<msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 0, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 1, 0>, msf_core::StateVar_TEigen::Quaternion<double, 2, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 3, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 4, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, 5>, msf_core::StateVar_TEigen::Quaternion<double, 6, 3>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 7>, msf_core::StateVar_TEigen::Quaternion<double, 8>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 9>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 10> >, StateDef_T = msf_updates::StateDefinition, geometry_msgs::PoseWithCovarianceStamped = geometry_msgs::PoseWithCovarianceStamped_std::allocator]' /home/pi/msf_ws/src/ethzasl_msf/msf_core/include/msf_core/msf_sensormanagerROS.h:207:7: instantiated from 'void msf_core::MSF_SensorManagerROS::PublishStateAfterPropagation(const boost::shared_ptr&) const [with EKFState_T = msf_core::GenericState_T<boost::fusion::vector<msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 0, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 1, 0>, msf_core::StateVar_TEigen::Quaternion<double, 2, 0>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 3, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 4, 1>, msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, 5>, msf_core::StateVar_TEigen::Quaternion<double, 6, 3>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 7>, msf_core::StateVar_TEigen::Quaternion<double, 8>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 9>, msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, 10> >, msf_updates::StateDefinition>]' /home/pi/msf_ws/src/ethzasl_msf/msf_updates/src/position_pose_msf/main.cpp:27:1: instantiated from here /home/pi/msf_ws/devel/include/glog/logging.h:694:1: warning: comparison between signed and unsigned integer expressions [-Wsign-compare] ^[[B/tmp/ccKgrNsb.s: Assembler messages: /tmp/ccKgrNsb.s:9036: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:9046: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:9063: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17301: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17311: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17328: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17412: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17422: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17439: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17508: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17518: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:17535: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:19353: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:19363: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 /tmp/ccKgrNsb.s:19380: Warning: swp{b} use is deprecated for ARMv6 and ARMv7 Linking CXX executable /home/pi/msf_ws/devel/lib/msf_updates/position_pose_sensor [100%] Built target position_pose_sensor

and then it stops.

simonlynen commented 9 years ago

@zuzu837 there is no error message in your output just the compile-trace back of a warning.

zaidtas commented 9 years ago

Yeah, I realised that. Now, the library is build, i didn't source to check that up. Let me set everything up and get back to you on the results. thank you :)

zaidtas commented 9 years ago

I think msf works fine, I can get output on /msf_core/pose, it also show it recieves measurement from /svo/pose. Now, anyway to visualise the output and check if it's working correct?

simonlynen commented 9 years ago

There are multiple plotting scripts in the repository to plot the estimates. Use these or write your own rqt_plot commands.

zaidtas commented 9 years ago

Sorry to bother you again :/

I used the paramater fix file from this page: https://pixhawk.org/dev/ros/visual_estimation I realised I was using a different IMU from the pixhawk one and also my camera to imu translation and rotation vectors were wrong. ( which I will fix ). I am just trying to understand things better. :)

So, this is how I do things: I first launch the apm and Mavros, then I launch the camera node and Svo. when I start recieving position estimates from svo I launch the modified pose_sensor.launch file. I get this

* pose sensor got first measurement from topic //svo/pose * which I think is correct!

Now i initialise the filter and it gives the following output:

initial measurement pos:[-0.195877 0.0186402 -0.092778] orientation: [0.998, 0.0102, -0.0127, -0.0678] [ WARN] [1436967129.372745404]: Using simulated core plus fixed diag initial error state covariance. [ INFO] [1436967129.383230212]: Initializing msf_core (built: Jun 26 2015) [ INFO] [1436967129.383437837]: Core parameters: fixed_bias: 0 fuzzythres: 0.1 noise_acc: 0.013 noise_accbias: 0.000108 noise_gyr: 0.00065 noise_gyrbias: 2.12e-06

[ INFO] [1436967129.383849100]: Core init with state: --------- State at time 7129.27s: --------- 0 : [0-2] : Matrix<3, 1> : [-0.195877 0.0186402 -0.092778] 1 : [3-5] : Matrix<3, 1> : [0 0 0] 2 : [6-9] : Quaternion (w,x,y,z) : [0.998, 0.0102, -0.0127, -0.0678] 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] [1436967130.107689412]: /svo/pose: message drop curr seq:281 expected: 266 [ WARN] [1436967143.669012936]: large time-gap re-initializing to last state

I would like to understand the following messages: [ WARN] [1436967129.372745404]: Using simulated core plus fixed diag initial error state covariance.

and

/svo/pose: message drop curr seq:281 expected: 266

Oh and here are the results.

rostopic echo /msf_core/pose gives output but the position is diverging a lot ( I guess it's because I am using wrong parameters )

rostopic echo /msf_core/state_out gives no output.

I am sorry, but I was working on different aspects of the projectand therefore it took me time to try what you suggested

simonlynen commented 9 years ago

I would like to understand the following messages: [ WARN] [1436967129.372745404]: Using simulated core plus fixed diag initial error state covariance.

This means that the initial covariance is initialized to default values which is fine.

/svo/pose: message drop curr seq:281 expected: 266

This means that several pose messages from SVO did not arrive at the MSF. The estimator monitors if the sequence number of the messages is continuous and warns if messages are dropped.

zaidtas commented 9 years ago

and what's the explanation to this:

rostopic echo /msf_core/pose gives output but the position is diverging a lot.

rostopic echo /msf_core/state_out gives no output.

simonlynen commented 9 years ago

@zuzu837 There can be many reasons for that but common reasons are:

zaidtas commented 9 years ago

thanks, I will try to debug these issues one by one. For a start, can I understand the difference between the methods explained in the two tutorials by different sources:

1.( I am following this one ) https://pixhawk.org/dev/ros/visual_estimation errors are prone in this parameter file: ( along with this I am using the pose_sensor.launch file )

#########IMU PARAMETERS####### ####### pixhawk - MPU6050 core/core_noise_acc: 0.013 core/core_noise_accbias: 0.000108 core/core_noise_gyr: 0.00065 core/core_noise_gyrbias: 0.00000212

pose_sensor/pose_fixed_scale: false pose_sensor/pose_noise_scale: 0.0 pose_sensor/pose_noise_p_wv: 0.0 pose_sensor/pose_noise_q_wv: 0.0 pose_sensor/pose_noise_q_ic: 0.0 pose_sensor/pose_noise_p_ic: 0.0 pose_sensor/pose_delay: 0.02 pose_sensor/pose_noise_meas_p: 0.005 pose_sensor/pose_noise_meas_q: 0.02 pose_sensor/pose_initial_scale: 1

q_ic is the quaternion representing the rotation of the camera in IMU frame. Unit quaternion here as we rotate the coordinate frames in SVO parameters.

pose_sensor/init/q_ic/w: 1.0 pose_sensor/init/q_ic/x: 0.0 pose_sensor/init/q_ic/y: 0.0 pose_sensor/init/q_ic/z: 0.0

p_ic is the translation between the IMU and the camera in meters.

pose_sensor/init/p_ic/x: 0.0
pose_sensor/init/p_ic/y: 0.0 pose_sensor/init/p_ic/z: 0.0

pose_sensor/pose_absolute_measurements: true pose_sensor/pose_use_fixed_covariance: true pose_sensor/pose_measurement_world_sensor: false # we do not publish the world in camera frame as set in SVO parameters.

pose_sensor/pose_fixed_scale: false pose_sensor/pose_fixed_p_ic: true pose_sensor/pose_fixed_q_ic: true pose_sensor/pose_fixed_p_wv: false pose_sensor/pose_fixed_q_wv: false

I think I know what q_ic and p_ic to use. but what about the other values, how can I find the correct values to use specially for:

core/core_noise_acc: 0.013 core/core_noise_accbias: 0.000108 core/core_noise_gyr: 0.00065 core/core_noise_gyrbias: 0.00000212

I am using MPU9250. :)

  1. http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/custom_sensor_design

This one I think is supposed to help you design a custom sensor module which are not already defined in the msf_updates stack.

Thanks again :)

simonlynen commented 9 years ago

@zuzu837 What exactly is your question now? You will need to set the correct IMU-camera calibration as mentioned before. I don't understand how you can have an identity transformation between camera and IMU.

The precise values of the IMU noise densities are not that important for the beginning.

zaidtas commented 9 years ago

I am sorry, the correct transformation parameters are:

pose_sensor/init/q_ic/w: 1 pose_sensor/init/q_ic/x: 0 pose_sensor/init/q_ic/y: 0 pose_sensor/init/q_ic/z: 0 pose_sensor/init/p_ic/x: -0.055
pose_sensor/init/p_ic/y: -0.015 pose_sensor/init/p_ic/z: -0.063

But, it still doesn't work with these parameters and gives a diverging output.

MarcGyongyosi commented 8 years ago

Hi, so if you're rotating in SVO (which I think you are since you are giving a unit quaternion in MSF) then the parameters that were put into the pixhawk wiki are incorrect. I.e. only rotating around the x axis as described over in the wiki is in my opinion not sufficient. You have to align the IMU frame with the VSLAM frame. I never got SVO to work properly on my setup so I stopped testin that, but basically it's just a simple rotation.