Robot stuck in it own footprint

[thanhnguyen]

Hi Mathieu No I will continue try to fix problems with the new Version, I just want to show to my profesor that I make something really run, so I need a good version With no error on a running laptop. I still keep a developing Laptop with me which always updated to the newest version and try to solve the problems of course. Cheers

[thanhnguyen]

Now I build opencv from source to do SIFT and SURF. But follow your instruction to download open cv 2.4 but I don't know why it downloaded version 3.0 and now it conflicted with libopencv 2.4 .core.so in rtabmap. I don't know how to solve this proplem so I reinstall everything 😩

[thanhnguyen]

Hi Mathieu,

1/The navigation with odometry from wheels encoder of my robot run wheel with rtabmap version 0.12.5, after I go back to this version everything work well. I think in your new version 0.13.1 have some code in your odometry.cpp not competitive with my volksbot_driver.

2/ Now I want to use some hector slam, so I try to run the file demo_hector_mapping in your demo, and I add the navigation code to make the robot move with move_base pakage. Below I show you the node graph. I think everything should be ok. But the robot not run when I send command from 2D Nav. I also go to azimut3 config and change files local_costmap_parameter like this:

global_frame: scanmatcher_frame
robot_base_frame: base_footprint
update_frequency: 2
publish_frequency: 1
rolling_window: true
width: 3.0
height: 3.0
resolution: 0.025
origin_x: 0
origin_y: 0

plugins:
  - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

also the same for local_costmap_params_2d.yaml.

Finally I got the map like this:



and node_graph like this


You see, I think everything look good, but I the robot no run. Do you see any thing wrong here?
Thank you in advance.

[matlabbe]

Hi,

I think TF /scanmatcher_frame -> /base_footprint is a fixed transform. Referring to Tf tree in this post, you would need to use this instead:

global_frame: hector_map
robot_base_frame: base_footprint

cheers,
Mathieu

[thanhnguyen]

Hi Mathieu,

Yes I tried it, but nothing change. Do you think problem is somewhere else?

This is my tf tree and graph"





and here the source code:

<launch>

  <!-- HECTOR MAPPING VERSION: use this with ROS bag demo_mapping_no_odom.bag generated  -->
  <!--                         from demo_mapping.bag with:                               -->
  <!-- rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"' -->
  <!-- WARNING : Database is automatically deleted on each startup                       -->
  <!--           See "delete_db_on_start" option below...                                -->
  
  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
 


<!-- Volksbot 3 bringup: launch motors/odometry -->
  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
  </include>
  <include file="$(find lms1xx)launch/LMS1xx.launch"/>
  <include file="$(find volksbot_driver)launch/volksbot.launch"/> 
  
  
  <node pkg="tf" type="static_transform_publisher" name="camera" 
   args="0 0 0 0 0 0 base_link camera_link 100" /> 
  
  <node pkg="tf" type="static_transform_publisher" name="laser" 
   args="0 0 0 0 0 0 base_link laser 100" /> 
  
  <!-- teleop -->
  <node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard" output="screen">
  <param name="scale_linear" value="0.5" type="double"/><param name="scale_angular" value="1.5" type="double"/>
  <remap from="turtlebot_teleop_keyboard/cmd_vel" to="cmd_vel"/>
  </node>



  <param name="use_sim_time" type="bool" value="false"/>
  
  <node pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />

  <!-- Odometry from laser scans -->
  <!-- We use Hector mapping to generate odometry for us -->
  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    
    <!-- Frame names -->
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_footprint" />
    <param name="odom_frame" value="odom" />
    
    <!-- Tf use -->
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="true"/>
    <param name="pub_odometry" value="true"/>
    
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="2048"/>
    <param name="map_multi_res_levels" value="2" />
    
    <!-- Map update parameters -->
    <param name="map_update_angle_thresh" value="0.06" />
    
    <!-- Advertising config --> 
    <param name="scan_topic" value="/scan"/>
  </node>

  <group ns="rtabmap">
    <!-- SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
	  <param name="frame_id" type="string" value="base_footprint"/>
	
	  <param name="subscribe_depth" type="bool" value="true"/>
	  <param name="subscribe_scan"  type="bool" value="true"/>
	  
          <!-- Inputs -->
	  <remap from="odom" to="/scanmatch_odom"/>
	  <remap from="scan" to="/scan"/>
          <remap from="rgb/image"       to="/camera/rgb/image_raw"/>
  	  <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
  	  <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
  	
	  <remap from="goal_out"  to="current_goal"/>	
          <remap from="move_base" to="/planner/move_base"/>
          <remap from="grid_map"  to="/rtabmap/grid_map"/>

	  <!-- RTAB-Map's parameters -->
	  <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
	  <param name="Vis/MaxDepth"       type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
	  <param name="Vis/InlierDistance" type="string" value="0.1"/>  <!-- 3D visual words correspondence distance -->
	  
	  <param name="Reg/Force3DoF"      type="string" value="true"/>
     
          
          <param name="Reg/Force3DoF"          type="string" value="true"/>
          <param name="RGBD/ProximityBySpace" type="string" value="true"/>
	  <param name="Icp/CorrespondenceRatio"  type="string" value="0.25"/> 
	  
          <param name="Grid/FromDepth" type="string" value="false"/>
          <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
    </node>
    
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
  	  <param name="subscribe_depth"     type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
    
      <remap from="rgb/image"       to="/camera/rgb/image_raw"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
      <remap from="scan"            to="/scan"/>
      <remap from="odom"            to="/scanmatch_odom"/>
      
      
    </node>
  
  </group>
  
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/camera/rgb/image_raw"/>
    <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
    <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
    <remap from="cloud"           to="voxel_cloud" />
    
    

    <param name="voxel_size" type="double" value="0.01"/>
  </node>


