px4 / px4-avoidance

PX4 avoidance ROS node for obstacle detection and avoidance.

Obstacle Detection and Avoidance

Warning: This project is currently not maintained.

We'd welcome community support to maintain and update the project. If you're interested in contributing, please contact the PX4 development team through normal channels.

PX4 computer vision algorithms packaged as ROS nodes for depth sensor fusion and obstacle avoidance. This repository contains three different implementations:

• local_planner is a local VFH+* based planner that plans (including some history) in a vector field histogram
• global_planner is a global, graph based planner that plans in a traditional octomap occupancy grid
• safe_landing_planner is a local planner to find safe area to land

The three algorithms are standalone and they are not meant to be used together.

The local_planner requires less computational power but it doesn't compute optimal paths towards the goal since it doesn't store information about the already explored environment. An in-depth discussion on how it works can be found in this thesis. On the other hand, the global_planner is computationally more expensive since it builds a map of the environment. For the map to be good enough for navigation, accurate global position and heading are required. An in-depth discussion on how it works can be found in this thesis. The safe_landing_planner classifies the terrain underneath the vehicle based on the mean and standard deviation of the z coordinate of pointcloud points. The pointcloud from a downwards facing sensor is binned into a 2D grid based on the xy point coordinates. For each bin, the mean and standard deviation of z coordinate of the points are calculated and they are used to locate flat areas where it is safe to land.

Note The most developed and used planner is the local_planner. This is where you should start.

The documentation contains information about how to setup and run the two planner systems on the Gazebo simulator and on a companion computer running Ubuntu 20.04 (recommended), for both avoidance and collision prevention use cases.

Note PX4-side setup is covered in the PX4 User Guide:

• Obstacle Avoidance
• Collision Prevention

Table of Contents

• Getting Started
  • Installation
    • Installation for Ubuntu
  • Run the Avoidance Gazebo Simulation
    • Local Planner
    • Global Planner
    • Safe Landing Planner
  • Run on Hardware
    • Prerequisite
    • Local Planner
    • Global Planner
• Troubleshooting
• Advanced
  • Message Flows
    • PX4 and local planner
    • PX4 and global planner
• Contributing

Getting Started

Installation

Installation

This is a step-by-step guide to install and build all the prerequisites for running the avoidance module on Ubuntu 20.04 with ROS Noetic (includes Gazebo 11). You might want to skip some steps if your system is already partially installed.

Note: These instructions assume your catkin workspace (in which we will build the avoidance module) is in ~/catkin_ws, and the PX4 Firmware directory is ~/Firmware. Feel free to adapt this to your situation.

1. Add ROS to sources.list:

   sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
   sudo apt install curl
   curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
   sudo apt update

2. Install ROS with Gazebo:

   sudo apt install ros-noetic-desktop-full
   
   # Source ROS
   echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
   source ~/.bashrc

   Note We recommend you use the version of Gazebo that comes with your (full) installation of ROS. If you must to use another Gazebo version, remember to install associated ros-gazebo related packages:

   • For Gazebo 8, sudo apt install ros-noetic-gazebo8-*
   • For Gazebo 9, sudo apt install ros-noetic-gazebo9-*

3. Dependencies for building packages

   sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

4. Install and initialize rosdep.

   rosdep init
   rosdep update

5. Install catkin and create your catkin workspace directory.

   sudo apt install python3-catkin-tools
   mkdir -p ~/catkin_ws/src

6. Install MAVROS (version 0.29.0 or above).

   Note: Instructions to install MAVROS from sources can be found here.

   sudo apt install ros-noetic-mavros ros-noetic-mavros-extras

7. Install the geographiclib dataset

   wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
   chmod +x install_geographiclib_datasets.sh
   sudo ./install_geographiclib_datasets.sh

8. Install avoidance module dependencies (pointcloud library and octomap).

   sudo apt install libpcl1 ros-noetic-octomap-*

9. Clone this repository in your catkin workspace in order to build the avoidance node.

   cd ~/catkin_ws/src
   git clone https://github.com/PX4/avoidance.git

10. Actually build the avoidance node.

    catkin build -w ~/catkin_ws

    Note that you can build the node in release mode this way:

    catkin build -w ~/catkin_ws --cmake-args -DCMAKE_BUILD_TYPE=Release

11. Source the catkin setup.bash from your catkin workspace:

    echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
    source ~/.bashrc

Run the Avoidance Gazebo Simulation

In the following section we guide you through installing and running a Gazebo simulation of both local and global planner.

Build and Run the Simulator

1. Clone the PX4 Firmware and all its submodules (it may take some time).

   cd ~
   git clone https://github.com/PX4/Firmware.git --recursive
   cd ~/Firmware

2. Install PX4 dependencies.

   # Install PX4 "common" dependencies.
   ./Tools/setup/ubuntu.sh --no-sim-tools --no-nuttx
   
   # Gstreamer plugins (for Gazebo camera)
   sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev
   

3. Build the Firmware once in order to generate SDF model files for Gazebo. This step will actually run a simulation (that you can immediately close).

   # This is necessary to prevent some Qt-related errors (feel free to try to omit it)
   export QT_X11_NO_MITSHM=1
   
   # Build and run simulation
   make px4_sitl_default gazebo
   
   # Quit the simulation (Ctrl+C)
   
   # Setup some more Gazebo-related environment variables (modify this line based on the location of the Firmware folder on your machine)
   . ~/Firmware/Tools/simulation/gazebo-classic/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_default

4. Add the Firmware directory to ROS_PACKAGE_PATH so that ROS can start PX4:

   export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/Firmware

5. Finally, set the GAZEBO_MODEL_PATH in your bashrc:

   echo "export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/catkin_ws/src/avoidance/avoidance/sim/models:~/catkin_ws/src/avoidance/avoidance/sim/worlds" >> ~/.bashrc

The last three steps, together with sourcing your catkin setup.bash (source ~/catkin_ws/devel/setup.bash) should be repeated each time a new terminal window is open.

OR you can ensure your ~/.bashrc is setup correctly so that each new terminal is sourced correctly.

sudo nano ~/.bashrc

Ensure that at the bottom of that document you have exactly this:

source /opt/ros/noetic/setup.bash
source ~/catkin_ws/devel/setup.bash
. ~/Firmware/Tools/simulation/gazebo-classic/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_default
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/catkin_ws/src/avoidance/avoidance/sim/models:~/catkin_ws/src/avoidance/avoidance/sim/worlds
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/Firmware

You should now be ready to run the simulation using local or global planner.

Local Planner (default, heavily flight tested)

This section shows how to start the local_planner and use it for avoidance in mission or offboard mode.

The planner is based on the 3DVFH+ algorithm.

Note: You may need to install some additional dependencies to run the following code (if not installed):

sudo apt install ros-noetic-stereo-image-proc ros-noetic-image-view

