search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
916 stars 549 forks source link

Detected not valid consecutive stamps #1156

Closed zacharyyamaoka closed 1 month ago

zacharyyamaoka commented 1 month ago

Hello!

I am having an undesirable behaviour I was hoping you could help with!

I am using a intel real sense d405. I am sending rgb, depth and camera info straight to an RGBD odometry node.

The headers on the rgb, depth and camera info are synced perfectly. The odometry node however throws an error because the "stamps should always be greater"

[preprocess.py-3] [INFO] [1716442518.000482384] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442517, nanosec=948470459)
[preprocess.py-3] [INFO] [1716442518.000750444] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442517, nanosec=948470459)
[preprocess.py-3] [INFO] [1716442518.001016701] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442517, nanosec=948470459)
[rgbd_odometry-7] [INFO] [1716442518.027858494] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=355, std dev=0.000427m|0.039831rad, update time=0.032465s
[rgbd_odometry-7] [WARN] [1716442518.028543712] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442517.948471s new=1716442517.948471s). New stamp should be always greater than previous stamp. This new data is ignored.

The node is only subscribed to the rgb, depth and info topic.

bam-denailer@bam-denailer:~/rtabmap_ws/src$ ros2 node info /bam_BAMDENAILER/rgbd_odometry
/bam_BAMDENAILER/rgbd_odometry
  Subscribers:
    /bam_BAMDENAILER/camera_top/aligned_depth_to_color/image_raw/preprocessed: sensor_msgs/msg/Image
    /bam_BAMDENAILER/camera_top/color/camera_info/preprocessed: sensor_msgs/msg/CameraInfo
    /bam_BAMDENAILER/camera_top/color/image_rect_raw/preprocessed: sensor_msgs/msg/Image
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /bam_BAMDENAILER/odom: nav_msgs/msg/Odometry
    /bam_BAMDENAILER/odom_info: rtabmap_msgs/msg/OdomInfo
    /bam_BAMDENAILER/odom_info_lite: rtabmap_msgs/msg/OdomInfo
    /bam_BAMDENAILER/odom_last_frame: sensor_msgs/msg/PointCloud2
    /bam_BAMDENAILER/odom_local_map: sensor_msgs/msg/PointCloud2
    /bam_BAMDENAILER/odom_local_scan_map: sensor_msgs/msg/PointCloud2
    /bam_BAMDENAILER/odom_rgbd_image: rtabmap_msgs/msg/RGBDImage
    /bam_BAMDENAILER/odom_sensor_data/compressed: rtabmap_msgs/msg/SensorData
    /bam_BAMDENAILER/odom_sensor_data/features: rtabmap_msgs/msg/SensorData
    /bam_BAMDENAILER/odom_sensor_data/raw: rtabmap_msgs/msg/SensorData
    /diagnostics: diagnostic_msgs/msg/DiagnosticArray
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /tf: tf2_msgs/msg/TFMessage
  Service Servers:
    /bam_BAMDENAILER/log_debug: std_srvs/srv/Empty
    /bam_BAMDENAILER/log_error: std_srvs/srv/Empty
    /bam_BAMDENAILER/log_info: std_srvs/srv/Empty
    /bam_BAMDENAILER/log_warning: std_srvs/srv/Empty
    /bam_BAMDENAILER/pause_odom: std_srvs/srv/Empty
    /bam_BAMDENAILER/reset_odom: std_srvs/srv/Empty
    /bam_BAMDENAILER/reset_odom_to_pose: rtabmap_msgs/srv/ResetPose
    /bam_BAMDENAILER/resume_odom: std_srvs/srv/Empty
    /bam_BAMDENAILER/rgbd_odometry/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /bam_BAMDENAILER/rgbd_odometry/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /bam_BAMDENAILER/rgbd_odometry/get_parameters: rcl_interfaces/srv/GetParameters
    /bam_BAMDENAILER/rgbd_odometry/get_type_description: type_description_interfaces/srv/GetTypeDescription
    /bam_BAMDENAILER/rgbd_odometry/list_parameters: rcl_interfaces/srv/ListParameters
    /bam_BAMDENAILER/rgbd_odometry/set_parameters: rcl_interfaces/srv/SetParameters
    /bam_BAMDENAILER/rgbd_odometry/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

The odometry does work, but it doesn't seem to be tracking well as I move the object, and I thought this is perhaps caused by it dropping the data, as per the error message,

Thank you for help!!

zacharyyamaoka commented 1 month ago

Here are some more logs. The ingoing messages are the same, rgb, depth, camera info with synced time stamps, but every so often the odom throws the error:

[preprocess.py-3] [INFO] [1716442765.664873713] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442765.665144267] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=619203613)
[preprocess.py-3] [INFO] [1716442765.665321798] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=619203613)
[preprocess.py-3] [INFO] [1716442765.665568017] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=619203613)
[rgbd_odometry-7] [INFO] [1716442765.709361231] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=171, std dev=0.000392m|0.035182rad, update time=0.033887s

[preprocess.py-3] [INFO] [1716442765.863733909] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442765.863961553] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=819199463)
[preprocess.py-3] [INFO] [1716442765.864138273] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=819199463)
[preprocess.py-3] [INFO] [1716442765.864301988] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442765, nanosec=819199463)
[rgbd_odometry-7] [INFO] [1716442765.909813614] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=173, std dev=0.000506m|0.043152rad, update time=0.035185s

