Detect terrain slope with filtered odometry and RTAB-map (IMU, LMS111, Husky, R200)
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).
A more closer view:
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 ...
Is your odometry 6DoF ?
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
.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.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?It seems that it is considering a 2D motion.
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?