Any of the following three launch file scripts can be used to run local planner:

Note: The scripts run the same planner but simulate different sensor/camera setups. They all enable Obstacle Avoidance and Collision Prevention.

• local_planner_stereo: simulates a vehicle with a stereo camera that uses OpenCV's block matching algorithm (SGBM by default) to generate depth information

  roslaunch local_planner local_planner_stereo.launch

  Note: The disparity map from stereo-image-proc is published as a stereo_msgs/DisparityImage message, which is not supported by rviz or rqt. To visualize the message, first open a new terminal and setup the required environment variables:

  source devel/setup.bash

  Then do either of:

  • run:

    rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color

  • publish the DisparityImage as a simple sensor_msgs/Image:

    rosrun topic_tools transform /stereo/disparity /stereo/disparity_image sensor_msgs/Image 'm.image' 

  The disparity map can then be visualized by rviz or rqt under the topic /stereo/disparity_image.

• local_planner_depth_camera: simulates vehicle with one forward-facing kinect sensor

  roslaunch local_planner local_planner_depth-camera.launch

• local_planner_sitl_3cam: simulates vehicle with 3 kinect sensors (left, right, front)

  roslaunch local_planner local_planner_sitl_3cam.launch

You will see the Iris drone unarmed in the Gazebo world. To start flying, there are two options: OFFBOARD or MISSION mode. For OFFBOARD, run:

# In another terminal 
rosrun mavros mavsys mode -c OFFBOARD
rosrun mavros mavsafety arm

The drone will first change its altitude to reach the goal height. It is possible to modify the goal altitude with rqt_reconfigure GUI. Then the drone will start moving towards the goal. The default x, y goal position can be changed in Rviz by clicking on the 2D Nav Goal button and then choosing the new goal x and y position by clicking on the visualized gray space. If the goal has been set correctly, a yellow sphere will appear where you have clicked in the grey world.

For MISSIONS, open QGroundControl and plan a mission as described here. Set the parameter COM_OBS_AVOID true. Start the mission and the vehicle will fly the mission waypoints dynamically recomputing the path such that it is collision free.

Global Planner (advanced, not flight tested)

This section shows how to start the global_planner and use it for avoidance in offboard mode.

roslaunch global_planner global_planner_stereo.launch

You should now see the drone unarmed on the ground in a forest environment as pictured below.

To start flying, put the drone in OFFBOARD mode and arm it. The avoidance node will then take control of it.

# In another terminal
rosrun mavros mavsys mode -c OFFBOARD
rosrun mavros mavsafety arm

Initially the drone should just hover at 3.5m altitude.

From the command line, you can also make Gazebo follow the drone, if you want.

gz camera --camera-name=gzclient_camera --follow=iris

One can plan a new path by setting a new goal with the 2D Nav Goal button in rviz. The planned path should show up in rviz and the drone should follow the path, updating it when obstacles are detected. It is also possible to set a goal without using the obstacle avoidance (i.e. the drone will go straight to this goal and potentially collide with obstacles). To do so, set the position with the 2D Pose Estimate button in rviz.

Safe Landing Planner

This section shows how to start the safe_landing_planner and use it to land safely in mission or auto land mode. To run the node:

roslaunch safe_landing_planner safe_landing_planner.launch

You will see an unarmed vehicle on the ground. Open QGroundControl, either plan a mission with the last item of type Land or fly around the world in Position Control, click the Land button on the left side where you wish to land. At the land position, the vehicle will start to descend towards the ground until it is at loiter_height from the ground/obstacle. Then it will start loitering to evaluate the ground underneeth. If the ground is flat, the vehicle will continue landing. Otherwise it will evaluate the close by terrain in a squared spiral pattern until it finds a good enough ground to land on.

Run on Hardware

Prerequisite

Camera

Both planners require a 3D point cloud of type sensor_msgs::PointCloud2. Any camera that can provide such data is compatible.

The officially supported camera is Intel Realsense D435. We recommend using Firmware version 5.9.13.0. The instructions on how to update the Firmware of the camera can be found here

Tip: Be careful when attaching the camera with a USB3 cable. USB3 might might interfere with GPS and other signals. If possible, always use USB2 cables.

Other tested camera models are: Intel Realsense D415 and R200, Occipital Structure Core.

Generating Point-clouds from Depth-maps

In case the point-cloud stream already exists, this step can be skipped.

Assuming there already exists a stream of depth-maps on the ROS-topic <depthmap_topic>, we need to generate a corresponding stream of depth-maps. Start by following the instructions from PX4/disparity_to_point_cloud. Now run the point-cloud generation with the parameters for the camera intrinsics:

rosrun disparity_to_point_cloud disparity_to_point_cloud_node \
    fx_:=fx fy_:=fy cx_:=cx cy_:=cy base_line_:=base_line disparity:=<depthmap_topic>

A stream of point-clouds should now be published to /point_cloud.

PX4 Autopilot

Parameters to set through QGC:

• COM_OBS_AVOID to Enabled
• MAV_1_CONFIG, MAV_1_MODE, SER_TEL2_BAUD to enable MAVLink on a serial port. For more information: PX4 Dev Guide

Companion Computer

• OS: Ubuntu 20.04 OS or a docker container running Ubuntu 20.04 must be setup (e.g. if using on a Yocto based system).
• ROS Noetic: see Installation
• Other Required Components for Intel Realsense:
  • Librealsense (Realsense SDK). The installation instructions can be found here
  • Librealsense ROS wrappers
• Other Required Components for Occipital Structure Core:
  • Download the Structure SDK. The version tested with this package is 0.7.1. Create the build directory and build the SDK

  mkdir build
  cd build
  cmake ..
  make

  • Clone the ROS wrapper in the catkin_ws
  • Copy the shared object Libraries/Structure/Linux/x86_64/libStructure.so from the SDK into /usr/local/lib/
  • Copy the headers from Libraries/Structure/Headers/ in the SDK to the ROS wrapper include directory ~/catkin_ws/src/struct_core_ros/include

Tested models:

• local planner: Intel NUC, Jetson TX2, Intel Atom x7-Z8750 (built-in on Intel Aero RTF drone)
• global planner: Odroid

Global Planner

The global planner has been so far tested on a Odroid companion computer by the development team.

Local Planner

Once the catkin workspace has been built, to run the planner with a Realsense D435 or Occipital Structure Core camera you can generate the launch file using the script generate_launchfile.sh

1. export CAMERA_CONFIGS="camera_namespace, camera_type, serial_n, tf_x, tf_y, tf_z, tf_yaw, tf_pitch, tf_roll" where camera_type is either realsense or struct_core_ros, tf_* represents the displacement between the camera and the flight controller. If more than one camera is present, list the different camera configuration separated by a semicolon. Within each camera configuration the parameters are separated by commas.
2. export DEPTH_CAMERA_FRAME_RATE=frame_rate. If this variable isn't set, the default frame rate will be taken.
3. export VEHICLE_CONFIG=/path/to/params.yaml where the yaml file contains the value of some parameters different from the defaults set in the cfg file. If this variable isn't set, the default parameters values will be used.