<!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="odom" to="/scanmatch_odom"/>
     <remap from="scan" to="/scan"/>
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/rtabmap/grid_map"/>
     <remap from="move_base_simple/goal" to="/planner_goal"/>
     <remap from="/planner/cmd_vel" to="/cmd_vel"/>
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/hector_config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/hector_config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/hector_config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/hector_config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/hector_config/base_local_planner_params.yaml" command="load" />
     </node>
   		
  </group>
  

    <!-- Throttling messages -->
  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
      <param name="rate" type="double" value="5"/>
      <param name="decimation" type="int" value="2"/>
   
      <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
    
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

    <!-- for the planner -->
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"            to="data_resized_image_depth"/>
      <remap from="depth/camera_info"      to="data_resized_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_footprint"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="Grid/MinClusterSize"     type="int" value="20"/>
      <param name="Grid/MaxObstacleHeight" type="double" value="0.4"/>
    </node>  
  </group>
</launch>

global_costmap.yaml

#global_costmap
global_frame: map
robot_base_frame: base_footprint
update_frequency: 1
publish_frequency: 1
always_send_full_costmap: false
plugins:
   - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
   - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

local_cosmap.yaml

global_frame: hector_map
robot_base_frame: base_footprint
update_frequency: 2.0
publish_frequency: 2.0
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.025
origin_x: -2.0
origin_y: -2.0

plugins:
  - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}

obstacle_layer:
  obstacle_range: 2.5
  raytrace_range: 3.0
  max_obstacle_height: 0.4
  track_unknown_space: true
 
  observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB

  laser_scan_sensor: {
    data_type: LaserScan, 
    topic: base_scan, 
    expected_update_rate: 0.2, 
    marking: true, 
    clearing: true
  }

  point_cloud_sensorA: {
    sensor_frame: base_footprint,
    data_type: PointCloud2, 
    topic: obstacles_cloud, 
    expected_update_rate: 0.5, 
    marking: true, 
    clearing: true,
    min_obstacle_height: 0.04
  }

  point_cloud_sensorB: {
    sensor_frame: base_footprint,
    data_type: PointCloud2,
    topic: ground_cloud,
    expected_update_rate: 0.5,
    marking: false,
    clearing: true,
    min_obstacle_height: -1.0 # make usre the ground is not filtered
  }

Thanks in advance

[matlabbe]

Look at the terminal if at least move_base is receiving the goal, then if so, if it seems to plan something. Then check also output cmd_vel to see if there is something, like if commands are published and always 0s, maybe move_base thinks it is on an obstacle. Show local and global costmap to debug in RVIZ.

[thanhnguyen]

Sent from my iPhone

[thanhnguyen]

Hi,
I just left school now so I can't give you pictur
But yes, move base receiving the goal, and I can
Display the global and local costmap also. On
Rviz I can also see the  Path planning of robot
It just not moving.
Cheers,

[matlabbe]

Is the cmd_vel from move_base published? If so, are they null or not? If they are not null, make sure the topic is connected to your robot controller and that the controller is actually working.

[thanhnguyen]

Hi Mathieu,

The cmd_vel from move_base published already. I can control the robot with teleop. But with 2D nav robot not move. Like the picture below.

[thanhnguyen]

And one more thing mathieu. this is a warning about loop closure is fall but it affect to program alot. After i run robot for a while. This error exits:

93.708955381]: Resizing costmap to 457 X 288 at 0.050000 m/pix
                                                              [ WARN] (2017-07-29 09:28:14.716) Rtabmap.cpp:1835::process() Rejected loop closure 1 -> 78: Not enough inliers 0/5 (matches=4) between 1 and 78
[ INFO] [1501313294.746786921]: rtabmap (79): Rate=1.00s, Limit=0.600s, RTAB-Map=0.0944s, Maps update=0.0208s pub=0.0015s (local map=46, WM=46)
                                                               [ WARN] (2017-07-29 09:28:15.660) util3d_registration.cpp:184::transformFromXYZCorrespondences() RANSAC refineModel: Refinement failed: got an empty set of inliers!
