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 diffdrivecontroller. 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 robotposeefk 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="base_frame" value="$(arg base_frame)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<param name="map_update_interval" value="5.0"/>
<param name="maxUrange" value="6.0"/>
<param name="maxRange" value="8.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="200"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>

<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>


<include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/>

<arg name="odom_frame_id"   default="odom"/>
<arg name="base_frame_id"   default="base_footprint"/>
<arg name="global_frame_id" default="map"/>
<arg name="odom_topic" default="/dummyRplidar_diff_drive_controller/odom" />
<arg name="laser_topic" default="scan" />
<arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>

<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find beginner_tutorials)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find beginner_tutorials)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find beginner_tutorials)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find beginner_tutorials)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find beginner_tutorials)/param/dwa_local_planner_params.yaml" command="load" />
<rosparam file="$(find beginner_tutorials)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find beginner_tutorials)/param/global_planner_params.yaml" command="load" />
<rosparam file="$(find beginner_tutorials)/param/navfn_global_planner_params.yaml" command="load" />
<!-- external params file that could be loaded into the move_base namespace -->
<rosparam file="$(arg custom_param_file)" command="load" />

<!-- reset frame_id parameters using user input data -->
<param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
<param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
<param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

<remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
<remap from="cmd_vel" to="/mobile_base/commands/velocity"/>
<remap from="cmd_vel" to="/dummyRplidar_diff_drive_controller/cmd_vel"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="scan" to="$(arg laser_topic)"/>
</node>

</launch>

And here's my urdf for my robot : https://github.com/erenaud3/myRobotUrdf/blob/master/dummySquareBase.urdf.xacro