Changing the serial number and DEPTH_CAMERA_FRAME_RATE don't have any effect on the Structure Core.

For example:

export CAMERA_CONFIGS="camera_main,realsense,819612070807,0.3,0.32,-0.11,0,0,0"
export DEPTH_CAMERA_FRAME_RATE=30
export VEHICLE_CONFIG=~/catkin_ws/src/avoidance/local_planner/cfg/params_vehicle_1.yaml
./tools/generate_launchfile.sh
roslaunch local_planner avoidance.launch fcu_url:=/dev/ttyACM0:57600

where fcu_url representing the port connecting the companion computer to the flight controller. The planner is running correctly if the rate of the processed point cloud is around 10-20 Hz. To check the rate run:

rostopic hz /local_pointcloud

If you would like to read debug statements on the console, please change custom_rosconsole.conf to

log4j.logger.ros.local_planner=DEBUG

Safe Landing Planner

Once the catkin workspace has been built, to run the planner with a Realsense D435 and Occipital Structure Core, you can generate the launch file using the script generate_launchfile.sh. The script works the same as described in the section above for the Local Planner. For example:

export CAMERA_CONFIGS="camera_main,struct_core,819612070807,0.3,0.32,-0.11,0,0,0"
export VEHICLE_CONFIG_SLP=~/catkin_ws/src/avoidance/safe_landing_planner/cfg/slpn_structure_core.yaml
export VEHICLE_CONFIG_WPG=~/catkin_ws/src/avoidance/safe_landing_planner/cfg/wpgn_structure_core.yaml
./safe_landing_planner/tools/generate_launchfile.sh
roslaunch safe_landing_planner safe_landing_planner_launch.launch

In the cfg/ folder there are camera specific configurations for the algorithm nodes. These parameters can be loaded by specifying the file in the VEHICLE_CONFIG_SLP and VEHICLE_CONFIG_WPG system variable for the safe_landing_planner_node and for the waypoint_generator_node respectively.

The size of the squared shape patch of terrain below the vehicle that is evaluated by the algorithm can be changed to suit different vehicle sizes with the WaypointGeneratorNode parameter smoothing_land_cell. The algorithm behavior will also be affected by the height at which the decision to land or not is taken (loiter_height parameter in WaypointGeneratorNode) and by the size of neighborhood filter smoothing (smoothing_size in LandingSiteDetectionNode).

For different cameras you might also need to tune the thresholds on the number of points in each bin, standard deviation and mean.

Troubleshooting

I see the drone position in rviz (shown as a red arrow), but the world around is empty

Check that some camera topics (including /camera/depth/points) are published with the following command:

rostopic list | grep camera

If /camera/depth/points is the only one listed, it may be a sign that gazebo is not actually publishing data from the simulated depth camera. Verify this claim by running:

rostopic echo /camera/depth/points

When everything runs correctly, the previous command should show a lot of unreadable data in the terminal. If you don't receive any message, it probably means that gazebo is not publishing the camera data.

Check that the clock is being published by Gazebo:

rostopic echo /clock

If it is not, you have a problem with Gazebo (Did it finish loading the world? Do you see the buildings and the drone in the Gazebo UI?). However, if it is publishing the clock, then it might be a problem with the depth camera plugin. Make sure the package ros-kinetic-gazebo-ros-pkgs is installed. If not, install it and rebuild the Firmware (with $ make px4_sitl_default gazebo as explained above).

I see the drone and world in rviz, but the drone does not move when I set a new "2D Nav Goal"

Is the drone in OFFBOARD mode? Is it armed and flying?

# Set the drone to OFFBOARD mode
rosrun mavros mavsys mode -c OFFBOARD
# Arm
rosrun mavros mavsafety arm

I see the drone and world in rviz, but the drone does not follow the path properly

Some tuning may be required in the file "<Firmware_dir>/posix-configs/SITL/init/rcS_gazebo_iris".

I see the drone and world in rviz, I am in OFFBOARD mode, but the planner is still not working

Some parameters that can be tuned in rqt reconfigure.

Advanced

Message Flows

More information about the communication between avoidance system and the Autopilot can be found in the PX4 User Guide

PX4 and local planner

This is the complete message flow from PX4 Firmware to the local planner.

PX4 topic	MAVLink	MAVROS Plugin	ROS Msgs.	ROS Topic
vehicle_local_position	LOCAL_POSITION_NED	local_position	geometry_msgs::PoseStamped	mavros/local_position/pose
vehicle_local_position	LOCAL_POSITION_NED	local_position	geometry_msgs::TwistStamped	mavros/local_position/velocity
vehicle_local_position	ALTITUDE	altitude	mavros_msgs::Altitude	mavros/altitude
home_position	ALTITUDE	altitude	mavros_msgs::Altitude	mavros/altitude
vehicle_air_data	ALTITUDE	altitude	mavros_msgs::Altitude	mavros/altitude
vehicle_status	HEARTBEAT	sys_status	mavros_msgs::State	mavros/state
vehicle_trajectory_waypoint_desired	TRAJECTORY_REPRESENTATION_WAYPOINT	trajectory	mavros_msgs::Trajectory	mavros/trajectory/desired
none	MAVLINK_MSG_ID_PARAM_REQUEST_LIST	param	mavros_msgs::Param	/mavros/param/param_value
none	MISSION_ITEM	waypoint	mavros_msgs::WaypointList	/mavros/mission/waypoints

This is the complete message flow to PX4 Firmware from the local planner.

ROS topic	ROS Msgs.	MAVROS Plugin	MAVLink	PX4 Topic
/mavros/setpoint_position/local (offboard)	geometry_msgs::PoseStamped	setpoint_position	SET_POSITION_LOCAL_POSITION_NED	position_setpoint_triplet
/mavros/trajectory/generated (mission)	mavros_msgs::Trajectory	trajectory	TRAJECTORY_REPRESENTATION_WAYPOINT	vehicle_trajectory_waypoint
/mavros/obstacle/send	sensor_msgs::LaserScan	obstacle_distance	OBSTACLE_DISTANCE	obstacle_distance
/mavros/companion_process/status	mavros_msgs::CompanionProcessStatus	companion_process_status	HEARTBEAT	telemetry_status

PX4 and global planner

This is the complete message flow from PX4 Firmware to the global planner.

