PX4 Avoidance on Hardware

Hi, all

I’m trying to run Avoidance project on Intel Aero RTF.
I can run the code on Gazebo simulator and everything is ok.
Currently I’m trying to run this code on hardware.
I’m using instructions in:
https://docs.px4.io/en/flight_controller/intel_aero.html

My commands:

roslaunch realsense_camera r200_nodelet_default.launch - started ok

when I typing:
roslaunch local_planner local_planner_aero.launch

I get:

[ERROR] [1539018882.328336810]: /camera/driver - Couldn’t start camera – UVCIOC_CTRL_QUERY:UVC_SET_CUR error 5, Input/output error
[FATAL] [1539018882.598798177]: Failed to load nodelet ‘/camera/driverof typerealsense_camera/R200Nodeletto managercamera_nodelet_manager’
[FATAL] [1539018882.599824128]: Failed to load nodelet ‘/camera/depth_registered_sw_metric_rectof typedepth_image_proc/convert_metricto managercamera_nodelet_manager’
[FATAL] [1539018882.599911667]: Failed to load nodelet ‘/camera/depth_rectify_depthof typeimage_proc/rectifyto managercamera_nodelet_manager’
[FATAL] [1539018882.600472725]: Failed to load nodelet ‘/camera/rgb_rectify_colorof typeimage_proc/rectifyto managercamera_nodelet_manager’
[FATAL] [1539018882.598970317]: Failed to load nodelet ‘/camera/depth_metricof typedepth_image_proc/convert_metricto managercamera_nodelet_manager’
[FATAL] [1539018882.599303646]: Failed to load nodelet ‘/camera/rgb_rectify_monoof typeimage_proc/rectifyto managercamera_nodelet_manager’
[FATAL] [1539018882.599350947]: Failed to load nodelet ‘/camera/rgb_debayerof typeimage_proc/debayerto managercamera_nodelet_manager’
[FATAL] [1539018882.599539512]: Failed to load nodelet ‘/camera/depth_pointsof typedepth_image_proc/point_cloud_xyzto managercamera_nodelet_manager’
[FATAL] [1539018882.599593325]: Failed to load nodelet ‘/camera/depth_metric_rectof typedepth_image_proc/convert_metricto managercamera_nodelet_manager’
[FATAL] [1539018882.599670301]: Failed to load nodelet ‘/camera/register_depth_rgbof typedepth_image_proc/registerto managercamera_nodelet_manager’
[FATAL] [1539018882.599747990]: Failed to load nodelet ‘/camera/points_xyzrgb_sw_registeredof typedepth_image_proc/point_cloud_xyzrgbto managercamera_nodelet_manager’
[FATAL] [1539018882.600004156]: Failed to load nodelet '/camera/disparity_registered_swof typedepth_image_proc/disparityto managercamera_nodelet_manager ’
[FATAL] [1539018882.600074169]: Failed to load nodelet ‘/camera/ir_rectify_irof typeimage_proc/rectifyto managercamera_nodelet_manager’
[FATAL] [1539018882.600370123]: Failed to load nodelet ‘/camera/disparity_depthof type depth_image_proc/disparityto managercamera_nodelet_manager’
[ WARN] [1539018882.878547469]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539018882.879694484]: Pointcloud timeout: No position received, no WP to output…

[ WARN] [1539018882.928394900]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539018882.928590678]: Pointcloud timeout: No position received, no WP to output…

Anybody knows what is the problem ?

Best regards
Evgeny

Hi Evgeny,

Am I correctly assuming that you tried to run both lines:

roslaunch realsense_camera r200_nodelet_default.launch
roslaunch local_planner local_planner_aero.launch

at the same time?

This is redundant work and probably what’s causing the issues. The local_planner_aero.launch file starts its own instance of the realsense nodelet (in this line).

Does it run okay if you just start the local_planner_aero.launch ?

hope that helps,
Nico

Nico, Hi

thank you for the fast response.

I’m tried only
roslaunch local_planner local_planner_aero.launch

and then I get less errors, but some errors still persist:

[ WARN] [1539022102.388395382]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539022102.388712098]: Pointcloud timeout: No position received, no
WP to output…

[ INFO] [1539022104.913373469]: /camera/driver - Setting dynamic camera options
[ERROR] [1539022105.466151801]: Received an exception trying to transform a
point from “camera_optical_frame” to “local_origin”: Could not find a
connection between ‘local_origin’ and ‘camera_depth_optical_frame’ because
they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1539022105.467065937]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539022105.467215126]: Pointcloud timeout: No position received, no
WP to output…

