RTAB Error

[tonike24]

Hi!

My hardware is:

Turtlebot 3 burguer (with LIDAR).
Camera Orbecc Astra.

I'm trying to use the compressed topics because with normal topics I don't have enough bandwidth for my Astra camera and LIDAR working both at the same time. So this is the changes I did in my launch:

 
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color/" />
  <arg name="depth_topic"             default=<b>"/camera/depth/image_raw/" />  Not sure If I have to use camera/depth_registered/image_raw because when I use it, my camera/depth_registered/image_raw/_relay dont publish any menssage. Instead with /camera/depth/image_raw my relay publish normally.
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />

<arg name="compressed"              default="<b>true"/>        
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="<b>compressed"/>  One question here, is what is the difference between compressed and compresseDepth, beucase when I use compressedDepth i have the next problem:

javier@javier-GP62MVR-6RF:~$ rosnode info /rtabmap/republish_depth
--------------------------------------------------------------------------------
Node [/rtabmap/republish_depth]
Publications:
 * /camera/depth_registered/image_raw/_relay [sensor_msgs/Image]
 * /rosout [rosgraph_msgs/Log]

Subscriptions:
 * /camera/depth_registered/image_raw/compressedDepth [unknown type]

So I change it for compress then republish_depth can recognize what kind of type message is. 
After that I excute Rtabmap.launch with image_raw topic, but it dont works.

javier@javier-GP62MVR-6RF:~$ rostopic hz /camera/depth/image_raw/_relay
subscribed to [/camera/depth/image_raw/_relay]
average rate: 25.706
        min: 0.033s max: 0.053s std dev: 0.00500s window: 23

javier@javier-GP62MVR-6RF:~$ rosnode info /rtabmap/republish_depth
--------------------------------------------------------------------------------
Node [/rtabmap/republish_depth]
Publications:
 * /camera/depth_registered/image_raw/_relay [sensor_msgs/Image]
 * /rosout [rosgraph_msgs/Log]
 * /rtabmap/republish_depth/compressed/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /rtabmap/republish_depth/compressed/parameter_updates [dynamic_reconfigure/Config]

Subscriptions:
 * /camera/depth_registered/image_raw/compressed [sensor_msgs/CompressedImage]



So, when I use image_raw the node rtabmap.launch crash instantly with this ERROR:

/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom,
   /camera/rgb/image_rect_color/_relay,
   /camera/depth/image_raw/_relay,
   /camera/rgb/camera_info,
   /scan
[ INFO] [1527076711.935895670]: rtabmap 0.16.3 started...
[ INFO] [1527076712.060599226]: Odom: ratio=0.725000, std dev=0.003350m|0.003350rad, update time=0.008793s
[ INFO] [1527076712.265009350]: Odom: ratio=0.725000, std dev=0.002894m|0.002894rad, update time=0.011834s
terminate called after throwing an instance of 'cv_bridge::Exception'
  what():  Image is wrongly formed: step < width * byte_depth * num_channels  or  640 != 640 * 2 * 1
[ INFO] [1527076712.458091214]: Odom: ratio=0.727778, std dev=0.002724m|0.002724rad, update time=0.005320s
[ INFO] [1527076712.481740432]: /rtabmap/rtabmapviz: queue_size    = 15
[ INFO] [1527076712.481775263]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1527076712.481792466]: /rtabmap/rtabmapviz: approx_sync   = true
[ INFO] [1527076712.481839429]: Setup depth callback
[ INFO] [1527076712.565229428]:
/rtabmap/rtabmapviz subscribed to (approx sync):
   /rtabmap/odom,
   /camera/rgb/image_rect_color/_relay,
   /camera/depth/image_raw/_relay,
   /camera/rgb/camera_info,
   /scan
[ INFO] [1527076712.565313444]: rtabmapviz started.
[rtabmap/rtabmap-4] process has died [pid 26845, exit code -6, cmd /home/javier/catkin_ws/devel/lib/rtabmap_ros/rtabmap rgb/image:=/camera/rgb/image_rect_color/_relay depth/image:=/camera/depth/image_raw/_relay rgb/camera_info:=/camera/rgb/camera_info rgbd_image:=rgbd_image left/image_rect:=/stereo_camera/left/image_rect_color_relay right/image_rect:=/stereo_camera/right/image_rect_relay left/camera_info:=/stereo_camera/left/camera_info right/camera_info:=/stereo_camera/right/camera_info scan:=/scan scan_cloud:=/scan_cloud user_data:=/user_data user_data_async:=/user_data_async odom:=odom __name:=rtabmap __log:=/home/javier/.ros/log/a2af4b1a-5e6e-11e8-9b05-4ccc6a827051/rtabmap-rtabmap-4.log].
log file: /home/javier/.ros/log/a2af4b1a-5e6e-11e8-9b05-4ccc6a827051/rtabmap-rtabmap-4*.log
[ INFO] [1527076712.658184339]: Odom: ratio=0.722222, std dev=0.002794m|0.002794rad, update time=0.004310s
[ INFO] [1527076712.910954583]: Odom: ratio=0.722222, std dev=0.002452m|0.002452rad, update time=0.056268s
terminate called after throwing an instance of 'cv_bridge::Exception'
  what():  Image is wrongly formed: step < width * byte_depth * num_channels  or  640 != 640 * 2 * 1
[ INFO] [1527076713.060416581]: Odom: ratio=0.719444, std dev=0.002319m|0.002319rad, update time=0.005567s
[rtabmap/rtabmapviz-5] process has died [pid 26846, exit code -6, cmd /home/javier/catkin_ws/devel/lib/rtabmap_ros/rtabmapviz -d ~/.ros/rtabmap_gui.ini rgb/image:=/camera/rgb/image_rect_color/_relay depth/image:=/camera/depth/image_raw/_relay rgb/camera_info:=/camera/rgb/camera_info rgbd_image:=rgbd_image left/image_rect:=/stereo_camera/left/image_rect_color_relay right/image_rect:=/stereo_camera/right/image_rect_relay left/camera_info:=/stereo_camera/left/camera_info right/camera_info:=/stereo_camera/right/camera_info scan:=/scan scan_cloud:=/scan_cloud odom:=odom __name:=rtabmapviz __log:=/home/javier/.ros/log/a2af4b1a-5e6e-11e8-9b05-4ccc6a827051/rtabmap-rtabmapviz-5.log].
log file: /home/javier/.ros/log/a2af4b1a-5e6e-11e8-9b05-4ccc6a827051/rtabmap-rtabmapviz-5*.log

If I use as topic for my depth /camera/depth_registered/image_raw I have the next problem:

javier@javier-GP62MVR-6RF:~$ rostopic hz /camera/depth_registered/image_raw/_relay
subscribed to [/camera/depth_registered/image_raw/_relay]
no message

Hope you can help me and give me some answers about what can I do to solve this problem.

Cheers, Javier.

[matlabbe]

Hi,

when subscribing to compressed topics, the name of the topic should be the normal ones (raw), but image_transport parameter should be set to compressed. For depth image, image_transport should be set to compressedDepth to avoid compression artifacts and that the depth image keeps the right format (16 or 32 bits). Make sure compressed_depth_image_transport plugin is installed on all computers.

