Implementing PX4 avoidance in mission mode using the voxl and QGC

[scottesicdrone]

Thank you for your help, the UDP connection seems to be working for the most part! The connection is initiated and the ROS connections seem to be connected but it seems like the pointcloud isn't being updated fast enough. I checked the pointcloud topic made by the snap_dfs manager and that was being updated at around 2Hz at most and the avoidance algorithm requires the pointcloud to updated at least 10Hz. I also opened up the pointcloud in rviz and saw that the pointcloud was updating at that rate. Is this normal behavior or is something else going on here? I apologize that I keep encountering roadblocks but I do think I am close to getting this!

[Chad Sweet]

Check out the bottom of this readme where it recommends different parameters to increase frame rate. https://github.com/ATLFlight/dfs-ros-example

Basically, in any computer vision task it's a tradeoff of frame rate and resolution

[Chad Sweet]

And FWIW, you don't need high resolution for obstacle avoidance. Just need to see things not to run in to!

[Chad Sweet]

@scottesicdrone curious to see if you were you able to get things working?

[J Data]

@Chad-Sweet
I had a look the link you shared: snap_vio
But I think it is not working correctly with the voxl module I have.
Currently, I am running ROS noetic on the voxl module, and try to run the VIO with downward camer.
By using snap_camera_ros (I edited it a little bit to use the downward camera), I can see the downward camera is correctly set up for snap_vio. Also, I can see snap_imu is working correctly, so I can see the output of imu (angular_velocity and linear_acceleration).

But I cannot see any output of /downward/vio/odometry.

Could you please help me to run snap_vio with the voxl module?
What should I check first?

[J Data]

The running file is similar with this.

<!-- camera nodelet -->
<!-- VIO nodelet -->
<!-- fisheye camera info spoofer -->
<!-- imu nodelet -->
<!-- cpa nodelet -->

[J Data]

@Chad-Sweet
Update.

I have output /downward/vio/odometry with some error messages

[ INFO] [1648150781.892213161]: [VISLAM] Got uav1/imu to uav1/dfc transform; Initializing VISLAM
[ INFO] [1648150781.892816862]: [VISLAM] (from tf) tbc:
x: 0.2
y: 0
z: -0.025

[ INFO] [1648150781.893485304]: [VISLAM] (from tf) ombc:
X: 0
Y: 2.356
Z: 0

MachineVision is licensed as community user
LNX_8074 supported? 1
LNX_8096 supported? 1
LNX_IA64 supported? 1
WINDOWS supported? 0
AR ERROR: arFileOpen(): Failed to open file: /data/.ros/vislam/Configuration.SF.xml
FASTCV: fcvAvailableHardware Linux
mempool cur block size 307200, new block size 307200
AR ERROR: arFileOpen(): Failed to open file: /data/.ros/na

Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of a nan value in the transform (0.000000 0.000000 0.000000) (-nan -nan -nan nan)
         at line 244 in ~/geometry2/tf2/src/buffer_core.cpp
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan nan)
         at line 257 in ~/geometry2/tf2/src/buffer_core.cpp
[ERROR] [1648150782.073463584]: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of a nan value in the transform (0.000000 0.000000 0.000000) (-nan -nan -nan nan)
[ERROR] [1648150782.077556213]: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan nan)
[ERROR] [1648150782.094956631]: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of a nan value in the transform (0.000000 0.000000 0.000000) (-nan -nan -nan nan)
[ERROR] [1648150782.095484499]: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan nan)
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of a nan value in the transform (0.000000 0.000000 0.000000) (-nan -nan -nan nan)
         at line 244 in ~/tf2/src/buffer_core.cpp
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "grav" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan nan)
         at line 257 in ~/geometry2/tf2/src/buffer_core.cpp

and repeated error message of

[ERROR] [1648150919.088496536]: [VISLAM] ERROR CODE = 2048, Stamp: 1648150919.022946523

I am wondering how urdf should be set. Should there be any rotations of IMU or camera in urdf file?
urdf file I used has some rotations in y-axis for IMU and cameras.

[J Data]

@Chad-Sweet
I am using customized urdf for snap_vio which has different numbers (positions and orientations) of sdf_pro.urdf.

Is there any urdf file for VOXL?

[J Data]

@Chad-Sweet , No matter the tf setting in the urdf file is, the ros topic outputs are shown in the followings

1. rostopic echo mavros/imu/data (in base_link frame): x is backward, y is right, z is down
2. rostopic echo downward/imu (in imu frame): x is right, y is backward, z is down

[J Data]

@modalab Hello, can you help me to resolve this problem?