PX4 topic	MAVLink	MAVROS Plugin	ROS Msgs.	Topic
vehicle_local_position	LOCAL_POSITION_NED	local_position	geometry_msgs::PoseStamped	mavros/local_position/pose
vehicle_local_position	LOCAL_POSITION_NED	local_position	geometry_msgs::TwistStamped	mavros/local_position/velocity
vehicle_trajectory_waypoint_desired	TRAJECTORY_REPRESENTATION_WAYPOINT	trajectory	mavros_msgs::Trajectory	mavros/trajectory/desired

This is the complete message flow to PX4 Firmware from the global planner.

ROS topic	ROS Msgs.	MAVROS Plugin	MAVLink	PX4 Topic
/mavros/setpoint_position/local (offboard)	geometry_msgs::PoseStamped	setpoint_position	SET_POSITION_LOCAL_POSITION_NED	position_setpoint_triplet
/mavros/trajectory/generated (mission)	mavros_msgs::Trajectory	trajectory	TRAJECTORY_REPRESENTATION_WAYPOINT	vehicle_trajectory_waypoint

PX4 and safe landing planner

This is the complete message flow from PX4 Firmware to the safe landing planner.

PX4 topic	MAVLink	MAVROS Plugin	ROS Msgs.	ROS Topic
vehicle_local_position	LOCAL_POSITION_NED	local_position	geometry_msgs::PoseStamped	mavros/local_position/pose
vehicle_status	HEARTBEAT	sys_status	mavros_msgs::State	mavros/state
vehicle_trajectory_waypoint_desired	TRAJECTORY_REPRESENTATION_WAYPOINT	trajectory	mavros_msgs::Trajectory	mavros/trajectory/desired
none	MAVLINK_MSG_ID_PARAM_REQUEST_LIST	param	mavros_msgs::Param	/mavros/param/param_value

This is the complete message flow to PX4 Firmware from the safe landing planner.

ROS topic	ROS Msgs.	MAVROS Plugin	MAVLink	PX4 Topic
/mavros/trajectory/generated (mission)	mavros_msgs::Trajectory	trajectory	TRAJECTORY_REPRESENTATION_WAYPOINT	vehicle_trajectory_waypoint
/mavros/companion_process/status	mavros_msgs::CompanionProcessStatus	companion_process_status	HEARTBEAT	telemetry_status

Contributing

Fork the project and then clone your repository. Create a new branch off of master for your new feature or bug fix.

Please, take into consideration our coding style. For convenience, you can install the commit hooks which will run this formatting on every commit. To do so, run ./tools/set_up_commit_hooks from the main directory.

Commit your changes with informative commit messages, push your branch and open a new pull request. Please provide ROS bags and the Autopilot flight logs relevant to the changes you have made.

px4-avoidance's Issues

Update README according to new folders organization

Both planners are now split as two independent ROS packages (still in the same repo).

The READMEs need to be updated accordingly.

Unable to find PX4

I'm getting the following error while trying to launch the simulation.
$ roslaunch global_planner global_planner_sitl_mavros.launch

