Official RTAB-Map Forum

Re: Raytracing using stereo camera for better SLAM and obstacle avoidance.

Posted by karleno on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Raytracing-using-stereo-camera-for-better-SLAM-and-obstacle-avoidance-tp5632p5735.html

Okay, once again thank you a lot for the assistance. I managed to fix the librealsense2.so.2 problem by exporting its path with LD_LIBRARY_PATH as you said. Regarding the typo I mentioned earlier, what I meant was in the topics you declared in the remote launch file. You wrote: 
        <arg name="rgb_topic" value="/camera/rgbd_image"/>

and my question was whether it should be rgb_topic that is set, or if it really should be rgbd_topic, since the value that is related to it is /camera/rgbd_image. I just found it a bit confusing...

Now, when I run this launch file on board the robot: 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
<launch>

  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
      <arg name="align_depth" value="True"/>
      <arg name="linear_accel_cov" value="1.0"/>
      <arg name="unite_imu_method" value="linear_interpolation"/>
    	<arg name="color_width"         value="424"/>
    	<arg name="color_height"        value="240"/>
    	<arg name="infra_width"        value="424"/>
    	<arg name="infra_height"       value="240"/>
    	<arg name="depth_width"         value="424"/>
    	<arg name="depth_height"        value="240"/>
      <arg name="fisheye_width"       value="424"/>
   	  <arg name="fisheye_height"      value="240"/>

  	  <arg name="fisheye_fps"  value="30"/>
      <arg name="depth_fps"    value="30"/>
      <arg name="infra_fps"    value="30"/>
      <arg name="color_fps"    value="30"/>

  </include>

  <arg name="rate"  default="5"/>
  <arg name="approx_sync" default="false" />

  <group ns="camera">
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
      <param name="compressed_rate"  type="double" value="$(arg rate)"/>
      <param name="approx_sync"      type="bool"   value="$(arg approx_sync)"/>

      <remap from="rgb/image"       to="color/image_raw"/>
      <remap from="depth/image"     to="depth/image_rect_raw"/>
      <remap from="rgb/camera_info" to="color/camera_info"/>

      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->
    </node>
  </group>



  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">
      <param name="_use_ma" type="bool" value="false" />
      <param name="_publish_tf" type="bool" value="false" />
      <param name="_world_frame" type="string" value="enu" />
      <remap from="/imu/data_raw" to="/camera/imu"/>
  </node>

    <!--
    <node name="$(anon dynparam)" pkg="dynamic_reconfigure" type="dynparam"   args="set_from_parameters /camera/RGB_Camera">
      <param name="Enable_Auto_Exposure"    type="bool" value="false" />
      <param name="Exposure"                type="int"  value="30" />
      <param name="Auto_Exposure_Priority"  type="bool" value="true"/>
      <param name="Frames_Queue_Size"       type="int"  value="50" />
    </node>
  -->


    <include file="$(find robot_localization)/launch/ukf_template.launch"/>
    <param name="/ukf_se/frequency" value="300"/>
    <param name="/ukf_se/base_link_frame" value="camera_link"/>
    <param name="/ukf_se/odom0" value="rtabmap/odom"/>
    <rosparam param="/ukf_se/odom0_config">[true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true,
                                            true,true,true]
    </rosparam>
    <param name="/ukf_se/odom0_relative" value="true"/>
    <param name="/ukf_se/odom0_pose_rejection_threshold" value="10000000"/>
    <param name="/ukf_se/odom0_twist_rejection_threshold" value="10000000"/>

    <param name="/ukf_se/imu0" value="/imu/data"/>
    <rosparam param="/ukf_se/imu0_config">[false, false, false,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true,
                                           true,  true,  true]
    </rosparam>
    <param name="/ukf_se/imu0_differential" value="true"/>
    <param name="/ukf_se/imu0_relative" value="false"/>
    <param name="/ukf_se/use_control" value="false"/>
    <!-- <param name="/ukf_se/odom0_config" value="{true,true,true,}"/> -->


</launch>


and this launch file on the remote computer: 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
<launch>
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
        <arg name="compressed" value="true"/>
        <arg name="args" value="--delete_db_on_start"/>

      <!--  <arg name="stereo" value="false"/> -->

        <arg name="subscribe_rgbd" value="true"/>
        <arg name="rgbd_topic" value="/camera/rgbd_image"/> <!-- rgb changed to rgbd -->
        <arg name="rtabmapviz" value="false"/>
        <arg name="rviz" value="true"/>

        <arg name="odom_topic" value="/odometry/filtered"/>
        <arg name="visual_odometry" value="false"/>

        <arg name="queue_size" value="1000"/>

        <arg name="approx_sync" value="false"/> <!-- test -->
    </include>
</launch>


I get the following output from the remote launch file: 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
started roslaunch server http://10.90.141.197:42241/

SUMMARY
========