[preprocess.py-3] [INFO] [1716442766.066192281] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442766.066505605] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=19196533)
[preprocess.py-3] [INFO] [1716442766.066693304] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=19196533)
[preprocess.py-3] [INFO] [1716442766.066874983] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=19196533)
[rgbd_odometry-7] [INFO] [1716442766.105400662] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=128, std dev=0.000382m|0.034641rad, update time=0.039523s

[preprocess.py-3] [INFO] [1716442766.263421068] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442766.263712431] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=219196777)
[preprocess.py-3] [INFO] [1716442766.263903798] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=219196777)
[preprocess.py-3] [INFO] [1716442766.264071129] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=219196777)
[rgbd_odometry-7] [INFO] [1716442766.304188074] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=222, std dev=0.000424m|0.038764rad, update time=0.040266s
[rgbd_odometry-7] [WARN] [1716442766.304773995] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442766.219197s new=1716442766.219197s). New stamp should be always greater than previous stamp. This new data is ignored.

[preprocess.py-3] [INFO] [1716442766.473818865] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442766.474080973] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=419197266)
[preprocess.py-3] [INFO] [1716442766.474261419] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=419197266)
[preprocess.py-3] [INFO] [1716442766.474433600] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=419197266)
[rgbd_odometry-7] [INFO] [1716442766.516892608] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=153, std dev=0.000410m|0.038614rad, update time=0.032296s

[preprocess.py-3] [INFO] [1716442766.673267018] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442766.673545988] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=619197266)
[preprocess.py-3] [INFO] [1716442766.673726634] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=619197266)
[preprocess.py-3] [INFO] [1716442766.673892964] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=619197266)
[rgbd_odometry-7] [INFO] [1716442766.703179918] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=153, std dev=0.000336m|0.036406rad, update time=0.040742s
[rgbd_odometry-7] [WARN] [1716442766.703901102] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442766.619197s new=1716442766.619197s). New stamp should be always greater than previous stamp. This new data is ignored.

[preprocess.py-3] [INFO] [1716442766.874030271] [bam_BAMDENAILER.preprocess]: Sending new preprocessed images
[preprocess.py-3] [INFO] [1716442766.874264177] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=819198730)
[preprocess.py-3] [INFO] [1716442766.874435476] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=819198730)
[preprocess.py-3] [INFO] [1716442766.874606675] [bam_BAMDENAILER.preprocess]: builtin_interfaces.msg.Time(sec=1716442766, nanosec=819198730)
[rgbd_odometry-7] [INFO] [1716442766.899848631] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=141, std dev=0.000276m|0.031364rad, update time=0.037115s
[rgbd_odometry-7] [WARN] [1716442766.900558564] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442766.819199s new=1716442766.819199s). New stamp should be always greater than previous stamp. This new data is ignored.
matlabbe commented 1 month ago

What means pre-processed images? Are some same images published on same topic name? You can check with ros2 topic info /bam_BAMDENAILER/camera_top/aligned_depth_to_color/image_raw/preprocessed to see if there is more than one publisher for that topic.

Looking at this log:

[rgbd_odometry-7] [INFO] [1716442766.304188074] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=222, std dev=0.000424m|0.038764rad, update time=0.040266s
[rgbd_odometry-7] [WARN] [1716442766.304773995] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442766.219197s new=1716442766.219197s). New stamp should be always greater than previous stamp. This new data is ignored.
[rgbd_odometry-7] [INFO] [1716442766.516892608] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=153, std dev=0.000410m|0.038614rad, update time=0.032296s

There is a delay over 200 ms between the consecutive images. If you do ros2 topic hz /bam_BAMDENAILER/camera_top/aligned_depth_to_color/image_raw/preprocessed, has it a stable rate?

zacharyyamaoka commented 1 month ago

Hi Mathieu,

Thanks so much for taking a look at this! I wasn't able to fix the problem but I was still able to do the demo I wanted to do.

I will close this ticket for now.... :)

Thanks again for your time

On Sat, May 25, 2024 at 6:08 PM matlabbe @.***> wrote:

What means pre-processed images? Are some same images published on same topic name? You can check with ros2 topic info /bam_BAMDENAILER/camera_top/aligned_depth_to_color/image_raw/preprocessed to see if there is more than one publisher for that topic.

Looking at this log:

[rgbd_odometry-7] [INFO] [1716442766.304188074] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=222, std dev=0.000424m|0.038764rad, update time=0.040266s [rgbd_odometry-7] [WARN] [1716442766.304773995] [bam_BAMDENAILER.rgbd_odometry]: Odometry: Detected not valid consecutive stamps (previous=1716442766.219197s new=1716442766.219197s). New stamp should be always greater than previous stamp. This new data is ignored. [rgbd_odometry-7] [INFO] [1716442766.516892608] [bam_BAMDENAILER.rgbd_odometry]: Odom: quality=153, std dev=0.000410m|0.038614rad, update time=0.032296s

There is a delay over 200 ms between the consecutive images. If you do ros2 topic hz /bam_BAMDENAILER/camera_top/aligned_depth_to_color/image_raw/preprocessed, has it a stable rate?

— Reply to this email directly, view it on GitHub https://github.com/introlab/rtabmap_ros/issues/1156#issuecomment-2131861045, or unsubscribe https://github.com/notifications/unsubscribe-auth/AGZUQL3J5TT7X6OLE3WNM4TZEEYZNAVCNFSM6AAAAABIE5JQJCVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDCMZRHA3DCMBUGU . You are receiving this because you authored the thread.Message ID: @.***>