Re: Robot stuck in it own footprint

Posted by thanhnguyen on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Robot-stuck-in-it-own-footprint-tp3301p3420.html

Thank you, it work!!!
Now the robot can localization in the map use it odometry, not odometry from kinect any_more.

I have two question.

1/Question 1: when I sent the goal robot not move, topic of 2D Nav Goal is :/rtabmap/goal. And I check the rqt_graph, here the planner/move_base not have any connect with /cmd_vel, which that should be have if I use rgbd_odometry from kinect.



This is my launch files.

<launch>

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

  <!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->

  <include file="$(find volksbot_driver)launch/volksbot.launch"/>
  <include file="$(find lms1xx)launch/LMS1xx.launch"/>
  <include file="$(find freenect_launch)/launch/freenect.launch">
     <arg name="depth_registration" value="True" />
  </include>

  <node pkg="tf" type="static_transform_publisher" name="camera" 
   args="0 0 0 0 0 0 base_footprint camera_link 100" /> 
  <node pkg="tf" type="static_transform_publisher" name="laser" 
   args="0 0 0 0 0 0 base_footprint laser 100" />


  <!-- SLAM (robot side) -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"            type="string" value="base_footprint"/>
      <param name="subscribe_scan"      type="bool" value="true"/>
      <param name="use_action_for_goal" type="bool" value="true"/>
      <param name="cloud_decimation"    type="int" value="1"/>     <!-- we already decimate in memory below -->
      <param name="grid_eroded"         type="bool" value="true"/>
      <param name="grid_cell_size"      type="double" value="0.05"/>

      <remap from="odom"    to="/odom"/>
      <remap from="scan"    to="/scan"/>
      <remap from="mapData" to="mapData"/>

      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/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="RGBD/NeighborLinkRefining" type="string" value="true"/>           
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
  
      <param name="Reg/Strategy" type="string" value="1"/>                    
	 
      <param name="RGBD/AngularUpdate" type="string" value="0.1"/> 
      <param name="RGBD/LinearUpdate"  type="string" value="0.1"/> 
      <param name="RGBD/LocalRadius"   type="string" value="5"/>
	  
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
      <param name="Mem/NotLinkedNodesKept"  type="string" value="false"/>
      <param name="Mem/ImagePostDecimation"     type="string" value="4"/>
      
      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="false"/>
      <param name="Rtabmap/TimeThr"                  type="string" value="600"/>
      <param name="Rtabmap/DetectionRate"            type="string" value="1"/>

      <param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>

      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
      <param name="Optimizer/Strategy"        type="string" value="0"/>      
 
      <param name="Kp/DetectorStrategy"   type="string" value="0"/>
      <param name="Kp/MaxFeatures"        type="string" value="200"/>
      <param name="SURF/HessianThreshold" type="string" value="500"/>
	  
      <param name="Reg/Force3DoF"  type="string" value="true"/>
      <param name="Vis/MaxDepth"   type="string" value="5"/>
      <param name="Vis/MinInliers" type="string" value="5"/>
      <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>      

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  </group>
  
   <!-- 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>

  <!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="odom" to="/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"/>
     <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/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find rtabmap_ros)/launch/azimut3/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>

2/Question2: And the the "Map Cloud" which take info from /rtabmap/mapData and the /scan topic from laser still create separate two map. I think the different here is the Laser scanner and the kinect is not a part of the robot, I mount them on the robot and connect them directly to my Linux_Thinkpad.

Are you do the same for your azimut3 or your robot have the camera and laser scanner itself? And in my situation, how to fix this?