Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

low fps #358

Closed
jungladicitta opened this issue Oct 9, 2019 · 15 comments
Closed

low fps #358

jungladicitta opened this issue Oct 9, 2019 · 15 comments

Comments

@jungladicitta
Copy link

jungladicitta commented Oct 9, 2019

I have tested rtabmap on my jetson tx2 with Intel Realsense d435 camera and got very low fps and freezes.
I tried to run rs_rgbd.launch separately and it works fine.
Here is rtabmap parameters: rtabmap.launch

What can I do to speedup rtabmap?

I also tried to run rtabmap with this parameters on different maichine which have Intel core i5-9400f processor on it, and anyway rtabmap worked slow with freezes.

@matlabbe
Copy link
Member

matlabbe commented Oct 9, 2019

Can you show the cpu usage and terminal output of rtabmap launch file?
$ top
That rtabmap configuration would be quite low in cpu, with map refresh rate at 1 Hz.

@jungladicitta
Copy link
Author

@matlabbe
I run turtlbot3_slam_3d.launch file. Included launch files you can view in the same directory

Screenshot from 2019-10-10 05-32-38

nvidia@tegra-ubuntu:~$ roslaunch turtlebot3_slam_3d turtlebot3_slam_3d.launch
... logging to /home/nvidia/.ros/log/b26cde06-eb20-11e9-8806-00044bc771c7/roslaunch-tegra-ubuntu-28643.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
 
started roslaunch server http://JETSON_HOSTNAME:36666/
 
SUMMARY
========
 