... logging to /home/rajat/.ros/log/5e6ec4d2-daa4-11e8-aa40-4cbb58b223ff/roslaunch-rajat-Vostro-3449-6976.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/__init__.py", line 306, in main
    p.start()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 268, in start
    self._start_infrastructure()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 217, in _start_infrastructure
    self._load_config()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/parent.py", line 132, in _load_config
    roslaunch_strs=self.roslaunch_strs, verbose=self.verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/config.py", line 451, in load_config_default
    loader.load(f, config, verbose=verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 749, in load
    self._load_launch(launch, ros_config, is_core=core, filename=filename, argv=argv, verbose=verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 721, in _load_launch
    self._recurse_load(ros_config, launch.childNodes, self.root_context, None, is_core, verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 685, in _recurse_load
    val = self._include_tag(tag, context, ros_config, default_machine, is_core, verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 95, in call
    return f(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 627, in _include_tag
    default_machine, is_core, verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 685, in _recurse_load
    val = self._include_tag(tag, context, ros_config, default_machine, is_core, verbose)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 95, in call
    return f(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 589, in _include_tag
    inc_filename = self.resolve_args(tag.attributes['file'].value, context)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/xmlloader.py", line 183, in resolve_args
    return substitution_args.resolve_args(args, context=context.resolve_dict, resolve_anon=self.resolve_anon)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/substitution_args.py", line 370, in resolve_args
    resolved = _resolve_args(resolved, context, resolve_anon, commands)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/substitution_args.py", line 383, in _resolve_args
    resolved = commands[command](resolved, a, args, context)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/substitution_args.py", line 151, in _find
    source_path_to_packages=source_path_to_packages)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/substitution_args.py", line 197, in _find_executable
    full_path = _get_executable_path(rp.get_path(args[0]), path)
  File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 203, in get_path
    raise ResourceNotFound(name, ros_paths=self._ros_paths)
ResourceNotFound: px4
ROS path [0]=/opt/ros/kinetic/share/ros
ROS path [1]=/home/rajat/catkin_ws/src/avoidance/global_planner
ROS path [2]=/home/rajat/catkin_ws/src/avoidance/local_planner
ROS path [3]=/opt/ros/kinetic/share

The simulation works when I run make posix_sitl_default gazebo

Flight test master indoors

Creating issue to start discussion on how we can test avoidance for indoors

Document simulation setup in PX4 dev guide

This would assume that the repo is in shape to be publishable.

Save bag files on the host in the docker production setup

As noted in #48, the bags are saved inside the docker container running the avoidance node and, therefore, are lost whenever the container is destroyed.

Find a way to keep those bags out of the containers (by mounting their containing folder as a volume, so that they are kept on the host).

runOdroid script global_planner

Hi people.

I am trying to run the runOdroid script from my host laptop to a Jetson TX2.

In the beginning of the scrip there is . ~/.bas_ros and when the scrip reach the lines to run the commands in separate tabs e.g. gnome-terminal --tab -e "ssh -t [email protected] 'sleep 0; . ~/.bash_ros; roscore; bash -i'" \

I get the error in the terminal bash: roscore: command not found.

My best guess is that it has to do with the line ". ~/.bas_ros"

Could some one help me :) thank you :)

Flight test master outdoors

@baumanta Please organise the flight testing and ask for support from @bkueng on logistics.

Link to instructions to run on odroid is broken

Hi.

The link that points to running the package on odroid is broken.
https://github.com/PX4/avoidance#running-on-odroid

Could you please update it?

Thanks.

[global_planner_sitl] Failed to find a path

Got this error while testing avoidance (global_planner) in simulator.
The drone does not reach its destination and stops midway. Attached are the logs and screenshots



path-not-found.txt

[local_planner_sitl] Failed to launch gazebo/rviz

Tried running below command -
$ roslaunch avoidance local_planner_sitl_mavros.launch
Gazebo is stuck displaying "Preparing your world" dialog.
local-planner-sitl.txt

cfg file does not take effect until clicking in rqt_reconfigure

As far as I can tell, the default parameters set in the global_planner_node.cfg file are completely ignored until the dynamicReconfigureCallback function is triggered by clicking in the rqt_reconfigure window. Specifically, since the CELL_SCALE is a "level 2" parameter, I have to actually click that specific parameter in rqt_reconfigure in order to latch my .cfg file's default value.

Is this expected behavior? It seems like it would make sense to trigger this callback immediately on launch so that the user's default params are latched in, instead of using the values set in the .h header files.

Can't start the gazebo launch

Dear author,
Thank you for your sharing firstly, however, When I start the launch "roslaunch global_planner global_planner_sitl_mavros.launch ", I got an error, it tells me that failed like the following picture.

Do you know how to deal with it.
And what is more, Can I use the gazebo groundtruth to replace gps so that I can simulator the indoor situation?
I am looking for your answer, Thank you very much!

Simulation

I am trying to run local planner on iris drone with r200 camera. I have changed the point cloud topic and it shows it in rviz but it does not move to the goal. If I give the goal through rviz, the goal's coordinates are shown on terminal but drone doesn't move

Split global and local planner

Both planners have different dependencies and should therefore be independant in terms of build system.

bezier.h:132:12: error: return-statement with a value, in function returning 'void' [-fpermissive]

I am trying to build your node in ROS Melodic on Ubuntu 18.04 and it fails with:
/home/arek/catkin_ws/src/avoidance/global_planner/include/global_planner/bezier.h:132:12: error: return-statement with a value, in function returning 'void' [-fpermissive] return path;
Problematic line:
https://github.com/PX4/avoidance/blob/acebb50ba6d7d0cb8dc9a99aeddc7b9e8469485a/global_planner/include/global_planner/bezier.h#L132

I have pushed a fix just to build it but I assume it's not how the code is intended to work. I will try to push more commits if I will make the whole package work and test it.

make px4_sitl_default gazebo fails to build

Hello,
I'm a beginner in px4, ROS, and Gazebo. My target is to deploy this program on jetson tx2. But before that, I want to try the offboard simulator on my desktop.
Followed the steps mentioned in this repository and installed all the dependencies.
OS: Ubuntu 16.04
ROS-Kinetic
Gazebo 9
After I do "make px4_sitl_default gazebo", it opens a new Gazebo window and freezes making the program at "EKF commencing GPS fusion". I got no idea where should I start from as I can't run the program unless I've built it successfully. Am I supposed to create obstacles in the Gazebo simulator myself and provide the file path to the program? Thanks in advance.

Fast take-off in mission mode

During our tests we found that the mission take-off is violently fast when the avoidance is running. This may have gone unnoticed for a long time because we usually take off in position mode before starting the mission.

Sadly, we didn't have logging from boot enabled, so it is not completely captured in the logs below. But we can see that without the avoidance, the beginning of the log still shows the end of a smooth ramp in the z-setpoint, while the log with the enabled OA starts at a 3m altitude (to which it got presumably through a step)

No avoidance:

Avoidance enabled:

[Err] [RTShaderSystem.cc:489] Unable to find shader lib. Shader generating will fail

when I run make px4_sitl_default gazebo

local_planner_stereo.launch fails to run

Directly attempting to run this launch file resulted in an error in looking up the px4 launch file, which makes sense because there is no px4.launch file in the PX4 firmware folder.

So I figure the launch files are probably just a little out of date and I need to update some directories. I change it to look for px4.launch in mavros, where it does find it, but it then complains about multiple instances of mavros running. Indeed, px4.launch does start its own mavros node, so I tried to consolidate all of the launch files into a single file to try running the software
local_planner_stereo.txt.

I believe this has the correct configuration, but I now run into strange errors from the parser:

Error [parser.cc:689] Unable to find uri[model://iris_depth_camera]

<gravity>0 0 -9.8</gravity>

<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>

<atmosphere type='adiabatic'/>

<physics name='default_physics' default='0' type='ode'>

  <max_step_size>0.001</max_step_size>

  <real_time_factor>1</real_time_factor>

  <real_time_update_rate>1000</real_time_update_rate>

</physics>

<scene>

  <ambient>0.4 0.4 0.4 1</ambient>

  <background>0.7 0.7 0.7 1</background>

  <shadows>1</shadows>

</scene>

[INFO] [1536011813.196405, 10.104000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name iris

[spawn_model-11] process has finished cleanly

log file: /home/chuples/.ros/log/1c59b90e-afc4-11e8-a497-dcfe07d69cab/spawn_model-11*.log

^C[gazebo_gui-10] killing on exit

[gazebo-9] killing on exit

[local_planner_node-6] killing on exit

[rqt_reconfigure-7] killing on exit

[rviz-4] killing on exit

[mavros-8] killing on exit

[stereo/stereo_image_proc-5] killing on exit

[tf_90_deg-2] killing on exit

[tf_depth_camera-3] killing on exit

Warning: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty

     at line 126 in /tmp/binarydeb/ros-kinetic-tf2-0.5.17/src/buffer_core.cpp

[local_planner_node-6] escalating to SIGTERM

[gazebo-9] escalating to SIGTERM

[rosout-1] killing on exit

[master] killing on exit

shutting down processing monitor...

... shutting down processing monitor complete

Unable to build local_planner

Build of `local_planner' fails due to the following error

In file included from /home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node.cpp:1:0:
/home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node.h:18:48: fatal error: mavros_msgs/CompanionProcessStatus.h: No such file or directory

The entire output is as follows

$ catkin build -w ~/catkin_ws
-----------------------------------------------------------------
Profile:                     default
Extending:             [env] /opt/ros/kinetic
Workspace:                   /home/docker-rajat/catkin_ws
-----------------------------------------------------------------
Source Space:       [exists] /home/docker-rajat/catkin_ws/src
Log Space:         [missing] /home/docker-rajat/catkin_ws/logs
Build Space:        [exists] /home/docker-rajat/catkin_ws/build
Devel Space:        [exists] /home/docker-rajat/catkin_ws/devel
Install Space:      [unused] /home/docker-rajat/catkin_ws/install
DESTDIR:            [unused] None
-----------------------------------------------------------------
Devel Space Layout:          linked
Install Space Layout:        None
-----------------------------------------------------------------
Additional CMake Args:       None
Additional Make Args:        None
Additional catkin Make Args: None
Internal Make Job Server:    True
Cache Job Environments:      False
-----------------------------------------------------------------
Whitelisted Packages:        None
Blacklisted Packages:        None
-----------------------------------------------------------------
Workspace configuration appears valid.

NOTE: Forcing CMake to run for each package.
-----------------------------------------------------------------
[build] Found '2' packages in 0.0 seconds.                                                                                                    
[build] Updating package table.                                                                                                               
Starting  >>> catkin_tools_prebuild                                                                                                           
Finished  <<< catkin_tools_prebuild                [ 7.0 seconds ]                                                                            
Starting  >>> global_planner                                                                                                                  
Starting  >>> local_planner                                                                                                                   
______________________________________________________________________________________________________________________________________________
Warnings   << local_planner:cmake /home/docker-rajat/catkin_ws/logs/local_planner/build.cmake.000.log                                         
** WARNING ** io features related to openni2 will be disabled
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
** WARNING ** visualization features related to openni2 will be disabled
cd /home/docker-rajat/catkin_ws/build/local_planner; catkin build --get-env local_planner | catkin env -si  /usr/bin/cmake /home/docker-rajat/catkin_ws/src/avoidance/local_planner --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/docker-rajat/catkin_ws/devel/.private/local_planner -DCMAKE_INSTALL_PREFIX=/home/docker-rajat/catkin_ws/install; cd -
..............................................................................................................................................
______________________________________________________________________________________________________________________________________________
Warnings   << global_planner:cmake /home/docker-rajat/catkin_ws/logs/global_planner/build.cmake.000.log                                       
** WARNING ** io features related to openni2 will be disabled
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
** WARNING ** visualization features related to openni2 will be disabled
cd /home/docker-rajat/catkin_ws/build/global_planner; catkin build --get-env global_planner | catkin env -si  /usr/bin/cmake /home/docker-rajat/catkin_ws/src/avoidance/global_planner --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/docker-rajat/catkin_ws/devel/.private/global_planner -DCMAKE_INSTALL_PREFIX=/home/docker-rajat/catkin_ws/install; cd -
..............................................................................................................................................
Finished  <<< global_planner                       [ 11 minutes and 14.2 seconds ]                                                            
______________________________________________________________________________________________________________________________________________
Errors     << local_planner:make /home/docker-rajat/catkin_ws/logs/local_planner/build.make.000.log                                           
In file included from /home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node.cpp:1:0:
/home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node.h:18:48: fatal error: mavros_msgs/CompanionProcessStatus.h: No such file or directory
In file included from /home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node_main.cpp:1:0:
/home/docker-rajat/catkin_ws/src/avoidance/local_planner/src/nodes/local_planner_node.h:18:48: fatal error: mavros_msgs/CompanionProcessStatus.h: No such file or directory
compilation terminated.
compilation terminated.
make[2]: *** [CMakeFiles/local_planner_node.dir/src/nodes/local_planner_node.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/local_planner_node.dir/src/nodes/local_planner_node_main.cpp.o] Error 1
make[1]: *** [CMakeFiles/local_planner_node.dir/all] Error 2
make: *** [all] Error 2
cd /home/docker-rajat/catkin_ws/build/local_planner; catkin build --get-env local_planner | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -
..............................................................................................................................................
Failed     << local_planner:make                   [ Exited with code 2 ]                                                                     
Failed    <<< local_planner                        [ 11 minutes and 49.5 seconds ]                                                            
[build] Summary: 2 of 3 packages succeeded.                                                                                                   
[build]   Ignored:   None.                                                                                                                    
[build]   Warnings:  2 packages succeeded with warnings.                                                                                      
[build]   Abandoned: None.                                                                                                                    
[build]   Failed:    1 packages failed.                                                                                                       
[build] Runtime: 11 minutes and 58.2 seconds total.                                                                                           
[build] Note: Workspace packages have changed, please re-source setup files to use them.

Operating System is Ubuntu 16.04 with ROS Kinetic

Mavros version: Version: 0.27.0-0xenial-20181116-184226-0800
Mavlink version: Version: 2018.11.11-0xenial-20181113-004213-0800

local_planner_stereo.launch problem

I have made avoidance simulation work on the Ubuntu 18.04 (everything worked fine but I had to build octomap_mapping and octomap_ros).

The only problem is that I cannot make local_planner_stereo.launch to start, this may be a problem:

[FATAL] []: ODOM: invalid body frame orientation "frd"
[ WARN] []: MSG to TF: Quaternion Not Properly Normalized
[ INFO] []:  Pointcloud timeout: Hovering at current position 

and

The strange part is that local_planner_depth-camera.lauch and global_planner work perfectly. Do you know what's the problem?

Whole log: gist

global_planer

Dear people

I have few questions about the global_planner

I mention them here 😊

1.- I guess that for mapping a SLAM is used, right? Just wondering what type of SLAM is used or a reference to the article would be great 😊

2.- What kind of path planning method is used?

3.- And how about the control control strategy, is a PID control strategy?

Thank you and wishing all the good.

Error when running "rosrun mavros mavsafety arm"

As the title says, when I run this command, something goes wrong.The error message is as follows

Traceback (most recent call last):
File "/opt/ros/kinetic/lib/mavros/mavsafety", line 18, in
from mavros import command
File "/opt/ros/kinetic/lib/python2.7/dist-packages/mavros/command.py", line 23, in
from mavros_msgs.srv import CommandLong, CommandInt, CommandBool, CommandHome, CommandTOL, CommandTriggerControl, CommandTriggerInterval
ImportError: cannot import name CommandTriggerInterval

The operating system I am using is ubuntu16.04."rosrun mavros mavsys mode -c OFFBOARD" can execute successfully.The simulation interface can also display normally.

Check usage of std::clock()

std::clock measures the processor time and is in many cases not what you want.

For example it's used to calculate dt in https://github.com/PX4/avoidance/blob/master/local_planner/src/nodes/waypoint_generator.cpp#L243.
This is not correct and std::chrono::high_resolution_clock::now should be used instead.

std::clock is used at several places and all of them need to be validated.

README update

PR #51 removes AurelienRoy gazebo models. The README file needs to be updated accordingly

[FEATURE] dis-/enable offboard control

Hey,

would be nice to have some handshake to control the drone with avoidance for a specific manner and to get back the offboard control when i need it.

i will do it with a multiplexer node for the meantime.

best regards
tobi

Global planner yaw goal

As far as I can tell, when publishing to the /move_base_simple/goal topic, the global planner completely ignores the orientation of the PoseStamped object. Is there a way to set the orientation of the drone in the global planner?

OA: in manual mode ("collision avoidance")

Goal: in manual mode obstacle avoidance is used to avoid crashing into obstacles.
Intended behavior: not full avoidance of obstacle is needed, main goal is to avoid a crash.

• Ideas:
• 1. just stop when flight path into obstacle is detected. Might feel bad to the user because drone does not move even though he provides stick input
• 2. bounce back (idea by Jonathan): when you push sticks forward into an obstacle bounce back slightly (like the browser on an iphone when you scroll further up at the top of a page)

Use case: inspection

Algorithm Selection

Hi,

The PX4/avoidance uses the 3DVFH+ algorithm for obstacle avoidance while navigating.
This algorithm is based on a octomap (occupancy grid - octree), which is widely used in mapping, mainly because of its simplicity and low memory storage.

But recently the Voxblox algorithm for obstacle avoidance have been published and released with open-source ROS implementation.
This algorithm is based on Signed Distance Field (SDF) and is shown that generating this SDF is faster than constructing the octree, and then navigation algorithm are easier to do on SDF than on octree.

What do you think? Why did you choose 3DVFH+ instead of Voxblox?

Many thanks,
Alexandre

[Feature] Plug OMPL to Avoidance to open the possibility of using other motion planning algorithms

OMPL (Open Motion Planning Library) is a library which contains most of the state-of-the-art motion and path planning algorithms. It's already used in various robotics frameworks, including MoveIt!. Would be interesting to explore the possibility of integrate it with the Avoidance API in order to add other alternatives to the global planner.
Note also that the license is permissive and should set us good to go on the integration.
I am open to discuss this on the next avoidance call.
@mrivi, @baumanta, @nicovanduijn what do you think?

Timeouts

I get timeouts messages Pointcloud timeout , no cloud received on topic (Hovering at current position) both in the simulation and on hardware.

I have already spoken offline with @baumanta. As she suspected, the timeout happens because the transformation between the camera frame and local origin is not available.

Opening a issue so that we track.

R200 parameters for outdoor

Hi I am currently testing the avoidance pkg in an outdoor setup. I am using the R200 dvkit. I suspect that the camera is not tuned for outdoor operations. Do you have recommended R200 parameters for outdoor operations?
I am using the global planet in the master branch. It always reports that the goal position is occupied while it is actually not.

Thanks

Questions regarding planners

I've running avoidance simulations of both planners

I found that global planner works great and capable of real-time mapping and 3D path planning with rough scaled octomap and the path smoothing.
Also, planning may fail but it mostly keep drone from collisions
so the result in gazebo simulation is very desirable and satisfactory.

Whereas the local planner, It usually finds way in 2D, slowly progresses and often collide into trees or terrains and sometimes it goes around trees like forever due to it does not record previous point clouds
and computation in my desktop is also heavy (probably due to lots of visualization in RViz)

I wonder why only local planner is available for running on hardware such as Intel Aero RTF
and dev team focuses are in local planner, not the global planner

Thanks in advance.

Problems in connections with Gazebo Simulator

As I follow instruction on this repo.
I came to find some problems with connections.

1. Problem with the connection with PX4 SITL with Gazebo Simulation
   It was very easy to miss the problem that no error or warning messages are given.
   After information message shown,
   INFO [simulator] Waiting for simulator to connect on TCP port 4560
   and no update since
   unlike basic px4 Gazebo simulation
   make px4_sitl_default gazebo
   It does find connection with Gazebo right away and does work with additional mavros launch.

INFO  [simulator] Waiting for simulator to connect on TCP port 4560
INFO  [simulator] Simulator connected on TCP port 4560.

2. Problem with the connection with MAVROS.
   I think it's cause of root is problem 1,
   MAVROS diagnostics report as it is not connected to FCU, thus nothing can be done.

Hope this could be solved soon.
Thanks.

Fix deployment scripts

Following a debugging session with @SaliniVenate, a few points need to be fixed in the deployment scripts:

• tty:true seems to cause log issues.
• The run.sh output for "prod-debug" should mention that there should be one terminal for the VPN client and one for ROS
• The run.sh output was showing the wrong ip for the vpn route (172.20.255.255 instead of 172.18.255.255 for @SaliniVenate)

Global_planner pixhawk estimated pose

Dear people.

I am on the way thinking about implementing the global planner in real.

I can not figure out where in the mavros launch file is specified the transformation between NED->ENU conversion.

In other words, I would like to set up in my own mavros launch file the coordinates NED-Pixhawk aligned or transformed with the coordinates ENU-ROS.

Any help I appreciate it.

Thank you.

How to use the PX4 avoidance with AirSim?

I want to use the PX4 avoidance with AirSim (https://github.com/Microsoft/AirSim) instead of the Gazebo. But I can't find any document to setup it with AirSim, Can anyone give me an advice?

Readme suggestions

On quick scan I think I could use this readme to set up avoidance on simulation, but I doubt it would be possible to use it for working with real hardware.

Some possible improvements:

1. There is no index/TOC for the contents on github, so you have to read the whole contents to find out what is in the doc. Either add a note in introduction explaining what is covered (setting up the planners in simulation and on hardware - with links) or provide a TOC (which is what I would do).
2. Explain the difference between the local and global planner both in terms of what they do (algorithm) and what they should be used for. @baumanta provided starting point text for this in another issue:

   the local planner works only with the information it currently gets from the sensors and plans a path that is around a meter long. When the next set of information come in it plans a new piece of the path. The global planner, on the other hand, build a map of the environment. It gathers all the information ever received and then plans a path that reaches to the goal. This happens at a much lower frequency than in the local_planner. Also the global planner is much more costly on the computational side.
   And yes, the collision avoidance works only with the local planner. The local planner internally build a histogram where the free and blocked cells are marked. This is then compressed into 1D to serve as a laser scan emulation. The global planner does not use such a histogram.

3. The running on hardware docs should:
   • cover local_planner.
   • Explain minimal hardware dependencies - ie what camera or "class of camera" do you need? How do you attach it and hook it up to ROS. How must it be mounted. Cover for both types of planners and list the actual cameras used for testing this.
4. You refer to the rqt_reconfigure GUI - but it isn't covered elsewhere and nowhere do you explain what it is for (generally), how you start it, where it is located, what software delivers it (ie is it part of this repo?), and what each of the settings are for - e.g. "send_obstacles_fcu"
   • This should have its own section or possibly document that is linked to.

Docker demo fails to build

docker demo fails to build using docker-compose up

 ---> Running in fe0c65ad0217
Executing: /tmp/tmp.qUUyo5HYwi/gpg.1.sh --keyserver
hkp://ha.pool.sks-keyservers.net:80
--recv-key
421C365BD9FF1F717815A3895523BAEEB01FA116
gpg: requesting key B01FA116 from hkp server ha.pool.sks-keyservers.net
gpg: keyserver timed out
gpg: keyserver receive failed: keyserver error
ERROR: Service 'ubuntu-avoidance' failed to build: The command '/bin/sh -c echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list &&     apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 &&     apt-get update' returned a non-zero code: 2

problem with docker git

submodule update fails for me in docker for libuavcan with

Unable to checkout 'e1c9a4f5064a336295125da25c0542a5cd601dd7' in submodule path 'src/modules/uavcan/libuavcan'

looks like git version issue.

if I add this

# Install latest git to avoid 
# `Unable to checkout 'e1c9a4f5064a336295125da25c0542a5cd601dd7' in submodule path 'src/modules/uavcan/libuavcan'`
RUN apt-get install -y software-properties-common && \
    add-apt-repository ppa:git-core/ppa && \
    apt-get update && \
    apt-get install -y git

into dockerfile before checking out Firmware it works.

Integrate demo changes into master

Some lessons have been learned from the local_planner demo. A few values were hard-coded for the sake of the demo, and should be merged into master.

Refactor the demo branch and merge what needs to be kept into master.

NOTE: this issue is meant to address PR #10.

Documentation

Hi
I want to use this package with different cameras (other than realsense) and I want to be able to have full control on parameters and topics, however I cant find proper documnetation about the nature of the parameters and topics.
Can someone help :)

docker-compose installation instructions should be updated

According to the readme in the docker folder, the docker-compose installation is instructed to be done as

apt install docker-compose

However, the docker-compose installed using apt is 1.8.1 which is outdated.
This produces an error as the docker-compose.yaml uses version: '3' which is not compatible with 1.8.1

A better approach is to install the docker-compose using the instructions provided here

Tests fail to build on master`

I'm guessing since 94f5c03 the tests fail to build.

/home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp: In member function ‘virtual void PlannerFunctions_testDirectionTree_Test::TestBody()’:
/home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:617:66: error: invalid initialization of reference of type ‘avoidance::PolarPoint&’ from expression of type ‘Eigen::Vector3f {aka Eigen::Matrix<float, 3, 1>}’
   bool res = getDirectionFromTree(p, path_node_positions, postion);
                                                                  ^
In file included from /home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:4:0:
/home/nico/catkin_ws/src/avoidance/local_planner/test/../src/nodes/planner_functions.h:73:6: note: in passing argument 1 of ‘bool avoidance::getDirectionFromTree(avoidance::PolarPoint&, const std::vector<geometry_msgs::Point_<std::allocator<void> > >&, const Vector3f&)’
 bool getDirectionFromTree(
      ^
/home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:618:69: error: invalid initialization of reference of type ‘avoidance::PolarPoint&’ from expression of type ‘Eigen::Vector3f {aka Eigen::Matrix<float, 3, 1>}’
   bool res1 = getDirectionFromTree(p1, path_node_positions, postion1);
                                                                     ^
In file included from /home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:4:0:
/home/nico/catkin_ws/src/avoidance/local_planner/test/../src/nodes/planner_functions.h:73:6: note: in passing argument 1 of ‘bool avoidance::getDirectionFromTree(avoidance::PolarPoint&, const std::vector<geometry_msgs::Point_<std::allocator<void> > >&, const Vector3f&)’
 bool getDirectionFromTree(
      ^
/home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:619:69: error: invalid initialization of reference of type ‘avoidance::PolarPoint&’ from expression of type ‘Eigen::Vector3f {aka Eigen::Matrix<float, 3, 1>}’
   bool res2 = getDirectionFromTree(p2, path_node_positions, postion2);
                                                                     ^
In file included from /home/nico/catkin_ws/src/avoidance/local_planner/test/test_planner_functions.cpp:4:0:
/home/nico/catkin_ws/src/avoidance/local_planner/test/../src/nodes/planner_functions.h:73:6: note: in passing argument 1 of ‘bool avoidance::getDirectionFromTree(avoidance::PolarPoint&, const std::vector<geometry_msgs::Point_<std::allocator<void> > >&, const Vector3f&)’
 bool getDirectionFromTree(
      ^
CMakeFiles/local_planner-test.dir/build.make:158: recipe for target 'CMakeFiles/local_planner-test.dir/test/test_planner_functions.cpp.o' failed

Not sure why travis doesn't see this problem

Memory issues: check dynamic data structures

While looking through the code I came accross https://github.com/PX4/avoidance/blob/master/local_planner/src/nodes/local_planner.cpp#L208 and https://github.com/PX4/avoidance/blob/master/local_planner/src/nodes/local_planner_node.cpp#L894.
Both instances increase an std::vector at each iteration but the size is not bounded, and nowhere elements are removed. This means the process will run out of memory when running long enough (it will take a long time though).

I suggest to check all dynamic data structures used in the entire project, as these 2 instances were the only ones that I checked.

Build issue related to yaml-cpp

local_planner fails to build with the following error

Errors     << local_planner:make /home/jalim/auterion_ws/logs/local_planner/build.make.000.log                                                                                                                                                                                                                                
/home/jalim/auterion_ws/devel/.private/local_planner/lib/liblocal_planner.so: undefined reference to `YAML::detail::node_data::empty_scalar[abi:cxx11]'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/jalim/auterion_ws/devel/.private/local_planner/lib/local_planner/local_planner_node] Error 1
make[1]: *** [CMakeFiles/local_planner_node.dir/all] Error 2
make: *** [all] Error 2
cd /home/jalim/auterion_ws/build/local_planner; catkin build --get-env local_planner | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

This seems to be related to yaml-cpp used for parsing yaml files for the rviz environment. Is there a specific requirement for yaml-cpp?
I have tried building building yaml-cpp from source at the yaml-cpp repo as well as installing with ros-kinetic-yaml-*

Could it be more desirable to have this contained within the workspace by using something like yaml-cpp_catkin ?

problems on run the simulation

When I run roslaunch local_planner local_planner_stereo.launch, there will be a problem

gazebo_ros_pkgs/gazebo_ros/scripts/gzserver -e ode /home/h/catkin_ws/src/avoidance/local_planner/../sim/worlds/simple_obstacle.world.

but when I change the world to empty.world of PX4 Firmware, no error.
Is it because of the version of gazebo or gazebo_ros_packages different ?
Thanks in advance.

OA in AUTO.MISSION mode

I have a question that: Does obstacle avoidance work in AUTO.MISSION mode?

Add depth_camera config into local planner

The #10 Demo branch is creating a new config for the depth camera and doing some fine-tuning (addressed in #13).

Merge the depth camera config into master.

Launch failing on 16.04 machine

@vilhjalmur89 The launch is failing for me with this error message:

[INFO] [WallTime: 1470245012.383872] [0.000000] Calling service /gazebo/spawn_sdf_model
[INFO] [WallTime: 1470245012.432271] [786.962000] Spawn status: SpawnModel: Model pushed to spawn queue, but spawn service timed out waiting for model to appear in simulation under the name iris_depth_camera
QInotifyFileSystemWatcherEngine::addPaths: inotify_add_watch failed: No space left on device
QFileSystemWatcher: failed to add paths: /home/pixhawk/.gazebo/models
QInotifyFileSystemWatcherEngine::addPaths: inotify_add_watch failed: No space left on device
QFileSystemWatcher: failed to add paths: /home/pixhawk/catkin_ws/src/Firmware/Tools/sitl_gazebo/models
QInotifyFileSystemWatcherEngine::addPaths: inotify_add_watch failed: No space left on device
QFileSystemWatcher: failed to add paths: /home/pixhawk/catkin_ws/src/detection/models
[spawn_model-8] process has finished cleanly