ros-simulation / gazebo_ros_pkgs

Wrappers, tools and additional API's for using ROS with Gazebo
http://wiki.ros.org/gazebo_ros_pkgs
753 stars 770 forks source link

gazebo_ros_imu plugin generates very noisy linear acceleration data #245

Open shokoofeh opened 10 years ago

shokoofeh commented 10 years ago

Hi, we've noticed that the gazebo_ros_imu plugin generates very noisy imu data. The following are two related questions demonstrating the problem both in rviz and rostopic echo. http://answers.gazebosim.org/question/6983/gazebo-linear-acceleration-noise-gazebo-2-indigo/ and http://answers.gazebosim.org/question/4717/imu-sensor-plugin-libgazebo_ros_imuso-strange-data/

Has this been observed before?

We have used hector_gazebo_ros_imu which seems to generates reasonable data. You can compare the published imu data by these two plugins by getting following packages:

git clone -b jsim-imu-test --single-branch https://github.com/jackal/jackal.git
git clone -b jsim-imu-test --single-branch https://github.com/jackal/jackal_simulator.git

Usage is: (for gazebo_ros_imu plugin)

roslaunch jackal_gazebo jackal_world.launch gazebo_plugin:=jackal.gazebo

(for hector_gazebo_ros_imu)

roslaunch jackal_gazebo jackal_world.launch gazebo_plugin:=jackal.hector.gazebo
hsu commented 10 years ago

I've added comments to the answers.gazebosim.org posts asking for a model or world to reproduce the issue.

tagging some codes before moving on (while compiling jackal :):

hector_gazebo_ros_imu vs gazebo_ros_imu

hsu commented 10 years ago

It appears the problem may not be due to the plugins, but comes from bad contact point/normals between mesh and cylinder (wheel) created by the collision engine. If I replace the mesh ground with a large box or ground plane, the noise in acceleration values are a few (~3~5) order of magnitudes lower. Or if I stretch the ground mesh wider (see below, and remember to reposition the vehicle), the acceleration values are smooth when the vehicle is away from mesh edges or mesh vertices of the terrain, e.g.

    <model name="landscape">
      <link name="landscape_link">
        <pose>-15.0 -15.0 0.5 0 0 0</pose>
        <collision name="landscape_collision">
          <geometry>
            <mesh>
          <uri>file://scenario1.dae</uri>
              <scale>10 10 0.4</scale>
            </mesh>
          </geometry>

          <surface>
            <contact>
              <ode>
                <kp>100000.0</kp>
                <kd>100.0</kd>
                <max_vel>0.01</max_vel>
                <min_depth>0.002</min_depth>
              </ode>
            </contact>
          </surface>

        </collision>
        <visual name="landscape">
          <geometry>
            <mesh>
          <uri>file://scenario1.dae</uri>
              <scale>10 10 0.4</scale>
            </mesh>
          </geometry>
          <cast_shadows>false</cast_shadows>
        </visual>
      </link>
      <static>true</static>
    </model>

Have you tried using heightmap?

mikepurvis commented 10 years ago

@hsu, thanks for the response. We borrowed this world from the hratc repo. Is there a better example of a pre-built "outdoor world" that we could use without building one from scratch?

My perception is that it's more than just acceleration data— it seems like the orientation data is all over the place too. Should that also be as affected by bad contact points?

shokoofeh commented 10 years ago

@hsu, I've made mentioned changes. it seems the noise in acceleration data is almost same (see attached plots) but there is no gravity in linear acceleration data.

gazebo_ros_imu plugin: ros-1-z

hector_gazebo_imu: hec-3-z