PARAMETERS
 * /camera/realsense2_camera/accel_fps: 250
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: True
 * /camera/realsense2_camera/aligned_depth_to_color_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/allow_no_texture_points: False
 * /camera/realsense2_camera/base_frame_id: camera_link
 * /camera/realsense2_camera/calib_odom_file: 
 * /camera/realsense2_camera/clip_distance: -1.0
 * /camera/realsense2_camera/color_fps: 15
 * /camera/realsense2_camera/color_frame_id: camera_color_frame
 * /camera/realsense2_camera/color_height: 480
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: 640
 * /camera/realsense2_camera/depth_fps: 15
 * /camera/realsense2_camera/depth_frame_id: camera_depth_frame
 * /camera/realsense2_camera/depth_height: 480
 * /camera/realsense2_camera/depth_optical_frame_id: camera_depth_opti...
 * /camera/realsense2_camera/depth_width: 640
 * /camera/realsense2_camera/enable_accel: True
 * /camera/realsense2_camera/enable_color: True
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_fisheye1: True
 * /camera/realsense2_camera/enable_fisheye2: True
 * /camera/realsense2_camera/enable_fisheye: True
 * /camera/realsense2_camera/enable_gyro: True
 * /camera/realsense2_camera/enable_infra1: False
 * /camera/realsense2_camera/enable_infra2: False
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: 
 * /camera/realsense2_camera/fisheye1_frame_id: camera_fisheye1_f...
 * /camera/realsense2_camera/fisheye1_optical_frame_id: camera_fisheye1_o...
 * /camera/realsense2_camera/fisheye2_frame_id: camera_fisheye2_f...
 * /camera/realsense2_camera/fisheye2_optical_frame_id: camera_fisheye2_o...
 * /camera/realsense2_camera/fisheye_fps: 15
 * /camera/realsense2_camera/fisheye_frame_id: camera_fisheye_frame
 * /camera/realsense2_camera/fisheye_height: 480
 * /camera/realsense2_camera/fisheye_optical_frame_id: camera_fisheye_op...
 * /camera/realsense2_camera/fisheye_width: 640
 * /camera/realsense2_camera/gyro_fps: 400
 * /camera/realsense2_camera/gyro_frame_id: camera_gyro_frame
 * /camera/realsense2_camera/gyro_optical_frame_id: camera_gyro_optic...
 * /camera/realsense2_camera/imu_optical_frame_id: camera_imu_optica...
 * /camera/realsense2_camera/infra1_frame_id: camera_infra1_frame
 * /camera/realsense2_camera/infra1_optical_frame_id: camera_infra1_opt...
 * /camera/realsense2_camera/infra2_frame_id: camera_infra2_frame
 * /camera/realsense2_camera/infra2_optical_frame_id: camera_infra2_opt...
 * /camera/realsense2_camera/infra_fps: 15
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_width: 640
 * /camera/realsense2_camera/initial_reset: False
 * /camera/realsense2_camera/json_file_path: 
 * /camera/realsense2_camera/linear_accel_cov: 0.01
 * /camera/realsense2_camera/odom_frame_id: camera_odom_frame
 * /camera/realsense2_camera/pointcloud_texture_index: 0
 * /camera/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /camera/realsense2_camera/pose_frame_id: camera_pose_frame
 * /camera/realsense2_camera/pose_optical_frame_id: camera_pose_optic...
 * /camera/realsense2_camera/publish_odom_tf: True
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/topic_odom_in: camera/odom_in
 * /camera/realsense2_camera/unite_imu_method: none
 * /pointcloud_to_laserscan/angle_increment: 0.0087
 * /pointcloud_to_laserscan/angle_max: 1.5708
 * /pointcloud_to_laserscan/angle_min: -1.5708
 * /pointcloud_to_laserscan/concurrency_level: 2
 * /pointcloud_to_laserscan/max_height: 1.7
 * /pointcloud_to_laserscan/min_height: 0.0
 * /pointcloud_to_laserscan/range_max: 4.0
 * /pointcloud_to_laserscan/range_min: 0.2
 * /pointcloud_to_laserscan/scan_time: 0.3333
 * /pointcloud_to_laserscan/target_frame: base_link
 * /pointcloud_to_laserscan/transform_tolerance: 0.01
 * /pointcloud_to_laserscan/use_inf: True
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 50.0
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /rtabmap/rtabmap/Grid/MaxObstacleHeight: 1.7
 * /rtabmap/rtabmap/Grid/NoiseFilteringMinNeighbors: 5
 * /rtabmap/rtabmap/Grid/NoiseFilteringRadius: 0.05
 * /rtabmap/rtabmap/Kp/MaxDepth: 1.75
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/Mem/RehearsalSimilarity: 0.45
 * /rtabmap/rtabmap/Optimizer/Slam2D: true
 * /rtabmap/rtabmap/RGBD/AngularUpdate: 0.05
 * /rtabmap/rtabmap/RGBD/LinearUpdate: 0.05
 * /rtabmap/rtabmap/RGBD/NeighborLinkRefining: true
 * /rtabmap/rtabmap/RGBD/OptimizeFromGraphEnd: false
 * /rtabmap/rtabmap/RGBD/ProximityBySpace: true
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/Rtabmap/DetectionRate: 1.0
 * /rtabmap/rtabmap/Rtabmap/TimeThr: 700
 * /rtabmap/rtabmap/Vis/InlierDistance: 0.1
 * /rtabmap/rtabmap/Vis/MaxDepth: 1.75
 * /rtabmap/rtabmap/Vis/MinInliers: 5
 * /rtabmap/rtabmap/database_path: rtabmap.db
 * /rtabmap/rtabmap/frame_id: base_link
 * /rtabmap/rtabmap/queue_size: 100
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: True
 
NODES
  /camera/
    color_rectify_color (nodelet/nodelet)
    points_xyzrgb_hw_registered (nodelet/nodelet)
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)
  /
    odomtry_publisher (odom_publisher/odomtry_publisher.py)
    pointcloud_to_laserscan (pointcloud_to_laserscan/pointcloud_to_laserscan_node)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz (rviz/rviz)
 
ROS_MASTER_URI=http://192.168.35.61:11311
 
