Error when run rtabmap_ros on Nao robot

[Robin-Wu]

I am a newer in Ros and Rtabmap_ros ,and  I want to run the Rtabmap_ros  with xbox 360 on NAO robot ,so  I make a  simple  launch file but i have some  questions:
(1)the 3d map is update but my Robot is not move

(2)the map is not  on horizon  

I think is the problem of tf between /base_link and /camera_link  ,but I have no idea to solve it
and my simple launch file is :
 

<launch>
 
  <node pkg="tf" type="static_transform_publisher" name="kinect_to_base_link"
       args="$0 0 0.2 0 0 0  /base_link /camera_link 100" > 
  </node>

 
  <include file="$(find openni_launch)/launch/openni.launch">
    <arg name="depth_registration" value="true"/>
  </include>
 
 
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="rtabmap_args"      value="--delete_db_on_start"/>
    <arg name="frame_id"          value="/base_link"/>
    <arg name="rtabmapviz"        value="true"/>
   
  </include>
 
</launch>

[matlabbe]

Hi,

What package are you using for Nao navigation (to make it move)? Can you point the robot toward a clear space? It seems to have obstacles right in front of it.

cheers,
Mathieu

[Robin-Wu]

Thank you for your so quickly reply!
I use the api from it to move it (not the Ros package ,because I can't find a Suitable package to make it move stably)
now I  point it toward a clear space.

[matlabbe]

For the navigation, I cannot help as I don't know the API, try on the website of the API. For mapping, a grid is created, so it seems okay. If you want to fill empty space between obstacles and the robot in the field of view of the camera, you could enable ray tracing by adding the Grid/RayTracing parameter:

<arg name="rtabmap_args"      value="--delete_db_on_start --Grid/RayTracing true"/>

To save some computation power, if you don't need a 3d map, you can also set Grid/3D to false.

cheers,
Mathieu

[Robin-Wu]

HI,
I'm so sorry for my late, and  thank you for your reply.
I fixed my problem  ,thank you!
I will try your suggestions

[Robin-Wu]

Hi,
Thank you for all your assistance.
This is my launch file:

 <launch>
 
  <node pkg="tf" type="static_transform_publisher" name="kinect_to_base_link"
       args="$0 0 0.2 0 0 0  /base_link /camera_link 100" > 
  </node>

 
  <include file="$(find openni_launch)/launch/openni.launch">
    <arg name="depth_registration" value="true"/>
  </include>
 
 
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="rtabmap_args"      value="--delete_db_on_start"/>
    <arg name="frame_id"          value="/base_link"/>
    <arg name="rtabmapviz"        value="true"/>
    <arg name="visual_odometry"   value="false"/>
    <arg name="odom_topic"        value="/odom"/>
   
   
   
  </include>
 
</launch>

from this file ,my odom_topic is /odom,and the graph is better than before,but  the map will swaying from side to side  and move when my robot is moveing(perhaps is because my robot's odom is sway when moving)




So ,Can you give me some suggestions?
Yours
Sincerely

 

[matlabbe]

Is your odometry in 6DoF or 3DoF? If it is in 3DoF, you have to update the TF between base_link and the camera link to inhibit the robot rotation while walking.

[Robin-Wu]

Hi,Thank you for your reply.
I have  update the TF between base_link and the camera link  as follows and when the robot is standing ,the graph is in front of it .
 <node pkg="tf" type="static_transform_publisher" name="kinect_to_base_link"
       args="$0 0 0.2 0 0 0  /base_link /camera_link 100" > 
  </node>
as for the DoF of  odometry ,I am not sure  the DoF of my robot's odometry (perhaps is 6Dof,because when it is walking ,the odometry in rviz will  swaying from side to side and is Pointing straight ahead while is standing。
Yours.
Robin

[matlabbe]

You can do

$ rostopic echo "odom_topic"

to see if z/pitch/roll values are changing (if so it is 6dof).

[Robin-Wu]

Thank you!
and when It shows that the parameter “w" is  changing   like this

[matlabbe]

Can you share a database of the robot walking? (rtabmap database is automatically saved to ~/.ros/rtabmap.db when closing the node)

[Robin-Wu]

I'm so sorry for my late ,and my rtabmap.db is rtabmap.db

[matlabbe]

Hi,

I ran again the db with odom (on left) and with recomputing visual odometry (VO, on right), here are the differences:



Odom poses:

VERTEX_SE3:QUAT 1 -0.014821 -0.042794 0.328887 0.019437 -0.001512 0.010566 0.999754
VERTEX_SE3:QUAT 4 0.017673 0.014843 0.321162 0.007263 0.083460 -0.067739 0.994180
VERTEX_SE3:QUAT 5 0.004120 -0.034643 0.312733 0.003894 -0.016783 0.077755 0.996824
VERTEX_SE3:QUAT 6 0.104585 -0.018804 0.313240 0.018442 0.007071 0.077293 0.996813
VERTEX_SE3:QUAT 8 0.207153 0.014679 0.313886 0.001545 -0.010961 0.076299 0.997024
VERTEX_SE3:QUAT 9 0.218968 0.024380 0.314038 -0.002430 0.006558 0.241268 0.970433
VERTEX_SE3:QUAT 10 0.205878 0.034067 0.314157 -0.002371 0.002109 0.320750 0.947159
VERTEX_SE3:QUAT 13 0.203086 0.036657 0.314035 -0.007530 0.007364 0.469115 0.883074
VERTEX_SE3:QUAT 14 0.194632 0.035410 0.314101 0.000690 0.004492 0.543298 0.839528

VO poses:

VERTEX_SE3:QUAT 1 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 4 0.047466 0.003567 -0.009966 -0.013537 0.112741 -0.011342 0.993467
VERTEX_SE3:QUAT 5 0.015019 -0.022147 -0.015365 0.002945 -0.020956 -0.055522 0.998233
VERTEX_SE3:QUAT 6 0.120729 -0.028245 -0.022842 0.013800 0.027280 -0.011168 0.999470
VERTEX_SE3:QUAT 8 0.225036 -0.011868 -0.000275 0.006574 -0.027281 0.013149 0.999520
VERTEX_SE3:QUAT 9 0.230639 -0.000577 -0.011600 -0.014138 -0.001122 0.165710 0.986073
VERTEX_SE3:QUAT 10 0.218711 0.001877 -0.009298 -0.007840 0.004223 0.250492 0.968078
VERTEX_SE3:QUAT 13 0.224081 0.008963 -0.015470 -0.038566 -0.012594 0.386984 0.921194
VERTEX_SE3:QUAT 14 0.220637 -0.014765 -0.006043 -0.016738 0.025084 0.483956 0.874573

As the cloud map with VO seems correct, the error would be coming from Nao's odometry. You may want to use VO instead and make sure the robot is navigating always in the environment with visual features in sight.

cheers,
Mathieu

[Robin-Wu]

OK，thank you ！
I will try it again.
Yours
Robin

[Robin-Wu]

Hi!,
I have another questions that if  I set the visual_odometry false ,the graph in rviz is ok  like this

but  when i running the costmap ,it shows that Timed out waiting for transform from base_link to rtabmap/grid_m ap to become available before running costmap.
but when i set it to true  ,it will be so many the same things in my rviz and the robot model is wrong

Yours
Robin

[matlabbe]

Can you show TF tree when visual_odometry:=true?

$ rosrun tf view_frames
$ evince frames.pdf

 
When visual_odometry is true, make sure you don't publish at the same time odom TF from the robot.

cheers,
Mathieu

[Robin-Wu]

This is my frames.pdf
Yours,
Robin

[Robin-Wu]

Oh！  I think It's the problem of the tf between my kinect and my robot.
I have already set the  tf  transform like this  
  <node pkg="tf" type="static_transform_publisher" name="kinect_to_base_link"
       args="$0 0 0.2 0 0 0  /base_link /camera_link 100" /> 
but when I  launch it ,the kinect on my robot in rviz will move Frequently,like the pictures:


so  how can i resolve it .
Thanks
Yours,
Robin

[matlabbe]

Looking at your frames.pdf. /base_footprint should be the base frame of the robot (parent of base_link), so that you have /map -> /odom -> /base_footprint -> /base_link -> camera_link

cheers,
Mathieu

[Robin-Wu]

and  my launch file is like this:
 <launch>

<include file="$(find nao_bringup)/launch/nao_full_py.launch">
<arg name="nao_ip" default="192.168.1.109"/>
</include>




<node pkg="rviz" type="rviz" name="rviz" args="-d /home/robin/rviz.rviz"/>

 

 
  <include file="$(find openni_launch)/launch/openni.launch">
    <arg name="depth_registration" value="true"/>
  </include>

  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
          <arg name="rtabmap_args"      value="--delete_db_on_start"/>
         
         
         
         
         
         
         
           
         

         
         
         
         
  </include>
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera"
args="0 0 0.2 0 0 0 /base_link /camera_link 100"/>
  </launch>