Ask Your Question
0

Detect terrain slope with filtered odometry and RTAB-map (IMU, LMS111, Husky, R200)

asked 2016-09-02 07:54:49 -0600

Marcus Barnet gravatar image

updated 2016-09-05 05:42:24 -0600

Hi to all,

I'm trying to perform 3D mapping by using Husky A200, filtered odometry (wheel encoders + IMU), Sick laser LMS111 and Real Sense R200 with RTAB.

My purpose is to be able to detect terrain slopes or curvatures.

I tried to run the algorithm like discussed on my previous topic, by placing a slope between the intersection of two rows of grapes as you can see in this picture (the slope is placed on the red flag).

image description

A more closer view:

image description

As you can see, it is not possible to detect it on the 3D map since the robot move over the slope like if it was a normal flat surface. I was expecting to see the robot climbing over the slope or, at least, changing its position along its Z-axis. For the moment, the terrain looks like a flat surface where I places the slope.

Is it possible to find out how to detect slopes or terrain curvatures by using RTAB and filtered odometry?

EDIT:

This is the content of my localization.yaml file used by robot_localization for the filtered odometry. This is my control.launch from Husky and this is the ekf.launch file included in robot_localization package.

odom_frame: odom
base_link_frame: base_link
world_frame: odom

two_d_mode: false

frequency: 50

odom0: husky_velocity_controller/odom
odom0_config: [false, false, false,
               false, false, false,
               true, true, true,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_queue_size: 10

imu0: imu/data
imu0_config: [false, false, false,
              true, true, true,
              false, false, false,
              true, true, true,
              true, true, true]
imu0_differential: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

and this is my control.launch file.

<?xml version="1.0"?>
<launch>



  <rosparam command="load" file="$(find husky_control)/config/control.yaml" />

  <node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>

  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
    <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
     <param name="two_d_mode" value="false"/>
  <!--  <param name="odom1" value="/vo"/> 
    <rosparam param="odom1_config">[true, true, true, false, false, true, false, false, false, false, false, false, false, false, false]</rosparam> -->
  </node>

  <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>

  <node pkg="twist_mux" type="twist_mux" name="twist_mux">
    <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
    <remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
  </node>

<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="-0.3 0 1.1 1.5 3.14 1.5 /front_bumper_link /realsense_frame 100" />
 <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="-0.3 0 0.6 0 0 0 /front_bumper_link /laser 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="0 0 0 0 0 0 /realsense_frame /camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_depth"
      args="0 0 0 0 0 0 /realsense_frame /camera_depth_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_imu"
      args="0 0 0 0 0 0 /imu_link /base_imu 100" />

</launch>

EDIT 2: (04 - 09 - 2016)

I can't see any variation in Z values and this is very strange! Can this be related to my XSENS MTi-10 sensor?

This is the output of rostopic echo /odometry ... (more)

edit retag flag offensive close merge delete

Comments

1

Is your odometry 6DoF ?

matlabbe gravatar imagematlabbe ( 2016-09-03 13:20:01 -0600 )edit

I edited my question to add the content of my configuration. It should be 6DoF since I'm using IMU Xsens Mti-10 and I set to "true": roll, pitch, yaw, pitch velocity, roll velocity, yaw velocity, X acceleration, Y acceleration, and Z acceleration.

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-03 14:16:29 -0600 )edit
1

Normally, you should see /odom frame in RVIZ moving in 6DoF if so. Show your TF frame with: $ rosrun tf tf_echo /odom /base_link and see if the z value changes.

matlabbe gravatar imagematlabbe ( 2016-09-03 18:31:46 -0600 )edit

I can see Z variations on my /imu/data topic, but no changes on Z axis on /odom topic. I edited my first post by adding output values from /odom and /imu/data. How can this happen? It seems that robot_localization is not computing Z axis information. Is it correct? Or the problem is another?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-04 04:13:07 -0600 )edit

It seems that it is considering a 2D motion.

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-04 04:21:13 -0600 )edit

I also found out that the 3d view shows the rows in wrong way since the row are parallels and not criss-crossed. May be the x-y-z IMU frame is not aligned with x-y-z robot frame? May be x-axis on IMU is aligned with the y-axis on the robot?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-04 07:14:33 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-09-04 07:58:55 -0600

matlabbe gravatar image

Hi,

Your IMU doesn't seem to compute orientation data, only angular velocity. For common wheel odometry (guessing husky_velocity_controller/odom), you would have x, y, theta position data and x, theta velocity data:

odom0_config: [true, true, false,
               false, false, true,
               true, false, false,
               false, false, true,
               false, false, false]
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              true, true, true,
              true, true, true] <-- could be (false, false, false) if acceleration adds too much drift

cheers

edit flag offensive delete link more

Comments

Thank you! But I already tried this configuration without any variation on Z axis. Do you think I should use another IMU sensor if I want to detect Z variations and visualize slopes in RVIZ?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-04 08:47:37 -0600 )edit

I did the test with the new configuration, but nothing changed.. still no Z-axis information. The output of /odom is still the same as before, it didn't changed. How can this happen?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-04 12:24:59 -0600 )edit
1

Did you try with imu0_differential: false? The IMU has pitch/roll information, there should be at least pitch/roll information in the filtered odometry (z would change also if there is pitch). Maybe the imu message is just not connected (or not appropriately) to robot_localization

matlabbe gravatar imagematlabbe ( 2016-09-04 18:58:55 -0600 )edit

I tried to set imu0_differential: false but nothing changed. Can it be a problem with the /tf between /odom and base_link?

Marcus Barnet gravatar imageMarcus Barnet ( 2016-09-05 05:32:44 -0600 )edit

TF /odom -> /base_link should be equal to /odometry/filtered, as in your example. But why Z, roll and ptich are not computed it is robot_localization related question. Make sure you don't have a "two_d_mode: true" hidden somewhere.

matlabbe gravatar imagematlabbe ( 2016-09-09 14:03:31 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-09-02 07:54:49 -0600

Seen: 362 times

Last updated: Sep 05 '16