process[pointcloud_to_laserscan-1]: started with pid [28673]
process[odomtry_publisher-2]: started with pid [28674]
process[robot_state_publisher-3]: started with pid [28684]
process[camera/realsense2_camera_manager-4]: started with pid [28687]
process[camera/realsense2_camera-5]: started with pid [28694]
process[camera/color_rectify_color-6]: started with pid [28702]
process[camera/points_xyzrgb_hw_registered-7]: started with pid [28710]
process[rtabmap/rtabmap-8]: started with pid [28722]
process[rviz-9]: started with pid [28746]
[ INFO] [1570686175.457254256]: Initializing nodelet with 6 worker threads.
[ INFO] [1570686175.803963558]: RealSense ROS v2.2.8
[ INFO] [1570686175.804094600]: Running with LibRealSense v2.28.1
[ INFO] [1570686175.811141187]: Starting node...
[ INFO] [1570686175.954869070]: Initializing nodelet with 6 worker threads.
[ INFO] [1570686176.393844260]: getParameters...
[ INFO] [1570686176.603539216]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1570686176.603689138]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1570686176.603759283]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1570686176.603830356]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1570686176.603975286]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1570686176.604056695]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1570686176.604127384]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1570686176.604195161]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1570686176.609156250]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1570686176.829369647]: rtabmap: frame_id      = base_link
[ INFO] [1570686176.829524497]: rtabmap: map_frame_id  = map
[ INFO] [1570686176.829623090]: rtabmap: use_action_for_goal  = false
[ INFO] [1570686176.829722292]: rtabmap: tf_delay      = 0.050000
[ INFO] [1570686176.829816949]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1570686176.829899254]: rtabmap: odom_sensor_sync   = false
[ INFO] [1570686176.995508388]: setupDevice...
[ INFO] [1570686176.995736071]: JSON file is not provided
[ INFO] [1570686176.995938378]: ROS Node Namespace: camera
[ INFO] [1570686176.996095052]: Device Name: Intel RealSense D435
[ INFO] [1570686176.996222573]: Device Serial No: 818312070288
[ INFO] [1570686176.996354895]: Device FW version: 05.11.11.100
[ INFO] [1570686176.996483377]: Device Product ID: 0x0B07
[ INFO] [1570686176.996606738]: Enable PointCloud: Off
[ INFO] [1570686176.996729780]: Align Depth: On
[ INFO] [1570686176.996835925]: Sync Mode: On
[ INFO] [1570686177.000754696]: Device Sensors: 
[ INFO] [1570686177.003126535]: Stereo Module was found.
[ INFO] [1570686177.003614830]: RGB Camera was found.
[ INFO] [1570686177.004029299]: (Fisheye, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.004212789]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.004377016]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.004597946]: (Gyro, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.004785981]: (Accel, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.004952191]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1570686177.006843640]: num_filters: 0
[ INFO] [1570686177.006993146]: Setting Dynamic reconfig parameters.
[ INFO] [1570686177.371744477]: Done Setting Dynamic reconfig parameters.
[ INFO] [1570686177.503579961]: depth stream is enabled - width: 640, height: 480, fps: 15
[ INFO] [1570686177.524033700]: color stream is enabled - width: 640, height: 480, fps: 15
[ INFO] [1570686177.526706119]: setupPublishers...
[ INFO] [1570686177.549373616]: Expected frequency for depth = 15.00000
[ INFO] [1570686177.892553048]: Setting RTAB-Map parameter "Grid/MaxObstacleHeight"="1.7"
[ INFO] [1570686177.945177577]: Expected frequency for color = 15.00000
[ INFO] [1570686177.948513492]: Setting RTAB-Map parameter "Grid/NoiseFilteringMinNeighbors"="5"
[ INFO] [1570686177.954665509]: Setting RTAB-Map parameter "Grid/NoiseFilteringRadius"="0.05"
[ INFO] [1570686178.245408587]: Expected frequency for aligned_depth_to_color = 15.00000
[ INFO] [1570686178.644959976]: setupStreams...
[ INFO] [1570686178.739180925]: insert Depth to Stereo Module
[ INFO] [1570686178.742872718]: insert Color to RGB Camera
[ INFO] [1570686178.880730655]: SELECTED BASE:Depth, 0
[ INFO] [1570686179.009801854]: RealSense Node Is Up!
[ INFO] [1570686179.021790556]: Setting RTAB-Map parameter "Kp/MaxDepth"="1.75"
[ INFO] [1570686179.333483463]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1570686179.336299308]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1570686179.472293996]: Setting RTAB-Map parameter "Mem/RehearsalSimilarity"="0.45"
[ INFO] [1570686179.724022977]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.05"
[ INFO] [1570686179.784075832]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.05"
[ INFO] [1570686179.889233154]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="true"
[ INFO] [1570686179.900641112]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false"
[ INFO] [1570686179.974448677]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true"
[ INFO] [1570686180.070563002]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1570686180.089729815]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1570686180.111059281]: Setting RTAB-Map parameter "Rtabmap/DetectionRate"="1.0"
[ INFO] [1570686180.349423905]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="700"
[ INFO] [1570686181.079755324]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1"
[ INFO] [1570686181.093122125]: Setting RTAB-Map parameter "Vis/MaxDepth"="1.75"
[ INFO] [1570686181.119098278]: Setting RTAB-Map parameter "Vis/MinInliers"="5"
[ WARN] [1570686182.301741472]: Rtabmap: Parameter name changed: "Optimizer/Slam2D" -> "Reg/Force3DoF". Please update your launch file accordingly. Value "true" is still set to the new parameter name.
[ WARN] [1570686182.707527707]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan" or "subscribe_scan_cloud" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add <param name="Grid/FromDepth" type="string" value="false"/>
[ INFO] [1570686182.707992449]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" is true.
[ WARN] [1570686182.708185124]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 10 (default 0) as "subscribe_scan" is true and "Reg/Strategy" uses ICP. Proximity detection by space will be also done by merging close scans. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
[ INFO] [1570686183.883980149]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1570686183.884470171]: rtabmap: Deleted database "/home/nvidia/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1570686183.884637758]: rtabmap: Using database from "/home/nvidia/.ros/rtabmap.db" (0 MB).
[ INFO] [1570686184.670317214]: rtabmap: Database version = "0.19.3".
[ INFO] [1570686184.829175152]: /rtabmap/rtabmap: subscribe_depth = true
[ INFO] [1570686184.829345138]: /rtabmap/rtabmap: subscribe_rgb = true
[ INFO] [1570686184.829452084]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1570686184.829569141]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1570686184.829690583]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1570686184.830422881]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1570686184.830564418]: /rtabmap/rtabmap: subscribe_scan = true
[ INFO] [1570686184.830690020]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1570686184.831124330]: /rtabmap/rtabmap: queue_size    = 100
[ INFO] [1570686184.831247500]: /rtabmap/rtabmap: approx_sync   = true
[ INFO] [1570686184.831552880]: Setup depth callback
[ INFO] [1570686184.969817005]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /odom,
   /camera/color/image_rect_color,
   /camera/depth/image_rect_raw,
   /camera/color/camera_info,
   /scan
