orbbec / OrbbecSDK_ROS1

OrbbecSDK ROS wrapper
Apache License 2.0
45 stars 27 forks source link

Invalid Quaternion for Femto Bolt #49

Open marcomasa opened 4 hours ago

marcomasa commented 4 hours ago

When enabling the "publish_tf" option, other nodes dependand on TF are logging errors:

[ERROR] [1727277411.522716528]: Ignoring transform for child_frame_id "femto_bolt_accel_frame" from authority "unknown_publisher" because of a nan value in the transform (-nan -0.000000 -inf) (-nan -nan -nan -nan)
[ERROR] [1727277411.522808829]: Ignoring transform for child_frame_id "femto_bolt_accel_frame" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)
[ERROR] [1727277411.522854529]: Ignoring transform for child_frame_id "femto_bolt_gyro_frame" from authority "unknown_publisher" because of a nan value in the transform (-nan -0.000000 -inf) (-nan -nan -nan -nan)
[ERROR] [1727277411.522912130]: Ignoring transform for child_frame_id "femto_bolt_gyro_frame" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)

The ROS message in /femto_bolt/gyro_accel/sample also shows an invalid quaternion (cannot be 4 times zero!) :

header: 
  seq: 249
  stamp: 
    secs: 1727277526
    nsecs: 640000000
  frame_id: "camera_gyro_optical_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.001064225216396153
  y: -0.0026605629827827215
  z: 0.01489915233105421
angular_velocity_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]
linear_acceleration: 
  x: -0.013159179128706455
  y: -0.05861816182732582
  z: -9.943553924560547
linear_acceleration_covariance: [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]
---

How can i solve this?

marcomasa commented 3 hours ago

This is also "confirmed" by the start logs:

[ INFO] [1727279518.445303048]: Image stream depth subscribed
[ INFO] [1727279518.610421738]: Publishing static transform from ir to depth
[ INFO] [1727279518.610572059]: Translation 0, 0, 0
[ INFO] [1727279518.610682975]: Rotation 0, 0, 0, 1
[ INFO] [1727279518.610719780]: Publishing static transform from color to depth
[ INFO] [1727279518.610818494]: Translation 32.387, 1.58323, -2.90842
[ INFO] [1727279518.610912207]: Rotation 0.00136692, -0.0531993, 0.00166793, 0.998582
[ INFO] [1727279518.611009320]: Publishing static transform from depth to depth
[ INFO] [1727279518.611105734]: Translation 0, 0, 0
[ INFO] [1727279518.611175343]: Rotation 0, 0, 0, 1
[ INFO] [1727279518.611280158]: Publishing static transform from accel to depth
[ INFO] [1727279518.611391173]: Translation 0, inf, -nan
[ INFO] [1727279518.611500589]: Rotation -nan, -nan, -nan, -nan
[ INFO] [1727279518.611610604]: Publishing static transform from gyro to depth
[ INFO] [1727279518.611723620]: Translation 0, inf, -nan
[ INFO] [1727279518.611830034]: Rotation -nan, -nan, -nan, -nan

How can i set this manually? Or is there even a way to do so?