ccnyroboticslab / imu_tools Goto Github PK

test the code,have lager heading errors, can improve accuracy，used the method in paper：

An Adaptive Orientation Estimation Method for Magnetic and Inertial Sensors in the Presence of Magnetic Disturbances

http://www.mdpi.com/1424-8220/17/5/1161

Hey there,

it isnt really an issue just something that would have saved me some time.
When sending messages on the imu/mag topics that dont have timestamps (because somebody without a whole lot of ros knowledge wrote the code so send them) the filter outputs data without the orientation data computed.
It would be nice to have an error message for that case.

I noticed that it drifted a lot with my IMU so I did a simple test.

I supplied (0,0,0) for angular velocity, (0, 0, 9.81) for linear acceleration and (-0.420,+0.302, 0.835) for magnetic vector. The resulting orientation (I looked at Euler angles) was very noisy and didn't converge (picture below).

If, however, the original code is used (I just replaced ImuFilter::madgwickAHRSupdate with Update function from https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MadgwickAHRS.cs) the output is very smooth and converges quickly (picture below).

I suspect that a mistake was made while copying the original Madwick's code (something as silly as q1 instead of q2 could cause this).

I had to make this change for the visualization tutorials: ros-visualization/visualization_tutorials#32

I think your changes might be very similar.

Currently if I try to add your imu display in rviz on Kinetic, rviz just crashes.

Currently no covariance is provided for the output orientations. Short of a proper covariance propagation from the original sensor sources, I'd like to stub in a parametrized reasonable fixed covariance matrix, so that the output can be used with robot_localization and robot_pose_ekf

When use_mag is set to true (default), the node waits for matching pairs of messages on topics /imu/data_raw and either /imu/mag or /imu/magnetic_field. If the user does not provide one of the magnetic field topics, no data will be published on /imu/data. There should be a warning in this case.

see http://answers.ros.org/question/236966/imu_filter_madgwick-not-publishing-on-imudata/

Hi, I'm trying the imu_filter_madgwick installed from synaptics with all default parameters (with out Mag.)

I publish the sensor data in /imu/data_raw and messages are there, I can see how the acceleration vector changes according to the orientation.

I see in the node graph that the imu_filter_madgwick is subscribed to that topic, so it should be receiving what I'm publishing (I also tried the nodelet interface)

I can see that the /imu/data topic is there with rostopic list but nothing is coming from there when I use rostopic echo /imu/data. In fact, that topic does not appear in the node graph.

Am I missing something in the setup?
For instance, I see in Seb's videos that he starts with a certain configuration of the IMU and wait some seconds before moving it around.

Consider moving to the ros-drivers org, as well?

Hi,
I added the imu complementary filter (from indigo branch) to a my own library to process data (accelerometer, gyroscope and magnetometer) from a Invensense MPU9250.
The library work well but the yaw angle has a 180 degrees rotation when compared to the Mahony filter previously used in the same library (the yaw angle seems to be referred to the Geographic South Pole).

By reading the "Roberto G. Valenti, Ivan Dryanovski and Jizhong Xiao" paper, I see that the magnetic correction assume positive values (x component) for vectors pointing the Geographic North Pole but the magnetomer provide negative values in a such situation, so I need to invert the sign of the magnetic field before processing it.
Is right a such affirmation ?
Thank you.

I accidentally change the topic to an Imu without a fused orientation estimate. This crashed RVIZ with the following error:

rviz: /tmp/buildd/ros-fuerte-visualization-common-1.8.4/debian/ros-fuerte-visualization-common/opt/ros/fuerte/stacks/visualization_common/ogre/build/ogre_src_v1-7-3/OgreMain/include/OgreAxisAlignedBox.h:252: void Ogre::AxisAlignedBox::setExtents(const Ogre::Vector3&, const Ogre::Vector3&): Assertion `(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"' failed.

You should probably catch the 0/0/0/0 orientation case and check orientation_covariance[0] < 0 before attempting to draw.

Hello, I am new to using ROS and I need some help with integrating this package to the project I am working on. I have a bag file which publishes /imu/data_raw topic
rosbag play -l <file_name> /initial/name:= /imu/data_raw
The only difference I find in the imu messages from the samples and my bag file is frame_id. The sample bag file messages publishes frame_id: imu, where as my bag file publishes frame_id: base_link.
When I run rosrun imu_filter_madgwick imu_filter_node use_mag_ := false with sample bag file, the /imu_filter publishes messages to /imu/data, where as there are no messages published if I use my bag file.
Can anyone help?
Regards
Chirag

It would be usefull to have a C++ API to the madgwick algorithm.

Should be fairly easy to implement by splitting ImuFilter class. If you like, I'll submit a pull request.

I am using the Complementary Filter on a custom board. Using the MPU-9255.
When I examine the quaternion, it seems that the pitch and roll axes swap based on yaw.
I am basing this on the InvenSense Teapot graphing routine that I have used with numerous quaternion visualizations. It works as expected when I pass the data as ENU into the Madgwick (imu_filter) implememtation in the GitHub, but behaves very strangely when I pass NWU data into the Complementary Filter.

I would appreciate any pointers, as I have paired it back to the Gyro based prediction step.

Thanks!

Hi, I've noticed one missing piece of code in your implementation compared to the Madgwick's sample code in his paper.
Basically he also considered gyro drift (deg/s/s) in the parameter "zeta" to adjust the quaternion (when using mag readings).

Is there a reason why this is not done here?

Thanks!

Hi

When I am trying to install the plugin I get this error:
[ 25%] Building CXX object CMakeFiles/rviz_imu_plugin.dir/src/imu_display.o
In file included from /home/emi/work_electric/rviz_imu_plugin/include/rviz_imu_plugin/imu_display.h:38:0,
from /home/emi/work_electric/rviz_imu_plugin/src/imu_display.cpp:31:
/opt/ros/electric/stacks/visualization/rviz/src/rviz/visualization_manager.h:37:22: fatal error: wx/event.h: No such file or directory
compilation terminated.
on Ubuntu 11.10 and ROS Electric

Can you give me a clue to solve this?
Thank you

With version 0.5.15 release of tf2, it now checks for quaternion normalization.
As far as I am concerned, imu_filter_madgwick does not provide a normalized quaternion, hence an error :

Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "imu" from authority "unknown_publisher" because of an invalid quaternion in the transform (-0.018815 0.013563 -1.000259 0.046847)
         at line 253 in /tmp/binarydeb/ros-kinetic-tf2-0.5.15/src/buffer_core.cpp

For now, I fixed it that way :

void ImuFilterRos::publishTransform(const ImuMsg::ConstPtr& imu_msg_raw)
{
  double q0,q1,q2,q3;
  filter_.getOrientation(q0,q1,q2,q3);
  geometry_msgs::TransformStamped transform;
  transform.header.stamp = imu_msg_raw->header.stamp;
  double d = sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

  if (reverse_tf_)
  {
    transform.header.frame_id = imu_frame_;
    transform.child_frame_id = fixed_frame_;
    transform.transform.rotation.w = q0/d;
    transform.transform.rotation.x = -q1/d;
    transform.transform.rotation.y = -q2/d;
    transform.transform.rotation.z = -q3/d;
  }
  else {
    transform.header.frame_id = fixed_frame_;
    transform.child_frame_id = imu_frame_;
    transform.transform.rotation.w = q0/d;
    transform.transform.rotation.x = q1/d;
    transform.transform.rotation.y = q2/d;
    transform.transform.rotation.z = q3/d;
  }
  tf_broadcaster_.sendTransform(transform);

}

which might be unsafe to some extend given the possibilty of d=0.

How do we make that change? I want to drop the imu_tools package into my workspace, and just run catkin from there to compile it.

Hi,

I've started using the Xsens MTi-10 IMU for our project. I first use the ros driver (https://github.com/ethz-asl/ethzasl_xsens_driver) for the imu to get imu/data_raw and imu/mag published, so that your imu_tools can work.

But when I enabled the use_mag option, the output YAW value (I used tf to convert your quaternion to RPY) drifts a lot when I move the IMU device along a straight line, while the ROLL and PITCH values are quite stable. Also, is the yaw a value relative to the earth north?

Is this an issue that cannot be avoided if the IMU is used indoors? 'cuz the magnetic measurements are affected indoors?

Of course, if I don't use the mag, the large drift problem of yaw is no longer there. But a slow accumulated drift is still there. (around 2 deg/min or so).

Please correct me if I'm using any parts in a wrong way...

I have the following in my launch file:

  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_madgwick" output="screen" respawn="false" >
      <param name="~publish_debug_topics" value="True" />
      <param name="~use_mag" value="True" />
      <remap from="imu/data" to="imu/data2" />
  </node>

When use_mag is set to False, this generates the two topics /imu/rpy/raw and /imu/rpy/filtered. When use_mag is set to True, this only generates /imu/rpy/filtered.

Is this the intended behaviour?

Hi,

I tried imu_filter_madgwick with Microstrain Imu (http://www.ros.org/wiki/microstrain_3dmgx2_imu), but ran into problems due to the time step estimation. Namely, in the filter, dt is calculated using the header time stamps available in the consecutive Imu input messages. The reality is (at least with my hardware - Lenovo X200 laptop, Intel Core Duo 2.4GHz running Ubuntu 12.04) that the time step between samples is not constant. Sometimes samples come in bursts from the sensor so multiple consecutive messages have exactly the same time stamp. This will ruin the dt calculation. Nevertheless, the sampling rate for sensor is known (100HZ) and on average, 100 messages are received every second.

So what I would suggest:
-dt could be ROS parameter for the filter node
-if dt is not set, it is estimated based on the time stamps as it is done now

Best Regards,
Paul Kemppi

It seems very odd to me that the frame_id from data_raw is not simply copied directly to the data output message.

What is the rationale for the fixed frame value, and the optional TF publication?

I am having a strange problem when using the filter. When I do "rosrun imu_filter_madgwick imu_filter_node" everything seems to work fine except that the signs of the orientations switch about every second. At first I thought that it was every second on the second but it is not, as you can see below. This is the output from rostopic echo /imu/data using a Phidget Spatial 3/3/3 at the moment the switch occurs. The important part to look at is the orientation; I did not move the imu at all and I certainly did not flip it about every axis in a fraction of a second.

header:
seq: 651
stamp:
secs: 1374457284
nsecs: 922849322
frame_id: odom
orientation:
x: 0.0629537322659
y: -0.10274833374
z: 0.444023642596
w: 0.885972732823
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: 0.000351509311352
y: -0.00105452793405
z: -0.00152245070651
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.134691305761
y: 0.0928026039696
z: 9.67216991372

linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

header:
seq: 652
stamp:
secs: 1374457284
nsecs: 930849322
frame_id: odom
orientation:
x: -0.0625505324731
y: 0.102060354997
z: -0.444059882587
w: -0.886064103676
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.00316375833509
y: 0.000703018622703
z: -0.000936892742471
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.1239003053
y: 0.0868185037136
z: 9.66804971354
linear_acceleration_covariance: [8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06, 0.0, 0.0, 0.0, 8.66124974095918e-06]

If anyone could identify what is causing this I would very much appreciate it, and I would be happy to post any additional information that may be needed. Thanks!

I'm using the ComplementaryFilter code in https://github.com/ccny-ros-pkg/imu_tools/blob/indigo/imu_complementary_filter/src/complementary_filter.cpp. I wanted to make sure I understood how it worked and how you've aligned the axes so I did a quick test. Specifically, I'd construct a new ComplementaryFilter and then I'd call update 5 times passing in [1, 0, 0] for my accelerations and [0, 0, 0] for the gyros indicating a dt of 1 second (e.g. cf.update(1, 0, 0, 0, 0, 0, 1)). So, in this example, I should be telling the ComplementaryFilter that I'm seeing exactly 1 force of gravity in the x direction. I then ask for the orientation and I compute the pitch and roll. I then repeated the above experiment but passing [0, 1, 0] (1G in the y-direction) and [0, 0, 1] (1G in the z direction).

What I saw was that 1G in the x direction yields a pitch of 90 degrees and no roll which would indicate that x points "forward". However, 1G in the y or z directions yields no pitch and no roll. It makes sense that 1G in the z direction would produce no pitch or roll as z is, I think, up/down so that indicates that nothing has changed. However, if 1G in the x direction is pitch I'd expect 1G in the y direction to be roll but it isn't.

To do this experiment I wrote a tiny Python/cython wrapper for your code:

from libcpp cimport bool
import numpy as np
import quaternion as quat


cdef extern from "complementary_filter.h" namespace "imu_tools":
    cdef cppclass ComplementaryFilter:
        ComplementaryFilter() except +
        void setDoAdaptiveGain(bool do_adaptive_gain)
        void update(double ax, double ay, double az, 
                    double wx, double wy, double wz,
                    double dt)
        void getOrientation(double& q0, double& q1, double& q2,
                            double& q3) const
        void setOrientation(double q0, double q1, double q2, double q3)

cdef class OrientationFilter:
    """A Python wrapper for the ComplementaryFilter. Note that I didn't do anything to
    correct for the differences in axis definitions. See the RevlOrientation class for
    a more convenient way to use this code."""
    cdef ComplementaryFilter cf

    def __cinit__(self, do_adaptive_gain=True):
        """Constructor.

        Args:
            do_adaptive_gain: if True the gain from the accelerometers will be
               dynamically adjusted so it matters more when the device isn't
               being accelerated and less when it is.
        """
        self.cf = ComplementaryFilter()
        self.cf.setDoAdaptiveGain(do_adaptive_gain)


    def update(self, double ax, double ay, double az,
               double wx, double wy, double wz,
               double dt):
        """Pass new data to the filter to have the estimates updated.

        Args:
            ax, ay, az: acclerations in the x, y, and z direction. Units are
                multiples of the force of gravity so that 1.0 means 1G.
            wx, wy, wz: angular velocity in radiaans / second.
            dt: time delta in seconds.
        """
        self.cf.update(ax, ay, az, wx, wy, wz, dt)

    @property
    def orientation_quat(self):
        """Returns the orientation of the camera with respect to the world
        as a quaternion. The return value is a quaternion instance."""
        cdef double q0 = 0.0
        cdef double q1 = 0.0
        cdef double q2 = 0.0
        cdef double q3 = 0.0

        self.cf.getOrientation(q0, q1, q2, q3)
        return quat.quaternion(q0, q1, q2, q3)

And my test code looks like this:

import orientation_filter as of
from revl_orientation import RevlOrientation
import quaternion as quat
import numpy as np

def single_move(ax, ay, az, rx, ry, rz):
    f = of.OrientationFilter()
    for x in range(10):
        f.update(ax, ay, az, rx, ry, rz, 1)
    return f.orientation_quat

def to_pitch(q):
    q = q.components
    return np.degrees(np.arcsin(-2 * (q[0] * q[2] - q[3] * q[1])))

def to_roll(q):
    q = q.components
    tmp = 2 * (q[1] * q[2] + q[0] * q[3])
    tmp = tmp / (q[3]**2 + q[2]**2 - q[1]**2 - q[0]**2)
    return np.degrees(np.arctan(tmp))

def pnr(cw_quat):
    pitch = to_pitch(cw_quat)
    roll = to_roll(cw_quat)
    return (pitch, roll)

xg = single_move(1, 0, 0, 0, 0, 0)
yg = single_move(0, 1, 0, 0, 0, 0)
zg = single_move(0, 0, 1, 0, 0, 0)

xpr = pnr(xg)
ypr = pnr(yg)
zpr = pnr(zg)

print('When gravity is +x:', xpr)
print('When gravity is +y:', ypr)
print('When gravity is +z:', zpr)

the output is:

When gravity is +x: (90.0, -0.0)
When gravity is +y: (-0.0, 0.0)
When gravity is +z: (0.0, -0.0)

Is there a bug in the code or my understanding?

Thanks!!

Hello,
How can I find your proposed source code?

Thanks

Hi

I'm trying to compile imu_tools from source against ROS indigo on OS X (10.12.1, XCode 8.1). However, when compiling my workspace with catkin_make, I get this errors

I googled the error message, and everything pointed at message_filters not being added in the CMakeLists.txt or the package.xml. I checked, and even added it in the rviz_imu_plugin package (although I guess it's not necessary), but to no success.

I was wondering if you have any idea what might be causing this problem.

Thank you very much.

I would like to use your filter for several IMUs to find the rotation from one IMU to another. This requires that they all have the same base or world frame - is that true for this filter?

I thought it should be true of this filter as long as I am using the magnetometer data, but how do I tell if I am using the magnetometer data or not? My magnetometer data is being published to the ros topic "/imu/mag" as a Vector3 Stamped type message. Can the code in its current state use this magnetometer information?

I have been playing with this for hours, but cannot get any of my IMUs to have the same orientations even when I can see them both sitting in front of me in the same orientation. I'm not sure what the problem is.

Thanks.

Hi all
I'm building a robot running ROS on board using a Raspberry PI.

SYSTEM CONFIGURATION:
Raspberry PI running ubuntu wheezy and custom ROS Indigo.

PROBLEM:
I tried to install the imu_filter_madgwick package but failed. It seems to be necessary to install rviz as a prerequisite for the installation of the imu_filter_madgwick. Since I have a custom installation, I followed the procedure described in

http://wiki.ros.org/ROSberryPi/Installing%20ROS%20Indigo%20on%20Raspberry%20Pi

To be precise, I used the following commands to download the packages and install it:
1: cd ROS_Comm
2: rosinstall_generator ros_comm rviz imu_tools --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo-custom_ros.rosinstall
3: wstool merge -t src indigo-custom_ros.rosinstall
4: wstool update -t src
5: rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:wheezy
6: sudo apt-get update
7: sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/indigo

The error log is reported below.

QUESTION:
Is it a way to install only imu_madgwick_filter package without install imu_tools and rviz packages?

Any help is appreciated. Thank you very much.
Mauro

////@@@@
Error log:
==> make -j4 -l4 in '/home/pi/ROS_Comm/build_isolated/rviz'
Scanning dependencies of target interactive_marker_test
[ 0%] Built target connect_test
[ 0%] Building CXX object src/test/CMakeFiles/interactive_marker_test.dir/interactive_marker_test.cpp.o
Scanning dependencies of target rviz
[ 0%] [ 0%] [ 0%] Building CXX object src/rviz/CMakeFiles/rviz.dir/frame_manager.cpp.o
Building CXX object src/rviz/CMakeFiles/rviz.dir/frame_position_tracking_view_controller.cpp.o
Building CXX object src/rviz/CMakeFiles/rviz.dir/image/ros_image_texture.cpp.o
Linking CXX executable /home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/interactive_marker_test
/tmp/ccX1hvNB.s: Assembler messages:
/tmp/ccX1hvNB.s:2010: Warning: swp{b} use is deprecated for ARMv6 and ARMv7

...

/tmp/ccX1hvNB.s:7021: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccX1hvNB.s:7038: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
[ 0%] Built target interactive_marker_test
[ 0%] [ 0%] Building CXX object src/rviz/CMakeFiles/rviz.dir/image/image_display_base.cpp.o
Building CXX object src/rviz/CMakeFiles/rviz.dir/properties/tf_frame_property.cpp.o
/tmp/ccJX50cx.s: Assembler messages:
/tmp/ccJX50cx.s:1336: Warning: swp{b} use is deprecated for ARMv6 and ARMv7

...

/tmp/cc2QOWZ4.s:899: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
Linking CXX shared library /home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so
[ 55%] Built target rviz
Scanning dependencies of target rviz_image_view
Linking CXX executable /home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/rviz
Linking CXX executable /home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/color_editor_test
[ 55%] Building CXX object src/image_view/CMakeFiles/rviz_image_view.dir/image_view.cpp.o
Scanning dependencies of target default_plugin
/home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference to vtable for Assimp::IOSystem' /home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference totypeinfo for Assimp::IOSystem'
collect2: ld returned 1 exit status
src/rviz/CMakeFiles/executable.dir/build.make:212: recipe for target '/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/rviz' failed
make[2]: *** [/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/rviz] Error 1
CMakeFiles/Makefile2:1659: recipe for target 'src/rviz/CMakeFiles/executable.dir/all' failed
make[1]: *** [src/rviz/CMakeFiles/executable.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 56%] /home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference to vtable for Assimp::IOSystem' /home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference totypeinfo for Assimp::IOSystem'
collect2: ld returned 1 exit status
src/test/CMakeFiles/color_editor_test.dir/build.make:214: recipe for target '/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/color_editor_test' failed
make[2]: *** [/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/color_editor_test] Error 1
CMakeFiles/Makefile2:2148: recipe for target 'src/test/CMakeFiles/color_editor_test.dir/all' failed
make[1]: *** [src/test/CMakeFiles/color_editor_test.dir/all] Error 2
Building CXX object src/image_view/CMakeFiles/rviz_image_view.dir/main.cpp.o
[ 56%] Building CXX object src/image_view/CMakeFiles/rviz_image_view.dir/moc_image_view.cxx.o
[ 56%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/axes_display.cpp.o
/tmp/ccDnygzd.s: Assembler messages:
/tmp/ccDnygzd.s:1063: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccDnygzd.s:1073: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccDnygzd.s:1090: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
Linking CXX executable /home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/image_view
/tmp/ccMtlwCs.s: Assembler messages:
/tmp/ccMtlwCs.s:555: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccMtlwCs.s:565: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccMtlwCs.s:582: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
[ 57%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/depth_cloud_display.cpp.o
/home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference to vtable for Assimp::IOSystem' /home/pi/ROS_Comm/devel_isolated/rviz/lib/librviz.so: undefined reference totypeinfo for Assimp::IOSystem'
collect2: ld returned 1 exit status
src/image_view/CMakeFiles/rviz_image_view.dir/build.make:264: recipe for target '/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/image_view' failed
make[2]: *** [/home/pi/ROS_Comm/devel_isolated/rviz/lib/rviz/image_view] Error 1
CMakeFiles/Makefile2:1794: recipe for target 'src/image_view/CMakeFiles/rviz_image_view.dir/all' failed
make[1]: *** [src/image_view/CMakeFiles/rviz_image_view.dir/all] Error 2
[ 57%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/effort_display.cpp.o
[ 57%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/depth_cloud_mld.cpp.o
[ 57%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/camera_display.cpp.o
/tmp/cctioS6c.s: Assembler messages:
/tmp/cctioS6c.s:693: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/cctioS6c.s:702: Warning: swp{b} use is deprecated for ARMv6 and ARMv7

...

/tmp/cctioS6c.s:7239: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/cctioS6c.s:7249: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
[ 57%] Building CXX object src/rviz/default_plugin/CMakeFiles/default_plugin.dir/effort_visual.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-4.6/README.Bugs for instructions.
src/rviz/default_plugin/CMakeFiles/default_plugin.dir/build.make:356: recipe for target 'src/rviz/default_plugin/CMakeFiles/default_plugin.dir/effort_display.cpp.o' failed
make[2]: *** [src/rviz/default_plugin/CMakeFiles/default_plugin.dir/effort_display.cpp.o] Error 4
make[2]: *** Waiting for unfinished jobs....
/tmp/ccj6TSGG.s: Assembler messages:
/tmp/ccj6TSGG.s:1591: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/ccj6TSGG.s:1601: Warning: swp{b} use is deprecated for ARMv6 and ARMv7

...

/tmp/cczRZE7s.s:80706: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
/tmp/cczRZE7s.s:80723: Warning: swp{b} use is deprecated for ARMv6 and ARMv7
CMakeFiles/Makefile2:1744: recipe for target 'src/rviz/default_plugin/CMakeFiles/default_plugin.dir/all' failed
make[1]: *** [src/rviz/default_plugin/CMakeFiles/default_plugin.dir/all] Error 2
Makefile:123: recipe for target 'all' failed
make: *** [all] Error 2
<== Failed to process package 'rviz':
Command '['/opt/ros/indigo/env.sh', 'make', '-j4', '-l4']' returned non-zero exit status 2

Reproduce this error by running:
==> cd /home/pi/ROS_Comm/build_isolated/rviz && /opt/ros/indigo/env.sh make -j4 -l4

Command failed, exiting.
pi@raspberrypi ~/ROS_Comm $

The visibility and scale of rviz_imu_plugin elements are not properly initialized when read from an rviz configuration file.

• Add an rviz_imu_plugin node to rviz and set its visibility to false
• Save the configuration and restart rviz

The acceleration arrow will be visible although the plugin node itself is set to invisible. Also axes are not scaled correctly. ImuDisplay::onInitialize() sets the visibility manually instead of calling the proper update methods (e.g. ImuDisplay::updateAxes()) which also sets the scale.

how can i use rviz_imu_plugin to display my mpu6050?
and i did this way https://github.com/lligen/myserial
can you help me ，please，thank you very much

As I see there is no magnetic declination adjustment.
What is the reason for this, or it's just smth waiting for its implementation?

The imu_filter_madgwick node currently publishes according to the NED convention (i.e., yaw = 0 if the x axis points north). However, REP 103 states that the ENU convention should be used (i.e., yaw = 0 if x points east).

This is only relevant if magnetometer input is used to correct yaw, otherwise the initial yaw will be determined arbitrarily by the initial orientation of the device and not referenced to anything.

Fixing this issue involves both the magnetometer initialization code (in computeRPY()) and the filtering code itself (in madgwickAHRSupdate()), since both need to be consistent. Since there are bugs in computeRPY() anyway, it would be ideal if we'd use parts of the code used in the AHRS update for the initialization.

I don't have time to work on this myself, but would be happy to accept pull requests.

Admittedly i'm a newbie that's trying to wrap my head around this paper, but I think that there were two assumptions made about using the magnetometer data in the initial quaternion correction.

1. The magnetometer vector was normalized (page 7, 2nd paragraph)
2. transposition of q0 and q3 depending on lx >= or < 0 (page 11, eq 35)

I couldn't find neither of those applied in:

https://github.com/ccny-ros-pkg/imu_tools/blob/kinetic/imu_complementary_filter/src/complementary_filter.cpp#L349

What am I getting wrong or is this a bug?

Hi,

I'm running a PNI Trax IMU with the madwick filter with the magnetometer disabled. But imu/data starts with a random angle for the yaw angle. Is there a provision to set the initial yaw angle to zero?

Thanks

On Ubuntu 11.10 and ROS Groovy. Getting 1 package failure when i rosmake imu_tools (because property_manager.h does not exist in the latest rviz package).

Output on terminal:
{---------------------------------------------------------------------
[rosbuild] Building package rviz_imu_plugin
-- Using CATKIN_DEVEL_PREFIX: /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/groovy
-- This workspace overlays: /opt/ros/groovy
-- Found gtest: gtests will be built
-- catkin 0.5.63
[rosbuild] Including /opt/ros/groovy/share/roscpp/rosbuild/roscpp.cmake
[rosbuild] Including /opt/ros/groovy/share/rospy/rosbuild/rospy.cmake
-- Configuring done
-- Generating done
CMake Warning:
Manually-specified variables were not used by the project:

  CMAKE_TOOLCHAIN_FILE

-- Build files have been written to: /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build
cd build && make -j -l8
make[1]: Entering directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' make[2]: Entering directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
make[3]: Entering directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' make[3]: Leaving directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
[ 0%] Built target rospack_genmsg_libexe
make[3]: Entering directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' make[3]: Leaving directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
[ 0%] Built target rosbuild_precompile
make[3]: Entering directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' make[3]: Leaving directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
make[3]: Entering directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' [ 25%] Building CXX object CMakeFiles/rviz_imu_plugin.dir/src/imu_display.cpp.o In file included from /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/src/imu_display.cpp:31:0: /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/include/rviz_imu_plugin/imu_display.h:40:46: fatal error: rviz/properties/property_manager.h: No such file or directory compilation terminated. make[3]: *** [CMakeFiles/rviz_imu_plugin.dir/src/imu_display.cpp.o] Error 1 make[3]: Leaving directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
make[2]: *** [CMakeFiles/rviz_imu_plugin.dir/all] Error 2
make[2]: Leaving directory /opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build' make[1]: *** [all] Error 2 make[1]: Leaving directory/opt/ros/groovy/stacks/imu_tools/rviz_imu_plugin/build'
-------------------------------------------------------------------------------}
[ rosmake ] Output from build of package rviz_imu_plugin written to:
[ rosmake ] /root/.ros/rosmake/rosmake_output-20130312-225252/rviz_imu_plugin/build_output.log
[rosmake-2] Finished <<< rviz_imu_plugin [FAIL] [ 3.76 seconds ]
[ rosmake ] Halting due to failure in package rviz_imu_plugin.
[ rosmake ] Waiting for other threads to complete.
[ rosmake ] Results:
[ rosmake ] Built 64 packages with 1 failures.
[ rosmake ] Summary output to directory
[ rosmake ] /root/.ros/rosmake/rosmake_output-20130312-225252

This package provides a shared object, a nodelet, and a node, all with equivalent functionality. The normal expectation for GPL code is "no static or dynamic linking to it, except from other GPL code."

It seems clear that one can launch a GPL node alongside other BSD-licensed nodes, but what about a plugin/nodelet?

For now, I'll just stick with the node, but it would be great to know if the nodelet or creating a special-purpose plugin might be a possibility in the future.

Hi @idryanov , could you please change the default branch from "master" to something else, like hydro, for example (Settings -> Default Branch)? I don't have the permissions to do that.

This would allow me to delete the master branch. It's only an (often outdated) alias of fuerte anyway, and many people don't realize that, so they are using the wrong branch for groovy/hydro.

I was trying to make a catkin package for this library. Would one currently exist or is it in the works?

Also, as I was trying roll my own catkin conversion I also noticed that imu_filter.h includes ImuFilterMadgwickConfig.h which doesn't seem to be included in the github repo. Could this perhaps be auto generated and I am missing something in the cmake magic?

Let me know if you could use a hand doing to conversion to catkin. I would be more than happy to help out or test the package.

I first add rviz_imu_plugin, and then when I click remove button, the rviz crash... I tried many times, every time it is crash..

Hello,

Thanks again for the excellent contribution to the ROS community. I have still few suggestions related to the imu_filter:

Is there some reason why the initial orientation is set to (1, 0, 0, 0) and not based on e.g. the first acceleration vector (that could provide good estimate for the 2 degrees of freedom assuming that the sensor is initially stationary)? If the filter gain is set to a small value (0.01), it takes quite long time for the filter to converge especially if the sensor happens to have totally different initial attitude compared to the (1, 0, 0, 0). On the other hand, setting the gain to 1.0, will result faster convergence, but more noise is then introduced to the output later on.

How about adding a possibility to adjust the gain parameters via dynamic reconfigure (http://ros.org/wiki/dynamic_reconfigure) on the fly? That would be a convenient way to search the optimal gain values for a specific sensor / application.

Best Regards,
Paul

Needed to add namespace: std::isnan() and needed to add rosbuild_link_boost(imu_filter signals) to CMakeLists.txt. Diff

[kevin@tardis imu_filter_madgwick]$ git diff
diff --git a/imu_filter_madgwick/CMakeLists.txt b/imu_filter_madgwick/CMakeLists
index f0ae3a6..662bb22 100644
--- a/imu_filter_madgwick/CMakeLists.txt
+++ b/imu_filter_madgwick/CMakeLists.txt
@@ -16,6 +16,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

 # create imu_filter library
 rosbuild_add_library (imu_filter src/imu_filter.cpp)
+rosbuild_link_boost(imu_filter signals)

 # create imu_filter_nodelet library
 rosbuild_add_library (imu_filter_nodelet src/imu_filter_nodelet.cpp)
diff --git a/imu_filter_madgwick/src/imu_filter.cpp b/imu_filter_madgwick/src/im
index 7f5c11e..5c13246 100644
--- a/imu_filter_madgwick/src/imu_filter.cpp
+++ b/imu_filter_madgwick/src/imu_filter.cpp
@@ -163,7 +163,7 @@ void ImuFilter::madgwickAHRSupdate(
        float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1

        // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in 
-       if(isnan(mx) || isnan(my) || isnan(mz))
+       if(std::isnan(mx) || std::isnan(my) || std::isnan(mz))
   {
                madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
                return;

hello, dear, I want to combine your module with "robot_localization" module and "Ros Navigation" module to work on my 4 wheels robot, is it possible or feasible? I have some issues puzzled me.

1. how do i adapt the "robot_localization" module and "Ros Navigation" module ? I know I should pass the IMU filters result ---> robot_localization ---> Ros Navigation, As to navigation module, which is it start entry node ? amcl or robot_pose_ekf ?
2. Is there any other good alternative or suggestion? I think you must know the answer.

could you please give me some instructions on this? Thank you very much.

In other word, what the axis orientation of earth coordinate frame for this novel algorithm?
I rewrote the complementary filter for MATLAB, but something is not right. I just wondering if the orientation of my sensor data is correct for the algorithm.
My sensor data coordinate like this:
http://rbwhitaker.wdfiles.com/local--files/3d-coordinate-systems/cartesian.png

Currently, imu_filter_madgwick assumes all incoming data is on frame where the +ve z axis points 'up'. This does not work for NED IMUs since these have +ve z axis pointing 'down' towards the ground when the IMU is upright.

Working on a pull to transform the IMU data into a parametrized imu_reference_frame (e.g. base_link) before filtering. Documentation will have to be added that this frame has to be ENU for the filter to obtain the correct orientation estimate.

We have noticed that the variance/covariance matrix in messages sent by the complementary filter are unset (zero). A parameter to set a static variance similar to #41 would be enough probably.

Hello, I have troubles when trying to use imu_filter_madgwick to get absolute orientation of my robot.

First experiment with Phidgets 1044. I just plugged it in and connected /imu/raw_data and imu/mag to imu_filter_madgwick.
Second experiment with IMU embedded in Parrot SLAMdunk. I just plugged it in and connected it as well.

I compared the results this way : I moved the IMUs until the orientation in the topic /imu/data (output of imu_filter_madgwick) says "x:0 y:0 z0 w:1".
Then, I had a look at the signs of the accelerations and I draw the axis X,Y,Z on the picture above, for each IMU.

Not sure I am able to say why there is a problem but I really feel like if I do not correct anything, before feeding some EKF with the imu/data, something will not work on my robot with at least one of those two IMU.

Thank you in advance
Yvon

http://jenkins.ros.org/view/IbinT64/job/ros-indigo-imu-complementary-filter_binarydeb_trusty_amd64/

It seems like it does not declare a dependency on Eigen.

Or hey, use a shapeshifter and support both.

http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html

Hi, dear,
I'm using imu_complementary_filtermodule, and combined with robot_localizationpackage. but find something strange of imu orientation output of imu_complementary_filtermodule.

I have an NED global frame imu, and I transform the body-NED data (gyro and accelmeter) to body-ENU, and feed the results into imu_filter module. but the data is not correct after drawing in excel.

my drawing is here: here,

I drive car from north -> west->south->east. the global data and orientation is right, but local odometry orientation data which integrated directly from imu_filter package is not correct, it seems like NWU frame.
I wonder to know what is output of imu_complementary_filterorientation data's global frame based on? NWU frame?
or do I need to do the frame transform from NWU to ENU? because robot_localizationpackage only accept ENU frame data of imu.
Thank you!