[ WARN] (2017-07-29 09:28:15.660) Rtabmap.cpp:1835::process() Rejected loop closure 1 -> 78: Not enough inliers 0/5 (matches=6) between 1 and 78
[ INFO] [1501313295.676153376]: rtabmap (80): Rate=1.00s, Limit=0.600s, RTAB-Map=0.0597s, Maps update=0.0094s pub=0.0032s (local map=46, WM=46)
                                                               ^C[ WARN] (2017-07-29 09:28:16.706) Rtabmap.cpp:1835::process() Rejected loop closure 1 -> 78: Not enough inliers 0/5 (matches=4) between 1 and 78
[ INFO] [1501313296.715249552]: rtabmap (81): Rate=1.00s, Limit=0.600s, RTAB-Map=0.0697s, Maps update=0.0055s pub=0.0012s (local map=46, WM=46)

And my map overlap like this. This happen when I make robot turn left or right, I think the camera canot follow speed of robot.




Thanks in advance

[matlabbe]

Hi,

For move_base not moving, it is difficult for me to debug without the robot. Try to scale down the complexity of the launch file by enabling/disabling stuff and see it can help.

For the wrong loop closure, did you save the database somewhere to share it?

cheers,
Mathieu

[thanhnguyen]

Hi Mathieu

This is the data of wrong loop closure, when I make robot run about 3 minutes and the map become larger, the wrong loop closure exit?


Here is the database files of the wrong map:

rtabmap.db
I want ask you some a about theory:

1/ RTAB-Map is graph-base SLAM?

2/ Is the kinect important in when robot autonomous navigate or only the laser scanner is enough, because I see hector SLAM only use laser?


3/Navigation work with any rtabmap or hector, right? . Because it only need a static map to run in it.

4/ When I run demo_hector_mapping, I have some confuse in scanmatch_dom, how can from laser scan we got the odometry?

In this picture I display the scanmatch_odometry. Can you help me to explain this?


Thanks in advance,

[matlabbe]

Hi,

The database is empty. For your questions:
1) yes
2) yes, loop closure detection is appearance-based (bags-of-words)
3) yes
4) in that demo, we use hector_slam like a lidar-based odometry approach (e.g., the icp_odometry node on this example), then we redirect the correct topics to rtabmap odometry input.


cheers,
Mathieu

[thanhnguyen]

Hi Mathieu,

Thank you very much, I want to ask you when I try to run demo_turtlebot_in simulation:

I can't connect with the kinect in simulation:

[ INFO] [1502232684.504891411, 676.780000000]: No devices connected.... waiting for devices to be connected
[ WARN] [1502232685.779057872, 678.060000000]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info
[ INFO] [1502232687.505097373, 679.770000000]: No devices connected.... waiting for devices to be connected
[ WARN] [1502232688.106317259, 680.380000000]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmap subscribed to (approx sync):
   /camera/rgb/image_rect_color,
   /camera/depth_registered/image_raw,
   /camera/rgb/camera_info,
   /scan

However, when I type rostopic hz /camera/rgb/image_rect_color, they have in formation, only /scan not have. But rtabmap_ros just tell me that it can't connect with the kinnect in simulation. Here is my graph and tf tree.




Cheers,

[thanhnguyen]

Hi Mathieu,

A question about theory:

 For RTAB-map is the real algorithms used for SLAM and navigation there must be a map where areas with and without obstacles are labelled that robot can navigate, I mean the 2D map, in RTAB map how you got 2D map from 3D point cloud? And what is the mathematical model of this map, are you used grid map? And In rtabmap you need a laser scanner or a fake laser from kinnect to create map, righ?
Cheers,

[matlabbe]

The error msg "[ INFO] [1502232684.504891411, 676.780000000]: No devices connected.... waiting for devices to be connected" is not from rtabmap. It could be from the camera node. If you are in simulation, you don't have to start openni/freenect driver as gazebo (with its camera plugin) will give already the camera messages. Remap the topics if necessary (gazebo and real camera drivers may not have the same topic names).

[matlabbe]

Hi,

Depending if "Grid/FromDepth" is false or not, the grid map will be created from laser scans (2d ray tracing) or point cloud from the RGBD camera (obstacles and ground are segmented from the 3D cloud then projected on the ground to fill the 2d grid) respectively. See nav_msgs/OccupancyGrid for the signification of the cell values.

cheers,
Mathieu

[thanhnguyen]

Hi Mathieu,

I want to combine the visual odometry from  Kinect camera(and/or laser scanner) and wheel odometry from my robot, do you know how to do that, I tried to used your sensor fusion_test but this file you fusion the IMU and Kinect.

Cheers,

[matlabbe]

For fusion of multiple odometry sources, I would refer you to robot_localization documentation:
http://docs.ros.org/lunar/api/robot_localization/html/index.html

When adding a laser, we can use it to correct odometry on rtabmap side. See this example. Parameters RGBD/NeighborLinkRefining=true and Reg/Strategy=1 make rtabmap refining with ICP the odometry transform between two nodes added to map.

cheers,
Mathieu