[ INFO] [1570686185.089462965]: rtabmap 0.19.3 started...
[ INFO] [1570686186.179813033]: rtabmap (1): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1712s, Maps update=0.0044s pub=0.0006s (local map=1, WM=1)
[ INFO] [1570686188.425119972]: rtabmap (2): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1809s, Maps update=0.0004s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686190.410754206]: rtabmap (3): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1546s, Maps update=0.0004s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686192.399731549]: rtabmap (4): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1583s, Maps update=0.0005s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686193.235511198]: rtabmap (5): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1432s, Maps update=0.0006s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686195.554152735]: rtabmap (6): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1674s, Maps update=0.0005s pub=0.0000s (local map=1, WM=1)
[ WARN] [1570686196.344247862]: Messages of type 4 arrived out of order (will print only once)
[ INFO] [1570686197.412165399]: rtabmap (7): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1362s, Maps update=0.0011s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686198.538233787]: rtabmap (8): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1679s, Maps update=0.0005s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686199.397716235]: rtabmap (9): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1477s, Maps update=0.0004s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686201.254233670]: rtabmap (10): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1504s, Maps update=0.0005s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686203.435239654]: rtabmap (11): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1399s, Maps update=0.0006s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686205.475063497]: rtabmap (12): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1552s, Maps update=0.0005s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686206.375656009]: rtabmap (13): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1473s, Maps update=0.0003s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686208.581990609]: rtabmap (14): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1638s, Maps update=0.0005s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686210.382502774]: rtabmap (15): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1634s, Maps update=0.0005s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686211.399285549]: rtabmap (16): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1641s, Maps update=0.0003s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686212.521662008]: rtabmap (17): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1559s, Maps update=0.0008s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686213.454878880]: rtabmap (18): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1582s, Maps update=0.0004s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686214.638902712]: rtabmap (19): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1811s, Maps update=0.0004s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686215.609334776]: rtabmap (20): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1465s, Maps update=0.0272s pub=0.0004s (local map=1, WM=1)
[ INFO] [1570686217.631548990]: rtabmap (21): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1505s, Maps update=0.0338s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686218.380932890]: rtabmap (22): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1647s, Maps update=0.0420s pub=0.0004s (local map=1, WM=1)
[ INFO] [1570686220.763456721]: rtabmap (23): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1686s, Maps update=0.0442s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686221.681477679]: rtabmap (24): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1985s, Maps update=0.0507s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686223.393154650]: rtabmap (25): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2415s, Maps update=0.0452s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686225.570970096]: rtabmap (26): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1909s, Maps update=0.0363s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686226.494649714]: rtabmap (27): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1635s, Maps update=0.0382s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686227.230883726]: rtabmap (28): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1991s, Maps update=0.0299s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686228.614027252]: rtabmap (29): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1990s, Maps update=0.0402s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686230.541802469]: rtabmap (30): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1734s, Maps update=0.0381s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686231.166514783]: rtabmap (31): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1554s, Maps update=0.0304s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686233.617614410]: rtabmap (32): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1456s, Maps update=0.0415s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686234.626479538]: rtabmap (33): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1551s, Maps update=0.0284s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686236.460813933]: rtabmap (34): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1786s, Maps update=0.0521s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686238.651716182]: rtabmap (35): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2024s, Maps update=0.0498s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686240.366913271]: rtabmap (36): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2103s, Maps update=0.0330s pub=0.0000s (local map=1, WM=1)
[ INFO] [1570686242.588266209]: rtabmap (37): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1546s, Maps update=0.0294s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686244.557585882]: rtabmap (38): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1928s, Maps update=0.0341s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686246.655089502]: rtabmap (39): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1788s, Maps update=0.0316s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686247.656808347]: rtabmap (40): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1428s, Maps update=0.0433s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686249.497560847]: rtabmap (41): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1746s, Maps update=0.0500s pub=0.0001s (local map=1, WM=1)
[ INFO] [1570686251.566788767]: rtabmap (42): Rate=1.00s, Limit=0.700s, RTAB-Map=0.1861s, Maps update=0.0289s pub=0.0001s (local map=1, WM=1)
^C[rviz-9] killing on exit
[rtabmap/rtabmap-8] killing on exit
[camera/points_xyzrgb_hw_registered-7] killing on exit
[camera/color_rectify_color-6] killing on exit
[robot_state_publisher-3] killing on exit
[camera/realsense2_camera-5] killing on exit
[camera/realsense2_camera_manager-4] killing on exit
[odomtry_publisher-2] killing on exit
[pointcloud_to_laserscan-1] killing on exit
terminate called without an active exception
rtabmap: Saving database/long-term memory... (located at /home/nvidia/.ros/rtabmap.db)
rtabmap: 2D occupancy grid map saved.
rtabmap: Saving database/long-term memory...done! (located at /home/nvidia/.ros/rtabmap.db, 10 MB)
shutting down processing monitor...
... shutting down processing monitor complete
done

@jungladicitta
Copy link
Author

@matlabbe

That rtabmap configuration would be quite low in cpu, with map refresh rate at 1 Hz.

why? what is the problem to refresh every second? which parameters can I change?

@nacorti
Copy link

nacorti commented Oct 13, 2019

@jungladicitta use the /rtabmap/rtabmap/Rtabmap/DetectionRate: 1.0 param in your launch file to set the detection rate in Hz. Right now it's 1Hz, which updates once per second.

That said, I'm also using a D435 on a Jetson Nano, and am noticing that I get much lower FPS off my depth stream (10-15) while rendering in RViz than I have set incoming from RealSense (30 FPS). Also wondering if there are any best practices for optimizing memory/compute on small boards like Jetson.

@matlabbe
Copy link
Member

matlabbe commented Oct 15, 2019

The detection rate is set by default at 1 Hz (Rtabmap/DetectionRate parameter), I rarely set it higher unless the robot is moving fast. In the logs, the update time is between 150 and 200 ms, so only 15-20% of one core would be used for rtabmap.

I would suggest not running rviz on the jetson. And if you run rviz on remote computer, use an ethernet cable, and don't show raw point clouds from realsense. If you only show /rtabmap/mapData, the bandwidth would be not too high.

If rtabmap is publishing slower than 1 Hz with its processing time around 200 ms, the input topics may be poorly synchronized, causing some lags. If you have odom, scan, image topics, verify all their frame rate with rostopic hz (on the jetson).

@jungladicitta
Copy link
Author

jungladicitta commented Oct 15, 2019

@matlabbe thanks a lot for so detailed answer.

What do you mean by poorly synchronized? you mean the input topics should be publishing with the same rate, for example 100 Hz?

