deepak-1530 / fastplanneroctomap

Obstacle avoidance using RGBD Camera and PX4-Autopilot firmware.

FastPlannerOctomap

Obstacle avoidance for drones using FastPlanner (https://github.com/HKUST-Aerial-Robotics/Fast-Planner), PX4 and Octomap.

Modules

---

Mapping

• Mapping has been done using Octomap

• Fixed size euclidean distance map is computed using EDT3D Library (https://github.com/OctoMap/octomap/tree/devel/dynamicEDT3D) for collision check.

• Fixed Size EDT map gets updated around the drone after it travels a threshold distance from the previous location at which map was updated (here threshold is 1 metres).

• Point cloud from the RGBD sensor (RealSense D415 and D455 were tested) is downsampled using pcl_ros

• Zero mean Gaussian noise is added on the point cloud using pcl library

• Installing mapping dependencies

  sudo apt-get install ros-$DISTRO-octomap-*  
  sudo apt-get install ros-$DISTRO-pcl-ros
  
  git clone https://github.com/OctoMap/octomap 
         (Octomap source code is needed to generate EDT map (DynamicEDT3D class))
  
  cd octomap
  
  mkdir build
  
  cd build
  
  cmake .. && make
  
  sudo make install
  

Planning

• Path planning is done using Fast-Planner (https://github.com/HKUST-Aerial-Robotics/Fast-Planner) developed by HKUST Aerial Robotics Group
• Changes have been made in the source code in order to use it with Octomap.
• Goal location is currently given in cartesian coordinates (using RViZ 2D Nav Goal and height of the goal is taken from the user)

Control

• PX4-Autopilot firmware has been used to control the drone (https://github.com/PX4/PX4-Autopilot).

• Trajectory generated by the planner is traversed using offboard position control.

• Position messages (with and without yaw) is published on the topic /mavros/setpoint_local/position.

• Mavros has been used to pass the waypoints to the Pixhawk flight controller.

• Installing PX4-Autopilot

   Refer to https://github.com/PX4/PX4-Autopilot for setting up PX4 for simulation
  

• Installing Mavros

  sudo apt-get install ros-$DISTRO-mavros ros-$DISTRO-mavros-extras
  
  wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
  
  sudo bash ./install_geographiclib_datasets.sh   
  

Localization

• GPS is used for localizing the drone
• Poses given by the EKF in PX4 is used (/mavros/local_position/pose)

|

---

Install and Run (Tested in ros melodic)

• Building package

  sudo apt-get install libeigen3-dev
  
  mkdir catkin_ws
  cd catkin_ws
  mkdir src   
  cd src
  git clone https://github.com/deepak-1530/FastPlannerOctomap
  cd ..
  catkin_make
  

Running the Planner

• Simulation

    Terminal-1 : cd PX4-Autopilot && sudo no_sim=1 make px4_sitl_gazebo
  
    Terminal-2 : cd PX4-Autpilot && source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default && roslaunch gazebo_ros empty_world.launch (set your world file as required). On the gazebo window, select iris_depth_camera from the left panel.
  
    Terminal-3 : cd catkin_ws && **roslaunch FastPlannerOctomap MappingSim.launch** (give goal location using 2D Nav Goal option)
  
    Terminal-4 : **rosrun FastPlannerOctomap Planner** (or noYawPlanner if you want to plan the trajectory keeping the heading or yaw of the drone fixed). For the startOver option select either 1 or 0. Refer to the source code (FastPlannerOctomap/src/kinodynamic_astar.cpp and Planner.cpp for details). Also give the height (in metres) of the goal location when prompted.
  
    Terminal-5 : **rosrun FastPlannerOctomap Controller**
  

• Running on hardware

    Terminal-1 : Launch the depth camera (I used realsense_ros package and rs_camera.launch file)
  
    Terminal-2 : roslaunch mavros px4.launch 
  
    Terminal-3 : roslaunch FastPlannerOctomap MappingDrone.launch
  
                 Running the planner and controller remain the same as in simulation.
  
                 Remote Desktop is used to run rviz and give the goal location.
  

fastplanneroctomap's Issues

how can i connect my realsense depth camera with fastplanneroctomap

hello

thank you for your nice repository.

I have built a simulation with ardupilot and it works very well with your repositories.

Now I'm trying to get it to work with my realsense d435 or d455. But it doesn't work.

I checked that the data of depth/image_raw is received well, but still no voxel grid or point cloud is created.

Could you possibly show the rs_camera.launch file you used?

thanks!

It continues to fail with warnings in the arminig part.

Hi !!

In controller.cpp, there is a command to arminig after activating offboard mode when a trajectory is entered. However, when the trajectory is entered, arming fails with the following messages. mode also does not change. I really want to run your code. Please help.

INFO [commander] Armed by external command
INFO [tone_alarm] arming warning
WARN [commander] Failsafe enabled: no offboard
INFO [commander] Failsafe mode activated
INFO [tone_alarm] battery warning (fast)
INFO [commander] Failsafe mode deactivated
WARN [commander] Failsafe enabled: no offboard
INFO [commander] Failsafe mode activated
INFO [tone_alarm] battery warning (fast)
INFO [commander] Failsafe mode deactivated
INFO [commander] Disarmed by auto preflight disarming

ref) my recent video : https://youtu.be/V9D7GrUd87Q

there no bounds limit for map?

hi!!

I am setting avoidance and path through fast planner. But I think using octomap is lighter.

By using octomap is there no bounds limit for map? The fast planner has map size limitations.

thanks

Fails to build a trajectory everytime...

Hey @deepak-1530,

Thanks a lot for this great repo, it's highly useful and valuable. I am facing an issue every time I am trying to build a trajectory. I am running it on Jetson Ubuntu 20 with ROS Noetic (ROS 1) and the sensor is ZED Mini. (I also tried using laptop - same issue).

I changed the MappingDrone.launch file:

<launch>
  <!-- transform from local_origin_ned to map-->
  <node pkg="tf" type="static_transform_publisher" name="mapTF_broadcaster" args="0 0 0 0 0 0 1 local_origin_ned map 100" />

  <!-- conversion from camera frame to base link frame (align camera X-Y-Z axes with that of Mavros)-->
  <node pkg="tf" type="static_transform_publisher" name="camTF_broadcaster" args="0.1 0 0 0 0 0  base_link zed_base_link 100" />

  <!-- Publish camera pose in world frame (read from EKF pose estimates and published over TF for octomap generation)-->
  <node pkg="FastPlannerOctomap" type="pubCamPose" name="pubCamPose"/>

  <!--mention the octomap launch commands-->
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <param name="resolution" value="0.10" />
    
    <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
    <param name="frame_id" type="string" value="map" />
    
    <!-- maximum range to integrate (speedup!) -->
    <param name="sensor_model/max_range" value="15" />
    
    <!-- data source to integrate (PointCloud2) -->
    <remap from="cloud_in" to="zed_node/point_cloud/cloud_registered" />
  
</node>
</launch>

I changed camera_link to zedm_base_link because I use camera as a visual localization (link). I also changed the localization in my PX4 (link).
Also, I made changes to Planner to subscribe to zed_node/pose for pos, instead of mavros node. I am moving camera and my odometry position is changing and 3D map accordingly. Am I doing something wrong?

Next steps:

1. launch zed-camera node
2. launch mapping node
3. launch px4 node
4. run Planner

I have next outcome (last few lines of code):

(3.85 3.05 3.65)
(3.95 3.15 3.75)
(3.85 3.25 3.45)
(3.85 3.35 3.55)
(3.85 3.25 3.65)
(3.95 3.35 3.65)
(3.85 3.35 3.75)
(3.95 3.45 3.75)
(4.05 3.25 3.75)
(4.05 3.45 3.85)
Here
Here
Here 63
Collision (0.5032 0.0032 0.0032) -- 0.640312
Collision (0.505 0.005 0.005) -- 0.640312
Collision (0.5072 0.0072 0.0072) -- 0.640312
Collision (0.5098 0.0098 0.0098) -- 0.640312
Collision (0.5128 0.0128 0.0128) -- 0.640312
open set empty, no path!
use node num: 1
iter num: 1
Planner output status is >>>>> 3

No trajectory found ...
Trying again ...

I have a problem running px4.

Can you tell me which version of px4 you used?

SITL COMMAND: "/home/cho/PX4-Autopilot/build/px4_sitl_default/bin/px4" "/home/cho/PX4-Autopilot/build/px4_sitl_default"/etc -s etc/init.d-posix/rcS -t "/home/cho/PX4-Autopilot"/test_data
Creating symlink /home/cho/PX4-Autopilot/build/px4_sitl_default/etc -> /home/cho/PX4-Autopilot/build/px4_sitl_default/t

�[0______ __ __ ___
| ___ \ \ \ / / / |
| |/ / \ V / / /| |
| __/ / \ / /| |
| | / /^\ \ ___ |
_| / / |_/

px4 starting.

INFO [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
INFO [init] found model autostart file as SYS_AUTOSTART=10016
INFO [param] selected parameter default file eeprom/parameters_10016
INFO [parameters] BSON document size 291 bytes, decoded 291 bytes (INT32:12, FLOAT:3)
[param] Loaded: eeprom/parameters_10016
INFO [dataman] data manager file './dataman' size is 7866640 bytes
PX4 SIM HOST: localhost
INFO [simulator] Waiting for simulator to accept connection on TCP port 4560

My px4 is not connected.

thanks