Skip to content

Crash in vision_pose_estimate.cpp: can't compare time sources #1734

New issue
New issue
Closed
#1735
Closed
Crash in vision_pose_estimate.cpp: can't compare time sources#1734
#1735
@clydemcqueen

Description

@clydemcqueen
clydemcqueen
opened on Apr 25, 2022

Issue details

  1. launch mavros with the vision_pose plugin enabled.
  2. publish a pose to /mavros/vision_pose/pose
  3. crash in src/plugins/vision_pose_estimate.cpp

The crash happens on this line:

if (last_transform_stamp == stamp) {

last_transform_stamp is initialized to RCL_SYSTEM_TIME and stamp is initialized to RCL_ROS_TIME. These are not comparable.

I worked around this by initializing last_transform_stamp to RCL_ROS_TIME:

rclcpp::Time last_transform_stamp{0, 0, RCL_ROS_TIME};

I'm happy to provide a small PR.

MAVROS version and platform

Mavros: ros2 HEAD (cf95850)
ROS: Galactic
Ubuntu: 20.04

Autopilot type and version

[ x ] ArduPilot
[ ] PX4

Version: latest

Running ArduSub in SITL.

Node logs

[INFO] [mavros_node-7]: process started with pid [732555]
[mavros_node-7] [INFO 1650909935.492738541] [mavros_node/main]: Starting mavros_node container
[mavros_node-7] [INFO 1650909935.492826258] [mavros_node/main]: FCU URL: tcp://127.0.0.1:5760
[mavros_node-7] [INFO 1650909935.492842891] [mavros_node/main]: GCS URL: 
[mavros_node-7] [INFO 1650909935.492854033] [mavros_node/main]: UAS Prefix: /uas1
[mavros_node-7] [INFO 1650909935.492864241] [mavros_node/main]: Starting mavros router node
[mavros_node-7] [INFO 1650909935.505452315] [mavros_router/Router]: Built-in SIMD instructions: SSE, SSE2
[mavros_node-7] [INFO 1650909935.506282230] [mavros_router/Router]: Built-in MAVLink package version: 2022.3.3
[mavros_node-7] [INFO 1650909935.506831139] [mavros_router/Router]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[mavros_node-7] [INFO 1650909935.506850759] [mavros_router/Router]: MAVROS Router started
[mavros_node-7] [INFO 1650909935.507055734] [mavros_router/add_endpoint]: Requested to add endpoint: type: 0, url: tcp://127.0.0.1:5760
[mavros_node-7] [INFO 1650909935.507417395] [mavros_router/add_endpoint]: Endpoint link[1000] created
[mavros_node-7] [INFO 1650909935.510006335] [mavros_router/add_endpoint]: link[1000] opened successfully
[mavros_node-7] [INFO 1650909935.510684444] [mavros_router/add_endpoint]: Requested to add endpoint: type: 2, url: /uas1
[mavros_node-7] [INFO 1650909935.510839919] [mavros_router/add_endpoint]: Endpoint link[1001] created
[mavros_node-7] [INFO 1650909935.513455360] [mavros_router/add_endpoint]: link[1001] opened successfully
[mavros_node-7] [INFO 1650909935.515150528] [mavros_node/main]: Starting mavros uas node
[mavros_node-7] [INFO 1650909935.557601120] [mavros/operator()]: UAS Executor started, threads: 8
[mavros_node-7] [INFO 1650909935.557764788] [mavros/add_plugin]: Plugin actuator_control ignored
[mavros_node-7] [INFO 1650909935.557792146] [mavros/add_plugin]: Plugin adsb ignored
[mavros_node-7] [INFO 1650909935.557804642] [mavros/add_plugin]: Plugin altitude ignored
[mavros_node-7] [INFO 1650909935.557825817] [mavros/add_plugin]: Plugin cam_imu_sync ignored
[mavros_node-7] [INFO 1650909935.557841620] [mavros/add_plugin]: Plugin camera ignored
[mavros_node-7] [INFO 1650909935.585103256] [mavros/add_plugin]: Plugin command created
[mavros_node-7] [INFO 1650909935.585383400] [mavros/add_plugin]: Plugin command initialized
[mavros_node-7] [INFO 1650909935.585404469] [mavros/add_plugin]: Plugin companion_process_status ignored
[mavros_node-7] [INFO 1650909935.585415081] [mavros/add_plugin]: Plugin debug_value ignored
[mavros_node-7] [INFO 1650909935.585424372] [mavros/add_plugin]: Plugin distance_sensor ignored
[mavros_node-7] [INFO 1650909935.585434440] [mavros/add_plugin]: Plugin esc_status ignored
[mavros_node-7] [INFO 1650909935.585443615] [mavros/add_plugin]: Plugin esc_telemetry ignored
[mavros_node-7] [INFO 1650909935.585452416] [mavros/add_plugin]: Plugin fake_gps ignored
[mavros_node-7] [INFO 1650909935.585460965] [mavros/add_plugin]: Plugin ftp ignored
[mavros_node-7] [INFO 1650909935.585480708] [mavros/add_plugin]: Plugin geofence ignored
[mavros_node-7] [INFO 1650909935.585490055] [mavros/add_plugin]: Plugin global_position ignored
[mavros_node-7] [INFO 1650909935.585500355] [mavros/add_plugin]: Plugin gps_input ignored
[mavros_node-7] [INFO 1650909935.585509229] [mavros/add_plugin]: Plugin gps_rtk ignored
[mavros_node-7] [INFO 1650909935.585519525] [mavros/add_plugin]: Plugin gps_status ignored
[mavros_node-7] [INFO 1650909935.585528384] [mavros/add_plugin]: Plugin hil ignored
[mavros_node-7] [INFO 1650909935.585537177] [mavros/add_plugin]: Plugin home_position ignored
[mavros_node-7] [INFO 1650909935.595484963] [mavros/add_plugin]: Plugin imu created
[mavros_node-7] [INFO 1650909935.595877967] [mavros/add_plugin]: Plugin imu initialized
[mavros_node-7] [INFO 1650909935.595902343] [mavros/add_plugin]: Plugin landing_target ignored
[mavros_node-7] [INFO 1650909935.606319532] [mavros/add_plugin]: Plugin local_position created
[mavros_node-7] [INFO 1650909935.606519705] [mavros/add_plugin]: Plugin local_position initialized
[mavros_node-7] [INFO 1650909935.606547194] [mavros/add_plugin]: Plugin log_transfer ignored
[mavros_node-7] [INFO 1650909935.606562687] [mavros/add_plugin]: Plugin mag_calibration_status ignored
[mavros_node-7] [INFO 1650909935.606576256] [mavros/add_plugin]: Plugin manual_control ignored
[mavros_node-7] [INFO 1650909935.606590287] [mavros/add_plugin]: Plugin mocap_pose_estimate ignored
[mavros_node-7] [INFO 1650909935.606604229] [mavros/add_plugin]: Plugin mount_control ignored
[mavros_node-7] [INFO 1650909935.606618343] [mavros/add_plugin]: Plugin nav_controller_output ignored
[mavros_node-7] [INFO 1650909935.606635893] [mavros/add_plugin]: Plugin obstacle_distance ignored
[mavros_node-7] [INFO 1650909935.606650156] [mavros/add_plugin]: Plugin odometry ignored
[mavros_node-7] [INFO 1650909935.606686312] [mavros/add_plugin]: Plugin onboard_computer_status ignored
[mavros_node-7] [INFO 1650909935.606699965] [mavros/add_plugin]: Plugin param ignored
[mavros_node-7] [INFO 1650909935.606709781] [mavros/add_plugin]: Plugin play_tune ignored
[mavros_node-7] [INFO 1650909935.606718517] [mavros/add_plugin]: Plugin px ignored
[mavros_node-7] [INFO 1650909935.606727413] [mavros/add_plugin]: Plugin rallypoint ignored
[mavros_node-7] [INFO 1650909935.606738701] [mavros/add_plugin]: Plugin rangefinder ignored
[mavros_node-7] [INFO 1650909935.614520774] [mavros/add_plugin]: Plugin rc_io created
[mavros_node-7] [INFO 1650909935.614751057] [mavros/add_plugin]: Plugin rc_io initialized
[mavros_node-7] [INFO 1650909935.614772850] [mavros/add_plugin]: Plugin setpoint_accel ignored
[mavros_node-7] [INFO 1650909935.614783418] [mavros/add_plugin]: Plugin setpoint_attitude ignored
[mavros_node-7] [INFO 1650909935.624200197] [mavros/add_plugin]: Plugin setpoint_position created
[mavros_node-7] [INFO 1650909935.624254223] [mavros/add_plugin]: Plugin setpoint_position initialized
[mavros_node-7] [INFO 1650909935.624274111] [mavros/add_plugin]: Plugin setpoint_raw ignored
[mavros_node-7] [INFO 1650909935.624286757] [mavros/add_plugin]: Plugin setpoint_trajectory ignored
[mavros_node-7] [INFO 1650909935.624298721] [mavros/add_plugin]: Plugin setpoint_velocity ignored
[mavros_node-7] [INFO 1650909935.637025691] [mavros/add_plugin]: Plugin sys_status created
[mavros_node-7] [INFO 1650909935.637551321] [mavros/add_plugin]: Plugin sys_status initialized
[mavros_node-7] [INFO 1650909935.637579114] [mavros/add_plugin]: Plugin sys_time ignored
[mavros_node-7] [INFO 1650909935.637589637] [mavros/add_plugin]: Plugin tdr_radio ignored
[mavros_node-7] [INFO 1650909935.637598602] [mavros/add_plugin]: Plugin terrain ignored
[mavros_node-7] [INFO 1650909935.637607191] [mavros/add_plugin]: Plugin trajectory ignored
[mavros_node-7] [INFO 1650909935.637616274] [mavros/add_plugin]: Plugin tunnel ignored
[mavros_node-7] [INFO 1650909935.637630423] [mavros/add_plugin]: Plugin vfr_hud ignored
[mavros_node-7] [INFO 1650909935.637639671] [mavros/add_plugin]: Plugin vibration ignored
[mavros_node-7] [INFO 1650909935.660242330] [mavros/add_plugin]: Plugin vision_pose created
[mavros_node-7] [INFO 1650909935.660293168] [mavros/add_plugin]: Plugin vision_pose initialized
[mavros_node-7] [INFO 1650909935.660312206] [mavros/add_plugin]: Plugin vision_speed ignored
[mavros_node-7] [INFO 1650909935.660322667] [mavros/add_plugin]: Plugin waypoint ignored
[mavros_node-7] [INFO 1650909935.660332082] [mavros/add_plugin]: Plugin wheel_odomotry ignored
[mavros_node-7] [INFO 1650909935.660341104] [mavros/add_plugin]: Plugin wind_estimation ignored
[mavros_node-7] [INFO 1650909935.661649909] [mavros/operator()]: Built-in SIMD instructions: SSE, SSE2
[mavros_node-7] [INFO 1650909935.661684575] [mavros/operator()]: Built-in MAVLink package version: 2022.3.3
[mavros_node-7] [INFO 1650909935.661697040] [mavros/operator()]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[mavros_node-7] [INFO 1650909935.661705065] [mavros/operator()]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[mavros_node-7] [INFO 1650909936.631020168] [mavros_router/recv_message]: link[1001] detected remote address 1.191
[mavros_node-7] [WARN 1650909937.637972255] [mavros.sys/autopilot_version_cb]: VER: broadcast request timeout, retries left 4
[mavros_node-7] [INFO 1650909937.833203183] [mavros_router/recv_message]: link[1000] detected remote address 1.1
[mavros_node-7] [WARN 1650909937.833542547] [mavros.cmd/handle_command_ack]: CMD: Unexpected command 520, result 0
[mavros_node-7] terminate called after throwing an instance of 'std::runtime_error'
[mavros_node-7]   what():  can't compare times with different time sources
[ERROR] [mavros_node-7]: process has died [pid 732555, exit code -6, cmd '/home/clyde/ros2/orca4_ws/install/mavros/lib/mavros/mavros_node --ros-args --params-file /home/clyde/ros2/orca4_ws/install/orca_bringup/share/orca_bringup/params/mavros_params.yaml'].

Diagnostics

Before publishing a pose:

header:
  stamp:
    sec: 1650910738
    nanosec: 260109119
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '28552'
  - key: Messages sent
    value: '28552'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: tcp://127.0.0.1:5760'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '28478'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '62'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '874866'
  - key: Tx total bytes
    value: '1650'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
- level: "\0"
  name: 'mavros_router: endpoint 1001: /uas1'
  message: ok
  hardware_id: none
  values:
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.191'
---
header:
  stamp:
    sec: 1650910738
    nanosec: 276927878
  frame_id: ''
status:
- level: "\0"
  name: 'mavros: MAVROS UAS'
  message: connected
  hardware_id: uas:///uas1
  values: []
- level: "\x02"
  name: 'mavros: System'
  message: Sensor health
  hardware_id: uas:///uas1
  values:
  - key: Sensor present
    value: '0x1330FD8F'
  - key: Sensor enabled
    value: '0x1320FD8F'
  - key: Sensor health
    value: '0x1330FD0F'
  - key: 3D gyro
    value: Ok
  - key: 3D accelerometer
    value: Ok
  - key: 3D magnetometer
    value: Ok
  - key: absolute pressure
    value: Ok
  - key: computer vision position
    value: Fail
  - key: laser based position
    value: Ok
  - key: 3D angular rate control
    value: Ok
  - key: attitude stabilization
    value: Ok
  - key: yaw position
    value: Ok
  - key: z/altitude control
    value: Ok
  - key: x/y position control
    value: Ok
  - key: motor outputs / control
    value: Ok
  - key: AHRS subsystem health
    value: Ok
  - key: Logging
    value: Ok
  - key: Battery
    value: Ok
  - key: pre-arm check status. Always healthy when armed
    value: Ok
  - key: CPU Load (%)
    value: '0.0'
  - key: Drop rate (%)
    value: '0.0'
  - key: Errors comm
    value: '0'
  - key: 'Errors count #1'
    value: '0'
  - key: 'Errors count #2'
    value: '0'
  - key: 'Errors count #3'
    value: '0'
  - key: 'Errors count #4'
    value: '0'
- level: "\0"
  name: 'mavros: Battery'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Voltage
    value: '11.95'
  - key: Current
    value: '19.9'
  - key: Remaining
    value: '97.0'
- level: "\0"
  name: 'mavros: Heartbeat'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Heartbeats since startup
    value: '62'
  - key: Frequency (Hz)
    value: '1.000018'
  - key: Vehicle type
    value: Submarine
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: ALT_HOLD
  - key: System status
    value: ACTIVE
---

Check ID

Before publishing a pose:

Router topic: /uas1/mavlink_source, target: 1.1
OK. I got messages from 1.1.
---
Received 4069, from 1 addresses
address   list of messages
    1.1   0 (HEARTBEAT), 1 (SYS_STATUS), 129 (SCALED_IMU3), 2 (SYSTEM_TIME), 132 (DISTANCE_SENSOR), 137 (SCALED_PRESSURE2), 147 (BATTERY_STATUS), 24 (GPS_RAW_INT), 152, 27 (RAW_IMU), 29 (SCALED_PRESSURE), 30 (ATTITUDE), 158, 33 (GLOBAL_POSITION_INT), 163, 36 (SERVO_OUTPUT_RAW), 164, 165, 42 (MISSION_CURRENT), 173, 178, 62 (NAV_CONTROLLER_OUTPUT), 65 (RC_CHANNELS), 193, 74 (VFR_HUD), 77 (COMMAND_ACK), 111 (TIMESYNC), 241 (VIBRATION), 116 (SCALED_IMU2), 253 (STATUSTEXT), 251 (NAMED_VALUE_FLOAT), 125 (POWER_STATUS)

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions