Gmapping doesn't notice robot is stuck on Gazebo

asked 2018-06-01 11:02:32 -0600

erenaud gravatar image

Hello !

I am trying to do slam with gmapping on a custom robot simulated on Gazebo. My robot has a squared base, two wheel at the front, one caster at the back, a laser scan(similar to RpLidar) and an IMU. Odom is get thank to diff_drive_controller. At first, I noticed that my odom was awful. For example, when rotating the robot, the laser scan was totally wrong and didn't match the walls previoulsy seen. So I used robot_pose_efk to fuse IMU and odom data.

Now, the results are very satisfaying : the new scans match up the old ones when rotating the robot and I am able to perform SLAM on a "complexe" world. Troubles come when my robot get stucks somewhere. For example, if I use teleop to make the robot moving against the wall, rviz doesn't see that at all. It will act as the robot is continuing its path normally, though it is completely stuck on gazebo. As a result, my map become totally wrong.

My question is : how can I avoid this ? I thought fusing IMU and odom were going to be sufficient, but apparently I was wrong. So what then ? Adding bumpers or other sensors to detect a collision with a wall ? Can I achieve that with my already implemented laser scan ?

Here's my lauch file to start simulation :

<launch>
<arg name="model" default="$(find test_urdf_from_sw)/urdf/dummySquareBase.urdf.xacro"/>
<arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" />
<include file="$(find hector_gazebo_worlds)/launch/small_indoor_scenario.launch"/>
<arg name="gui" default="true" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>

<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
    args="-z 0.2 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />
<rosparam command="load" file="$(find test_urdf_from_sw)/config/jointsDummyRplidar.yaml" ns="dummyRplidar_joint_state_controller" />
<rosparam command="load" file="$(find test_urdf_from_sw)/config/diffDriveDummyRplidar.yaml" ns="dummyRplidar_diff_drive_controller" />
<node name="dummyRplidar_controller_spawner" pkg="controller_manager" type="spawner" args="dummyRplidar_joint_state_controller dummyRplidar_diff_drive_controller --shutdown-timeout 3">   
</node>

<node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
<param name="default_topic" value="/dummyRplidar_diff_drive_controller/cmd_vel"/>
</node>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="output_frame" value="odom"/>
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<param name="debug" value="false"/>
<param name="self_diagnose" value="false"/>

<remap from="odom" to="/dummyRplidar_diff_drive_controller/odom"/>
<remap from="imu_data" to="/imu"/>
</node>

<arg name="use_tf_static" default="false"/>
<node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
  <param name="publish_frequency" type="double" value="30.0" />
  <param name="use_tf_static" value="$(arg use_tf_static)"/>
</node>

</launch>

Here's my launch file to start gmapping:

<launch>
<!-- 3D sensor -->
<arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/>  <!-- r200, kinect, asus_xtion_pro -->

<arg name="scan_topic"  default="scan" />
<arg name="base_frame"  default="base_footprint"/>
<arg name="odom_frame"  default="odom"/>

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name ...
(more)
edit retag flag offensive close merge delete

Comments

Do you ever heard about navigation recovery behavior?

achmad_fathoni gravatar imageachmad_fathoni ( 2018-06-02 23:02:25 -0600 )edit

I don't quite see what recovery behaviours have to do with my problem. It happens even with teleop. In deed, my robot won't even start recovery behaviours since it won't notice it is stuck.

erenaud gravatar imageerenaud ( 2018-06-04 03:57:20 -0600 )edit