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 type
realsense_camera/R200Nodeletto manager
camera_nodelet_manager’
[FATAL] [1539018882.599824128]: Failed to load nodelet ‘/camera/depth_registered_sw_metric_rectof type
depth_image_proc/convert_metricto manager
camera_nodelet_manager’
[FATAL] [1539018882.599911667]: Failed to load nodelet ‘/camera/depth_rectify_depthof type
image_proc/rectifyto manager
camera_nodelet_manager’
[FATAL] [1539018882.600472725]: Failed to load nodelet ‘/camera/rgb_rectify_colorof type
image_proc/rectifyto manager
camera_nodelet_manager’
[FATAL] [1539018882.598970317]: Failed to load nodelet ‘/camera/depth_metricof type
depth_image_proc/convert_metricto manager
camera_nodelet_manager’
[FATAL] [1539018882.599303646]: Failed to load nodelet ‘/camera/rgb_rectify_monoof type
image_proc/rectifyto manager
camera_nodelet_manager’
[FATAL] [1539018882.599350947]: Failed to load nodelet ‘/camera/rgb_debayerof type
image_proc/debayerto manager
camera_nodelet_manager’
[FATAL] [1539018882.599539512]: Failed to load nodelet ‘/camera/depth_pointsof type
depth_image_proc/point_cloud_xyzto manager
camera_nodelet_manager’
[FATAL] [1539018882.599593325]: Failed to load nodelet ‘/camera/depth_metric_rectof type
depth_image_proc/convert_metricto manager
camera_nodelet_manager’
[FATAL] [1539018882.599670301]: Failed to load nodelet ‘/camera/register_depth_rgbof type
depth_image_proc/registerto manager
camera_nodelet_manager’
[FATAL] [1539018882.599747990]: Failed to load nodelet ‘/camera/points_xyzrgb_sw_registeredof type
depth_image_proc/point_cloud_xyzrgbto manager
camera_nodelet_manager’
[FATAL] [1539018882.600004156]: Failed to load nodelet '/camera/disparity_registered_swof type
depth_image_proc/disparityto manager
camera_nodelet_manager ’
[FATAL] [1539018882.600074169]: Failed to load nodelet ‘/camera/ir_rectify_irof type
image_proc/rectifyto manager
camera_nodelet_manager’
[FATAL] [1539018882.600370123]: Failed to load nodelet ‘/camera/disparity_depthof type
depth_image_proc/disparityto manager
camera_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