PARAMETERS
 * /points_xyzrgb/approx_sync: False
 * /points_xyzrgb/decimation: 4.0
 * /points_xyzrgb/voxel_size: 0.0
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /rtabmap/republish_rgbd_image/uncompress: True
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/approx_sync: False
 * /rtabmap/rtabmap/config_path:
 * /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
 * /rtabmap/rtabmap/frame_id: camera_link
 * /rtabmap/rtabmap/ground_truth_base_frame_id:
 * /rtabmap/rtabmap/ground_truth_frame_id:
 * /rtabmap/rtabmap/landmark_angular_variance: 9999.0
 * /rtabmap/rtabmap/landmark_linear_variance: 0.0001
 * /rtabmap/rtabmap/map_frame_id: map
 * /rtabmap/rtabmap/odom_frame_id:
 * /rtabmap/rtabmap/odom_sensor_sync: False
 * /rtabmap/rtabmap/odom_tf_angular_variance: 1.0
 * /rtabmap/rtabmap/odom_tf_linear_variance: 1.0
 * /rtabmap/rtabmap/publish_tf: True
 * /rtabmap/rtabmap/queue_size: 1000
 * /rtabmap/rtabmap/scan_normal_k: 0
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_rgbd: True
 * /rtabmap/rtabmap/subscribe_scan: False
 * /rtabmap/rtabmap/subscribe_scan_cloud: False
 * /rtabmap/rtabmap/subscribe_stereo: False
 * /rtabmap/rtabmap/subscribe_user_data: False
 * /rtabmap/rtabmap/wait_for_transform_duration: 0.2

NODES
  /rtabmap/
    republish_rgbd_image (rtabmap_ros/rgbd_relay)
    rtabmap (rtabmap_ros/rtabmap)
  /
    points_xyzrgb (nodelet/nodelet)
    rviz (rviz/rviz)

ROS_MASTER_URI=http://aaeon-up.local:11311/

process[rtabmap/republish_rgbd_image-1]: started with pid [10279]
process[rtabmap/rtabmap-2]: started with pid [10280]
process[rviz-3]: started with pid [10281]
process[points_xyzrgb-4]: started with pid [10282]
type is rtabmap_ros/point_cloud_xyzrgb
[ INFO] [1554993246.936455236]: Starting node...
[ INFO] [1554993247.064551895]: Initializing nodelet with 4 worker threads.
[ INFO] [1554993247.773640784]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1554993247.773706052]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1554993247.773735824]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1554993247.773759218]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1554993247.773780474]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1554993247.773798463]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1554993247.773816131]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1554993247.773836110]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1554993247.792051139]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1554993248.064722506]: Approximate time sync = false
[ INFO] [1554993248.936549633]: rtabmap: frame_id      = camera_link
[ INFO] [1554993248.936606636]: rtabmap: map_frame_id  = map
[ INFO] [1554993248.936649969]: rtabmap: use_action_for_goal  = false
[ INFO] [1554993248.936691488]: rtabmap: tf_delay      = 0.050000
[ INFO] [1554993248.936717004]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1554993248.936740996]: rtabmap: odom_sensor_sync   = false
[ INFO] [1554993256.334602678]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1554993256.344516490]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1554993276.796578777]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1554993276.797298050]: rtabmap: Deleted database "/home/karl/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1554993276.797913144]: rtabmap: Using database from "/home/karl/.ros/rtabmap.db" (0 MB).
[ INFO] [1554993276.964858652]: rtabmap: Database version = "0.19.1".
[ WARN] [1554993277.577076717]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.
[ INFO] [1554993277.685600572]: /rtabmap/rtabmap: queue_size    = 1000
[ INFO] [1554993277.685699014]: /rtabmap/rtabmap: rgbd_cameras = 1
[ INFO] [1554993277.685778739]: /rtabmap/rtabmap: approx_sync   = false
[ INFO] [1554993277.685927546]: Setup rgbd callback
[ INFO] [1554993277.845020333]:
/rtabmap/rtabmap subscribed to (exact sync):
   /odometry/filtered,
   /camera/rgbd_image_relay
[ INFO] [1554993278.056668174]: rtabmap 0.19.1 started...
[ WARN] [1554993282.845268805]: /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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.
/rtabmap/rtabmap subscribed to (exact sync):
   /odometry/filtered,
   /camera/rgbd_image_relay


And when I run "rostopic hz /odometry/filtered /camera/rgbd_image_relay", there are no messages sent which would explain the output from the remote launch file, pasted above. In addition, the resulting rqt_graph looks like this:

rqt_graph

Now I'm quite new at ROS and I can't quite get this remote mapping to work properly with the rgbd_sync nodelet. Do you have any idea how to fix the problem, how to publish /odometry/filtered and /camera/rgbd_image_relay correctly? I suspect the problem lies in the usage of the rgbd_sync nodelet but I can't get it to function at the moment...

Thank you in advance,

Karleno
Free forum by Nabble Edit this page