0
$\begingroup$

Rosanswers logo

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 http://www.dronyx.com/bags/husky/flag_flat.png

A more closer view:

image description http://www.dronyx.com/bags/husky/flat.png

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/filtered:

header: 
  seq: 4489
  stamp: 
    secs: 1472577599
    nsecs: 495876210
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 7.91059387636
      y: 5.49385587371
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.708492810159
      w: 0.705718030061
  covariance: [159.3346334741183, -144.12117639439816, 0.0, 0.0, 0.0, -28.938346143883855, -144.1211763943982, 216.90782859592676, 0.0, 0.0, 0.0, 26.05154084687186, 0.0, 0.0, 815187.6751831494, -38255.26094736863, -45460.481665947074, 0.0, 0.0, 0.0, -38255.260947368624, 4351.24016726083, 1.216019472616292e-13, 0.0, 0.0, 0.0, -45460.48166594722, -1.3888890038060708e-13, 4351.24016726083, 0.0, -28.938346143883916, 26.051540846871838, 0.0, 0.0, 0.0, 5.657229019245677]
twist: 
  twist: 
    linear: 
      x: 0.161577660792
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.000382893569291
  covariance: [0.0029562914310124846, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0029562914310124846, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004274702386211555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8977678428159488, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8977678428159488, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.008380430069515983]

and this is the output of $ rosrun tf tf_echo /odom /base_link :

At time 1472577567.076
- Translation: [5.883, 0.249, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.019, 1.000]
            in RPY (radian) [0.000, -0.000, 0.039]
            in RPY (degree) [0.000, -0.000, 2.232]
At time 1472577568.076
- Translation: [6.390, 0.269, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.019, 1.000]
            in RPY (radian) [0.000, -0.000, 0.039]
            in RPY (degree) [0.000, -0.000, 2.225]

This is the output of my rostopic echo /imu/data:

header: 
  seq: 3287
  stamp: 
    secs: 1472577575
    nsecs: 139333009
  frame_id: /base_imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: 0.131834924221
  y: -0.0338868610561
  z: 0.11178239435
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 1.2274068594
  y: -1.21246373653
  z: 9.32702445984
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---
header: 
  seq: 3288
  stamp: 
    secs: 1472577575
    nsecs: 159409046
  frame_id: /base_imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: 0.148351207376
  y: -0.0273859389126
  z: 0.0958278700709
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 1.30915474892
  y: -1.13592338562
  z: 8.60862922668
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---
header: 
  seq: 3289
  stamp: 
    secs: 1472577575
    nsecs: 179352045
  frame_id: /base_imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: 0.167083263397
  y: -0.0225309766829
  z: 0.0947325453162
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 1.27741312981
  y: -1.02555584908
  z: 8.2296962738
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---
header: 
  seq: 3290
  stamp: 
    secs: 1472577575
    nsecs: 199362993
  frame_id: /base_imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: 0.204487532377
  y: 0.00747932633385
  z: 0.117759332061
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 1.18366909027
  y: -1.17894041538
  z: 8.64842414856
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---
header: 
  seq: 3291
  stamp: 
    secs: 1472577575
    nsecs: 219384908
  frame_id: /base_imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
angular_velocity: 
  x: 0.250565439463
  y: 0.0498796999454
  z: 0.120644703507
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 1.15033447742
  y: -1.32312035561
  z: 9.01342487335
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]
---

Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2016-09-02

Post score: 0


Original comments

Comment by matlabbe on 2016-09-03:
Is your odometry 6DoF ?

Comment by Marcus Barnet on 2016-09-03:
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.

Comment by matlabbe on 2016-09-03:
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.

Comment by Marcus Barnet on 2016-09-04:
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?

Comment by Marcus Barnet on 2016-09-04:
It seems that it is considering a 2D motion.

Comment by Marcus Barnet on 2016-09-04:
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?

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by matlabbe with karma: 6409 on 2016-09-04

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Marcus Barnet on 2016-09-04:
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?

Comment by Marcus Barnet on 2016-09-04:
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?

Comment by matlabbe on 2016-09-04:
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

Comment by Marcus Barnet on 2016-09-05:
I tried to set imu0_differential: false but nothing changed. Can it be a problem with the /tf between /odom and base_link?

Comment by matlabbe on 2016-09-09:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.