When using rtabmap.launch, image_transport value is automatically set to compressed or compressedDepth when compressed argument is set to true (so you don't have to manage this yourself). Relays are also automatically created to make sure the image topics are subscribed only once by the remote computer.

On the computer running the camera, see if the default topics used by rtabmap are actually published:

$ rostopic hz /camera/rgb/image_rect_color
$ rostopic hz /camera/rgb/camera_info
$ rostopic hz /camera/depth_registration/image_raw

If "rostopic hz /camera/depth_registration/image_raw" doesn't publish anything, did you start the camera node with "depth_registration:=true"?

Example, launch on robot:

$ roslaunch astra_launch astra.launch depth_registration:=true

On remote computer, set the ROS_MASTER_URI to robot's IP, then launch this:

$ roslaunch rtabmap_ros rtabmap.launch compressed:=true

What do you see? If nothing, are there warnings/errors on the terminal?

cheers,
Mathieu

[tonike24]

Hi again.

I installed 'sudo apt-get install ros-kinetic-image-transport-plugins' in my TurtleBot3 which contain my camera Astra.

I'm have a lot of issues, lets explain:

FIRST : using topic camera/depth_registered/image_raw



As you can see here, we Z both topic working fine but I cant use them on RTAB because their Hz is so low, always I'm using a topic with sensor_msg/compressedDepth it reduces a lot my Hz in all my topics.



So I decide to use my topic sensor_msg/compressed intead of compressedDepth... but here continue my problem, something in my transports packages is wrong, but i installed my plugins.



SECOND: Nothing to do with topic camera/depth_registered/image_raw so I use now camera/depth/image_raw

 I changed my rtab.launch as you can see here. I'm using camera/depth/image_raw/compressed instead of compressed/Depth becasue it reduces a lot my Hz

 Look

So when i finally execute RTAB with this topics I have the next issue:




Hope you can help me, thank you.

[matlabbe]

Hi,

I can reproduce on Kinetic the very slow compressed depth topic (~2 Hz instead of 30 Hz), just by doing (even on same computer):

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ rostopic hz /camera/depth_registered/image_raw/compressedDepth
subscribed to [/camera/depth_registered/image_raw/compressedDepth]
no new messages
average rate: 2.363
	min: 0.405s max: 0.441s std dev: 0.01802s window: 3
average rate: 2.446
	min: 0.396s max: 0.441s std dev: 0.01651s window: 6

$ rostopic hz /camera/depth_registered/image_raw
subscribed to [/camera/depth_registered/image_raw]
average rate: 30.065
	min: 0.032s max: 0.035s std dev: 0.00118s window: 10
average rate: 29.968
	min: 0.031s max: 0.035s std dev: 0.00109s window: 40

There is maybe a bug in the compressedDepth plugin of image_transport causing its very low performance.

Just tested on Indigo, there is no such problem!

I updated rtabmap.launch for a workaround by using compressed rgbd_image topic instead. Example on the robot, launch:

# I use kinect
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ rosrun nodelet nodelet load rtabmap_ros/rgbd_sync camera/camera_nodelet_manager \
__name:=rgbd_sync \
camera/rgb/image:=/camera/rgb/image_rect_color \
camera/depth/image:=/camera/depth_registered/image_raw \
camera/rgb/camera_info:=/camera/rgb/camera_info

Verify on remote computer that compressed rgbd_image is received at 30 Hz:

$ rostopic hz /camera/rgbd_image/compressed
subscribed to [/camera/rgbd_image/compressed]
average rate: 29.869
	min: 0.030s max: 0.038s std dev: 0.00186s window: 28
average rate: 29.936
	min: 0.029s max: 0.039s std dev: 0.00229s window: 58

Launch rtabmap.launch with subscribing to compressed rgbd_image topic:

$ roslaunch rtabmap_ros rtabmap.launch subscribe_rgbd:=true rgbd_topic:=/camera/rgbd_image/compressed args:="-d"

cheers,
Mathieu

[tonike24]

Hi Mathieu,

It works perfect now! Thank you very much.

I have one question: How can I use my robot odometry instead of visual or ICP odometry?

Cheers,
Javier.

[matlabbe]

With rtabmap.launch:

$ roslaunch rtabmap_ros rtabmap.launch \
visual_odometry:=false \
odom_topic:=/odom \
subscribe_rgbd:=true \
rgbd_topic:=/camera/rgbd_image/compressed \
args:="-d" 

Adjust odom_topic to yours.

Cheers,
Mathieu

[tonike24]

Hi Mathieu,

Finally I can use RTAB without problems with my compressed topics, also my robot can localize itself. Everything is fine unless I want to navigate. So... I show you my problem:

This is my Tf_Tree, obviously is wrong:



So I decide to change my frame ID in my rtab.launch

  <arg name="frame_id"                default="<b>base_link"/>      I change my base_ink to Base_footprint. When I do this the TF tree is correct, like this:



But I have one problem... when my tf tree is correct and I execute my navigation.launch (Because my rtab.launch and navitation.launch are not in the same launch, I will copy my nav.launch later if u can see something wrong)

So I tried to change my robot base frame in costmaps to base_link instead of base_footprint.. Im misunderstanding this, could you explain it to me?.

[ WARN] [1528459815.208587375]: Costmap2DROS transform timeout. Current time: 1528459815.2085, global_pose stamp: 1528459811.5396, tolerance: 0.5000
[ WARN] [1528459815.208638059]: Could not get robot pose, cancelling reconfiguration


This is my nav.launch


<launch>

 <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>


 <arg name="scan_topic" default="/scan"/>
  <arg name="map_topic" default="/map"/>

   

   
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch.xml"/>

 
  <remap from="scan"             to="$(arg scan_topic)"/> 
  <remap from="map"              to="$(arg map_topic)"/>   

   <arg name="cmd_vel_topic" default="/cmd_vel" />
<arg name="odom_topic" default="odom" />
 
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
   

    <rosparam file="$(find rtabmap_ros)/launch/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rtabmap_ros)/launch/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rtabmap_ros)/launch/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rtabmap_ros)/launch/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rtabmap_ros)/launch/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find rtabmap_ros)/launch/param/dwa_local_planner_params.yaml" command="load" />

    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
         <remap from="odom" to="$(arg odom_topic)"/>
   
  </node>

   </launch>

[tonike24]

As you can see in this image when my frame ID in Rtab launch is base_link, my tf base_footprint (blue square in photo) is out of my robot. No sense. But if I change my frame ID in rtab.launch to base_footprint instead of base_link my tf base_footprint is correct in my robot but my lasers scan start failing. Like this:






Any suggest?

Thank you

[matlabbe]

Hi,

In your previous post, it seems you had another node publishing odom, like icp_doometry. Don't start icp_odometry. What is your rtab.launch file?

The frame_id should be base_footprint (the base frame of your robot).

For your last post, the problem is coming that two nodes were publishing odom.

cheers,
Mathieu

[tonike24]

Hi!

I solved the problem using my topic /odom published by my robot. I change it in my rtab.launch and everything works fine.

Thank you.