[ERROR] [1539022108.495960935]: Received an exception trying to transform a
point from “camera_optical_frame” to “local_origin”: Could not find a
connection between ‘local_origin’ and ‘camera_depth_optical_frame’ because
they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1539022108.496886808]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539022108.497022260]: Pointcloud timeout: No position received, no
WP to output…

[ERROR] [1539022111.523229441]: Received an exception trying to transform a
point from “camera_optical_frame” to “local_origin”: Could not find a
connection between ‘local_origin’ and ‘camera_depth_optical_frame’ because
they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1539022111.524257065]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539022111.524494756]: Pointcloud timeout: No position received, no
WP to output…

[ERROR] [1539022114.549666738]: Received an exception trying to transform a
point from “camera_optical_frame” to “local_origin”: Could not find a
connection between ‘local_origin’ and ‘camera_depth_optical_frame’ because
they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1539022114.551158818]: MSG to TF: Quaternion Not Properly Normalized
[ WARN] [1539022114.551434109]: Pointcloud timeout: No position received, no
WP to output…

[ERROR] [1539022117.578818481]: Received an exception trying to transform a
point from “camera_optical_frame” to “local_origin”: Could not find a
connection between ‘local_origin’ and ‘camera_depth_optical_frame’ because
they are not part of the same tree.Tf has two or more unconnected trees.
[ WARN] [1539022117.580010808]: MSG to TF: Quaternion Not Properly Normalized
[ERROR] [1539022117.586129293]: MODE: Unsupported FCU
[ERROR] [1539022117.587009816]: Pointcloud timeout: Landing failed!

I saved a full output to the file.
Just don’t know how to attach it.

Best regards
Evgeny

It seems that the TF node cannot find a transformation between the camera and the aero. In my experience, this is most often the case if PX4 doesn’t have a position estimate yet (EKF not initialized).

Can you confirm that your PX4 is running and has a reasonable position estimate? You could do a
rostopic echo /mavros/local_position/pose
in a separate terminal and see if you get something

P.S. If you are trying this indoors, then of course it won’t work, since the GPS signal is required for EKF2 to produce a position estimate…

Nico, Hi

Thank you.
I will try it outdoor.

Evgeny

Hi,
Tried outdoor.
Same result…
Here attached terminal output saved to the file.

EvgenyAvoidance_log_outdoor.pdf (48.9 KB)

Evgeny

Hi.

Can you share a snapshot of your tf tree?

Hi

Attached TF tree file.
frames.pdf (20.0 KB)

This pretty much confirms what I suspected, there doesn’t exist a link between the camera frame and the local coordinate frame. Think of it like this: the avoidance receives the point cloud, but does not know where those points are relative to the drone.

My best guess is that the missing piece is somewhere in the mavros configuration. How are you connecting your offboard to the flight control unit?

You are missing a connetion between the Pixhawk and the conmanion computer. The transform between local origin and fcu is published by mavros as well as the position of the drone. You seem to get neither of those.

Have you connected the pixhawk over serial? if so, check fcu_url in the launch file and set it to the correct tty port
If you connect over udp (e.g. mavlink router), set the fcu_url to the correct udp port

Ok, Now its working.

It was a problem of mavlink router settings.
Anyway thanks for assistance.
I flied using commands:

rosrun mavros mavsys mode -c OFFBOARD
rosrun mavros mavsafety arm

It going to specific location and hovering.
Avoidance algorithm working ( the drone trying to avoid obstacles).
Now I want to concentrate on the code understanding.
How I can control the drone ?
only with waypoints or in any other way?
What configuration parameters I can change ?
Is there some relevant documentation about code?

Best regards
Evgeny

Glad to hear it’s working for you now.

The “offboard” mode of PX4 is actually more of a hack for research purposes, used by various research groups running their custom offboard logic. Originally, the avoidance was developed using just the offboard mode of PX4.

More recently, the avoidance has been refactored to work in mission mode as well. This should be much easier to use from a user perspective: you can define a misison in QGC, make sure you set the parameter “MPC_OBS_AVOID” to 1 (I’m not sure what the default value is), and it should work. Note that this mission interface works for the local planner, but is not yet merged for the global planner (PR #76).

There is still little documentation on parameters and their meaning - feel free to contribute! You can fool around with rqt_reconfigure and dig into the code to find out. If you’re looking for a scientific explanation this might be a good read.

Hope that helps!
Nico

1 Like