I will try to set the same rate to the input topics, and then running rviz on remote computer.

@jungladicitta
Copy link
Author

jungladicitta commented Oct 15, 2019

@matlabbe about rviz on jetson, I was showing raw point cloud from camera in rviz with rtabmap running and rate of this raw point cloud from camera was ok. dont you think it does not matter where we run rviz?

@jungladicitta
Copy link
Author

jungladicitta commented Oct 15, 2019

@matlabbe
here is my input topics for rtabmap:

nvidia@tegra-ubuntu:~$ rostopic hz /camera/color/camera_info 
subscribed to [/camera/color/camera_info]
average rate: 13.375
	min: 0.013s max: 0.139s std dev: 0.04586s window: 12
average rate: 13.240
	min: 0.012s max: 0.139s std dev: 0.04470s window: 24
average rate: 12.122
	min: 0.012s max: 0.139s std dev: 0.04621s window: 35
average rate: 11.998
	min: 0.012s max: 0.139s std dev: 0.04869s window: 46
average rate: 12.024
	min: 0.012s max: 0.139s std dev: 0.04828s window: 59
^Caverage rate: 11.872
	min: 0.012s max: 0.139s std dev: 0.04759s window: 66
nvidia@tegra-ubuntu:~$ rostopic hz /camera/color/image_rect_color
subscribed to [/camera/color/image_rect_color]
average rate: 14.930
	min: 0.021s max: 0.106s std dev: 0.01705s window: 15
average rate: 15.008
	min: 0.021s max: 0.106s std dev: 0.01222s window: 30
^Caverage rate: 14.958
	min: 0.021s max: 0.106s std dev: 0.01100s window: 38
nvidia@tegra-ubuntu:~$ rostopic hz /camera/depth/image_rect_raw
subscribed to [/camera/depth/image_rect_raw]
average rate: 15.319
	min: 0.008s max: 0.105s std dev: 0.02354s window: 15
average rate: 15.039
	min: 0.008s max: 0.105s std dev: 0.01939s window: 30
average rate: 15.084
	min: 0.008s max: 0.111s std dev: 0.01912s window: 45
^Caverage rate: 15.082
	min: 0.008s max: 0.111s std dev: 0.01981s window: 52
nvidia@tegra-ubuntu:~$ rostopic hz /odom
subscribed to [/odom]
average rate: 10.019
	min: 0.081s max: 0.118s std dev: 0.01225s window: 9
average rate: 10.008
	min: 0.081s max: 0.118s std dev: 0.01042s window: 19
average rate: 10.000
	min: 0.081s max: 0.118s std dev: 0.00904s window: 29
average rate: 9.997
	min: 0.081s max: 0.118s std dev: 0.00870s window: 39
average rate: 10.003
	min: 0.075s max: 0.123s std dev: 0.00947s window: 49
average rate: 10.001
	min: 0.075s max: 0.123s std dev: 0.00885s window: 60
average rate: 10.001
	min: 0.075s max: 0.123s std dev: 0.00819s window: 70
average rate: 10.001
	min: 0.075s max: 0.123s std dev: 0.00788s window: 80
^Caverage rate: 10.002
	min: 0.075s max: 0.123s std dev: 0.00774s window: 83
nvidia@tegra-ubuntu:~$ rostopic hz /scan
subscribed to [/scan]
average rate: 8.868
	min: 0.099s max: 0.124s std dev: 0.00915s window: 8
average rate: 8.700
	min: 0.098s max: 0.150s std dev: 0.01421s window: 17
average rate: 8.826
	min: 0.097s max: 0.150s std dev: 0.01357s window: 26
average rate: 9.015
	min: 0.091s max: 0.150s std dev: 0.01304s window: 35
average rate: 8.885
	min: 0.091s max: 0.159s std dev: 0.01445s window: 44
average rate: 8.830
	min: 0.091s max: 0.159s std dev: 0.01451s window: 52
average rate: 8.837
	min: 0.076s max: 0.159s std dev: 0.01628s window: 61
average rate: 8.917
	min: 0.076s max: 0.159s std dev: 0.01544s window: 71
average rate: 8.946
	min: 0.076s max: 0.159s std dev: 0.01487s window: 80

What do you think? Are these topics poorly synchronized?

@nacorti
Copy link

nacorti commented Oct 15, 2019

@matlabbe appreciate the detailed response, thank you.

wrt resource consumption, I find that my CPU usage (all four cores) hovers around 90%, while GPU fluctuates pretty wildly and RAM/Swap is pretty underutilized. Is there an rtabmap setting that will use more of my RAM? Wondering if it'll get my CPU usage down a bit.

wrt topic framerates, my image and depth framerates are synced at about 15-25 hz, but my odom/pose is coming in at around 200 hz. What's happening is as my pose changes, rtabmap projects the stale depth/image map into an incorrect/inaccurate place on the map (until the next depth map comes in). Is there an rtabmap parameter that could prevent this from happening? approx_sync is false, and when it's set to true, I get errors because my timestamps aren't exactly the same. The alternative is downsampling the odom topic, which I'd like to avoid if possible.

@jungladicitta
Copy link
Author

@matlabbe
I have checked input topics rates for rtabmap and they are ok.
I have tried to run rviz on remote computer with using 5 GHz wifi and is still very slow.
Moreover, tf refreshes with big delay. Approximately 5-10 seconds.
what else can you recommend to check?

@matlabbe
Copy link
Member

@nacorti When you say CPU usage at 90%, how much is form rtabmap? From this post above, rtabmap is about 20% of one core. Actually point_cloud_to_laser_scan node is 117% usage. As for odometry topic published at 200 Hz, you can increase the queue_size parameter of rtabmap node. It will help to synchronize topics with very different frame rates. Another approach is to use TF for odometry by setting odom_frame_id parameter.

@jungladicitta Be careful when launching RVIZ on remote computer to not subscribe to point cloud or uncompressed image topics. Check your network bandwidth if it is stable. I would suggest to connect through ethernet to debug easily without wifi latencies and bandwidth limitations.

@nacorti
Copy link

nacorti commented Oct 23, 2019

@matlabbe

Wasn't able to deduce how much CPU usage was from Rtabmap vs RViz - I don't have an Ubuntu machine handy besides my Jetson to take your advice about running RViz separately over an ethernet cable. But, I completed a series of steps that (I think) increased my performance pretty significantly

  1. I turned off the IR streams from my D435, so I just get Depth and Color. Reduces load over USB/superfluous ROS topics
  2. Followed your advice about queue_size, set to 200
  3. Followed your advice from rtabmap #449, I increased Grid/CellSize and decreased Vis/MaxFeatures
  4. I actually increased the DetectionRate to 30Hz, which allows me to use fewer features and thus do less pre-processing on the image bc subsequent images aren't as different as they would be at 1HZ

Right now the big issue I have is running out of RAM once my map gets too large, but I think that's a limitation of the Nano more than anything. Going to try Mem/ImagePostDecimation and Mem/ReduceGraph to see if any change.

I'm also in the process of building OpenCV4 w GPU, then rebuilding RTabMap with OpenCV4 and seeing if that can boost framerate. I'll let you know how it goes.

Comments/critiques always welcome, and of course thank you for your usual prompt and informative replies :)

@jungladicitta
Copy link
Author

@matlabbe every topics published by camera is uncompressed right? I mean

/camera/depth/image_rect_raw
/camera/color/image_rect_color

@matlabbe
Copy link
Member

matlabbe commented Oct 23, 2019

@nacorti For the RAM problem, it depends of what you want to keep in the database. If you don't care about keeping raw RGB/depth image, you can set Mem/BinDataKept to false. If you don't need occupancy grids, set RGBD/CreateOccupancyGrid to false. 30 Hz is quite high, by decreasing the number of features in the images, you can also decrease the loop closure detection performance. If you don't care about global map optimization and loop closures (only in odometry mode), you can disable features extraction with Kp/MaxFeatures=-1. Mem/ImagePostDecimation can be a good way to keep raw RGB/Depth images in database at lower resolution, if you want to keep them. You may also check if it is only rtabmap that is taking a lot of RAM, or it could be also RVIZ or rtabmapviz if you show the point clouds.

@jungladicitta Yes, without suffix like "/compressed", they are the raw images.

@jungladicitta
Copy link
Author

I tried to connect everything to jetson via usb 3.0 hub, I mean camera, wheels and lidar, and now it looks ok in rviz.
I think problem was that I was transfering camera raw data through wi-fi.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants