Perception-Aware Trajectory Planner in Dynamic Environments

Overview

PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments

PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments
PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments

Citation

When using PANTHER, please cite PANTHER: Perception-Aware Trajectory Planner in Dynamic Environments (pdf, video):

@article{tordesillas2020panther,
  title={{PANTHER}: Perception-Aware Trajectory Planner in Dynamic Environments},
  author={Tordesillas, Jesus and How, Jonathan P},
  journal={arXiv preprint},
  year={2021}
}

If you are looking for a Planner

  • In Multi-Agent and Dynamic Environments, you may be interesed also in MADER (pdf, video):
  • In Static Unknown environments, you may be interesed also in FASTER (pdf, video)

General Setup

PANTHER has been tested with Ubuntu 18.04/ROS Melodic. Other Ubuntu/ROS version may need some minor modifications, feel free to create an issue if you have any problems.

You can use PANTHER with only open-source packages. Matlab is only needed if you want to introduce modifications to the optimization problem.

Dependencies

CGAL

These commands will install CGAL v4.12.4:

sudo apt-get install libgmp3-dev libmpfr-dev -y
mkdir -p ~/installations/cgal && cd ~/installations/cgal
wget https://github.com/CGAL/cgal/releases/download/releases%2FCGAL-4.14.2/CGAL-4.14.2.tar.xz
tar -xf CGAL-4.14.2.tar.xz && cd CGAL-4.14.2/ && cmake . -DCMAKE_BUILD_TYPE=Release && sudo make install

CasADi and IPOPT

Install CasADi from source (see this for more details) and the solver IPOPT:

sudo apt-get install gcc g++ gfortran git cmake liblapack-dev pkg-config --install-recommends
sudo apt-get install swig
sudo apt-get install coinor-libipopt-dev
cd ~/installations #Or any other folder of your choice
git clone https://github.com/casadi/casadi.git -b master casadi
cd casadi && mkdir build && cd build
cmake . -DCMAKE_BUILD_TYPE=Release -DWITH_PYTHON=ON -DWITH_IPOPT=ON .. 
sudo make install
Optional (recommended for better performance)

To achieve better performance, you can use other linear solvers for Ipopt (instead of the default mumps solver). Specifically, we found that MA27 and MA57 are usually faster than the default mumps solver.

Go to http://www.hsl.rl.ac.uk/ipopt/, and then

  • If you want the solver MA57 (or MA27, or both), click on Coin-HSL Full (Stable) Source. This is free for academia.
  • If you only want the solver MA27, click on Personal Licence, Source. This is free for everyone

And fill and submit the form. Then download the compressed file from the link of the email you receive. Uncompress that file, and place it in a folder ~/installations (for example). Then execute the following commands:

Note: the instructions below follow this closely

cd ~/installations/coinhsl-2015.06.23
wget http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/OLD/metis-4.0.3.tar.gz #This is the metis version used in the configure file of coinhsl
tar xvzf metis-4.0.3.tar.gz
#sudo make uninstall && sudo make clean #Only needed if you have installed it before
./configure LIBS="-llapack" --with-blas="-L/usr/lib -lblas" CXXFLAGS="-g -O3 -fopenmp" FCFLAGS="-g -O3 -fopenmp" CFLAGS="-g -O3 -fopenmp" #the output should say `checking for metis to compile... yes`
sudo make install #(the files will go to /usr/local/lib)
cd /usr/local/lib
sudo ln -s libcoinhsl.so libhsl.so #(This creates a symbolic link `libhsl.so` pointing to `libcoinhsl.so`). See https://github.com/casadi/casadi/issues/1437
echo "export LD_LIBRARY_PATH='\${LD_LIBRARY_PATH}:/usr/local/lib'" >> ~/.bashrc
Optional (only if you want to modify the optimization problem, MATLAB needed)

The easiest way to do this is to install casadi from binaries by simply following these commands:

cd ~/installations
mkdir casadi
wget https://github.com/casadi/casadi/releases/download/3.5.5/casadi-linux-matlabR2014b-v3.5.5.tar.gz
tar xvzf casadi-linux-matlabR2014b-v3.5.5.tar.gz -C ./casadi

Open Matlab, execute the command edit(fullfile(userpath,'startup.m')), and add the line addpath(genpath('/home/YOUR_USERNAME/installations/casadi')) in that file (changing YOUR_USERNAME with your username). This file startup.m is executed every time Matlab starts.

Then you can restart Matlab (or run the file startup.m), and make sure this works:

import casadi.*
x = MX.sym('x')
disp(jacobian(sin(x),x))

Then, to use a specific linear solver, you simply need to change the name of linear_solver_name in the file main.m, and then run that file.

Note: When using a linear solver different from mumps, you need to start Matlab from the terminal (typing matlab).More info in this issue.

Note: Instead of the binary installation explained in this section, another (but not so straightforward) way would be to use the installation from source done above, but it requires some patches to swig, see this.

Compilation

cd ~/Desktop && mkdir ws && cd ws && mkdir src && cd src
git clone https://github.com/mit-acl/panther.git
cd panther
sudo apt-get install git-lfs #Make sure you have git-lfs installed
git lfs install
git submodule init && git submodule update
catkin build
echo "source /home/YOUR_USER/Desktop/ws/devel/setup.bash" >> ~/.bashrc 

Running Simulations

Simply execute

roslaunch panther simulation.launch quad:=SQ01s

Now you can click Start on the GUI, and then press G (or click the option 2D Nav Goal on the top bar of RVIZ) and click any goal for the drone.

You can also change the following arguments when executing roslaunch

Argument Description
quad Name of the drone
perfect_controller If true, the drone will track perfectly the trajectories, and the controller and physics engine of the drone will not be launched. If false, you will need to clone and compile snap_sim, snap and outer_loop
perfect_prediction If true, the drone will have access to the ground truth of the trajectories of the obstacles. If false, the drone will estimate their trajectories (it needs gazebo=true in this case).
gui_mission If true, a gui will be launched to start the experiment
rviz If true, Rviz will be launched for visualization
gazebo If true, Gazebo will be launched
gzclient If true, the gui of Gazebo will be launched. If false, (and if gazebo=true) only gzserver will be launched. Note: right now there is some delay in the visualization of the drone the gui of Gazebo. But this doesn't affect the point clouds generated.

You can see the default values of these arguments in simulation.launch.

NOTE: (TODO) Right now the radius of the drone plotted in Gazebo (which comes from the scale field of quadrotor_base_urdf.xacro) does not correspond with the radius specified in mader.yaml.

Credits:

This package uses some the hungarian-algorithm-cpp and some C++ classes from the DecompROS and repos (both included in the thirdparty folder), so credit to them as well.


Approval for release: This code was approved for release by The Boeing Company in March 2021.

Comments
  • Error rqt_gui application

    Error rqt_gui application

    Hello everyone,

    I've been following the tutorial in the Readme.md. However, as con you can see in the attached image, I have some problems with the rqt_gui.

    I'm using ROS Noetic on Ubuntu 20.04. However the rqt_gui crashes. The error says:

    TypeError: arguments did not match any overloaded call: QByteArray(): too many arguments QByteArray(int, str): argument 1 has unexpected type 'str' QByteArray(Union[QByteArray, bytes, bytearray]): argument 1 has unexpected type 'str'

    I've tried to fix it by following the answer found in this link: https://answers.ros.org/question/365220/rqt_gui-error-in-perspective-manager-in-ros-noetic/ . Basically I have changed "QtCore.QByteArray('000000ff00000000fd00000001...00')" on the line number 436 by "b'000000ff00000000fd00000001...00'" on the demo_interface.perspective document (https://github.com/mit-acl/panther/blob/master/panther/rqt_cfgs/demo_interface.perspective). However it does not solve the problem.

    Any idea?

    Thanks

    Panther_ERROR

    opened by OscarRamosRubio 3
  • /usr/local/lib/libhsl.so: undefined symbol: isamax_

    /usr/local/lib/libhsl.so: undefined symbol: isamax_

    I follow the instruction and finish the complie. After I click a goal for the drone,I get the error:

    *IN REPLAN CB trajs_.size()= 1 [Selection] Probs of coll --> 0e-15,
    [Selection] Chosen Trajectory 0, P(collision)= 0e-5 [Selection] Focusing on obstacle, angle=24.4 deg initial_state= Pos, Vel, Accel, Jerk, Yaw, DYaw= 0 0 0 0 0 0 0 0 0 0 0 0 -0.32 0 final_state= Pos, Vel, Accel, Jerk, Yaw, DYaw= 4.23 -1.48 0.375 0 0 0 0 0 0 0 0 0 0 0 Solved so far0/1 in SolverIpopt::optimize [NL] Running A
    from0 0 0 to 4.23 -1.48 0.375, allowing time = 350 ms [A*] Running... [A*] openList_ is empty [A*] choosing closest complete path as solution complete_closest_dist_so_far_= 1.61 [ INFO] [1645000821.975377700, 5.100000000]: [NL] A
    found a feasible solution! Shortest path: -0.32 --> 0 --> 0.15708 --> 0.314159 --> 0.314159 --> 0.314159 --> 0.314159 --> 0.314159 --> 0.471239 --> 0.471239 --> 0.628319 --> 0.785398 --> 0.942478 --> 0.942478 Optimizing for YAW and POSITION! Exception of type: OPTION_INVALID in file "IpAlgBuilder.cpp" at line 321: Exception message: Selected linear solver MA27 not available. Tried to obtain MA27 from shared library "libhsl.so", but the following error occured: /usr/local/lib/libhsl.so: undefined symbol: isamax_

    EXIT: Invalid option encountered. solver : t_proc (avg) t_wall (avg) n_eval nlp_grad | 504.00us (504.00us) 362.12us (362.12us) 1 total | 1.81ms ( 1.81ms) 668.36us (668.36us) 1 optimstatus= Invalid_Option IPOPT failed to find a solution Solver failed

    The reason for this seems to be: /usr/local/lib/libhsl.so: undefined symbol: isamax_, how should I do to solve this? 2022-02-16 16-47-53 的屏幕截图

    opened by 958117216 1
  • README link to paper missing!

    README link to paper missing!

    Very minor issue of course -- I was able to find the paper on Arxiv anyway, but I thought you might want to know that the link in the README just links to the Arxiv homepage :)

    opened by nabeelsherazi 1
  • getCPs_MV_Jerk_ofInterval

    getCPs_MV_Jerk_ofInterval

    Change

    https://github.com/mit-acl/panther/blob/master/panther/matlab/main.m#L220

    and

    https://github.com/mit-acl/panther/blob/master/panther/matlab/main.m#L232

    to getCPs_XX... (basis...)

    opened by jtorde 0
  • DeserializingStream sanity check failed

    DeserializingStream sanity check failed

    While running roslaunch panther simulation.launch quad:=SQ01s, I am getting the following error and then the process dies (Trying to specify the goal position using G or 2D Nav Goal doesn't work as well) :

    [INFO] [1640295936.156948, 0.000000]: Loading model XML from ros parameter robot_description
    Starting perfect tracker node for: /SQ01s/
    [INFO] [1640295936.208657, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
    [INFO] [1640295938.287394, 0.000000]: Loading model XML from file 
    /home/piyush/panther_ws/src/panther/panther/meshes/tmp_0.urdf
    [INFO] [1640295938.290552, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
    [ INFO] [1640295953.298561116]: Finished loading Gazebo ROS API Plugin.
    [ INFO] [1640295953.302099473]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, 
    waiting...
    [ INFO] [1640295955.478527787]: waitForService: Service [/gazebo/set_physics_properties] is now available.
    [INFO] [1640295955.502009, 0.000000]: Calling service /gazebo/spawn_urdf_model
    [INFO] [1640295955.524182, 0.000000]: Calling service /gazebo/spawn_urdf_model
    **DISABLING PHYSICS**
    [INFO] [1640295955.601097, 0.001000]: Spawn status: SpawnModel: Successfully spawned entity
    [ INFO] [1640295955.607068139, 0.001000000]: Physics dynamic reconfigure ready.
    [INFO] [1640295957.281193, 0.197000]: Spawn status: SpawnModel: Successfully spawned entity
    [ INFO] [1640295957.297051071, 0.213000000]: Found transform SQ01s --> SQ01s/camera_depth_optical_frame
    Basis chosen: MINVO
    Parameters obtained, checking them...
    par_.fov_depth= 5
    par_.Ra= 4.5
    par_.drone_radius= 0.2
    Parameters checked
    Changing DroneStatus from TRAVELING to GOAL_REACHED
    [SQ01s/spawn_robot-3] process has finished cleanly
    log file: /home/piyush/.ros/log/9f674d0c-6439-11ec-9abd-d0abd521f44e/SQ01s-spawn_robot-3*.log
    terminate called after throwing an instance of 'casadi::CasadiException'
      what():  .../casadi/core/serializing_stream.cpp:55: Assertion "check==serialization_check" failed:
    DeserializingStream sanity check failed. Expected 123456789012345, but got -5863120225865027243.
    [ INFO] [1640295958.262307913, 1.174000000]: Camera Plugin: Using the 'robotNamespace' param: '/SQ01s/'
    [ INFO] [1640295958.267886238, 1.180000000]: Camera Plugin (ns = /SQ01s/)  <tf_prefix_>, set to "/SQ01s"
    [ WARN] [1640295958.268088816, 1.180000000]: dynamic reconfigure is not enabled for this image topic 
    [camera/rgb/image_raw] becuase <cameraName> is not specified
    [SQ01s/panther-13] process has died [pid 3137, exit code -6, 
    cmd /home/piyush/panther_ws/devel/lib/panther/panther_node ~state:=state ~who_plans:=who_plans 
    ~term_goal:=term_goal ~trajs_predicted:=tracker_predictor_node/trajs_predicted ~traj:=traj 
    ~trajs_zhejiang:=trajs_zhejiang ~goal:=goal ~setpoint:=setpoint __name:=panther 
    __log:=/home/piyush/.ros/log/9f674d0c-6439-11ec-9abd-d0abd521f44e/SQ01s-panther-13.log].
    log file: /home/piyush/.ros/log/9f674d0c-6439-11ec-9abd-d0abd521f44e/SQ01s-panther-13*.log
    

    I am using ROS Melodic on Ubuntu 18.04 and all the packages and dependencies including casadi were installed and built properly. Any help in resolving the above issue would be highly appreciated. Thanks in advance!

    opened by kpiyush25 7
Owner
MIT Aerospace Controls Laboratory
see more code at https://gitlab.com/mit-acl
MIT Aerospace Controls Laboratory
FastDynamicCast - Fast dynamic cast in C++ for MSVC, outperforming the regular dynamic cast by up to 25 times

Fast dynamic cast This is a single header, dynamic cast implementation which outperforms the regular dynamic_cast by up to 25 times. Works on MSVC 201

tobspr 85 Oct 23, 2022
Allocator Aware Containers Made Easy

Custom Allocator Aware containers made easy with "wit" Have you ever looked at the allocator specification and thought about how hard it would be to

Darth Rubik 4 Sep 25, 2021
Simple Useful Libraries: C++17/20 header-only dynamic bitset

dynamic_bitset Simple Useful Libraries: C++17/20 header-only dynamic bitset Requirements To use this dynamic bitset, you will need a C++17 (or later)

Maxime Pinard 116 Nov 16, 2022
Build a tree-sitter dynamic module

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! I should clarify that this module is NOT a standalone tree-sitter module. It is supo

Yuan Fu 19 Dec 3, 2022
An intrusive C++17 implementation of a Red-Black-Tree, a Weight Balanced Tree, a Dynamic Segment Tree and much more!

This is Ygg (short for Yggdrasil), a C++17 implementation of several intrusive data structures: several balanced binary search trees: a red-black Tree

Lukas Barth 97 Nov 22, 2022
Implementing dynamic lists in C

Dynamic-Lists Implementing dynamic lists in C <begginer level> DESCRIPTION: This header file <list.h> implements python-like lists and it's functions.

null 2 Feb 2, 2022
C++ library to create dynamic structures in plain memory of shared-memory segments

Ищи описание на хабре @mrlolthe1st. #define _CRT_SECURE_NO_WARNINGS #include "shared_structures.h" #include <iostream> #include <fstream> #include <ca

Александр Новожилов 4 Mar 30, 2022
NLP-based perching trajectory generation presented in our paper "Perception-Aware Perching on Powerlines with Multirotors".

Perception-Aware Perching on Powerlines with Multirotors This repo contains the code for the NLP-based perching trajectory generation presented in our

null 23 Nov 21, 2022
A Robust and Efficient Trajectory Planner for Quadrotors

Fast-Planner is developed aiming to enable quadrotor fast flight in complex unknown environments. It contains a rich set of carefully designed planning algorithms.

HKUST Aerial Robotics Group 1.5k Dec 3, 2022
Online multi-agent trajectory planner using linear safe corridor (LSC)

lsc_planner This package presents an efficient multi-agent trajectory planning algorithm which generates safe trajectories in obstacle-dense environme

Jungwon Park 32 Nov 17, 2022
Fast, Attemptable Route Planner for Navigation in Known and Unknown Environments

FAR Planner uses a dynamically updated visibility graph for fast replanning. The planner models the environment with polygons and builds a global visi

Fan Yang 334 Nov 25, 2022
RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments (Code & Dataset)

RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments RP-VIO is a monocular visual-inertial odometry (VIO) system that uses onl

Karnik Ram 166 Nov 22, 2022
Lightweight, Portable, Flexible Distributed/Mobile Deep Learning with Dynamic, Mutation-aware Dataflow Dep Scheduler; for Python, R, Julia, Scala, Go, Javascript and more

Apache MXNet (incubating) for Deep Learning Apache MXNet is a deep learning framework designed for both efficiency and flexibility. It allows you to m

The Apache Software Foundation 20.2k Nov 30, 2022
an obfuscator based on LLVM which can obfuscate the program execution trajectory

Othread Obfuscator Othread is an obfuscation tool based on LLVM, which can perfectly realize the obfuscation of C/C++ code under the Windows platform

null 17 Aug 2, 2022
A non-linear trajectory optimization library developed by Optimus Ride, Inc

A non-linear trajectory optimization library developed by Optimus Ride, Inc. This library implements a C++ version of the original open-source ALTRO solver developed by the Robotic Exploration Lab at Stanford and Carnegie Mellon Universities, also available open-source as an official Julia package.

Optimus Ride 56 Nov 28, 2022
Object Based Generic Perception Object Model

This model is a highly parameterizable generic perception sensor and tracking model. It can be parameterized as a Lidar or a Radar. The model is based on object lists and all modeling is performed on object level.

TU Darmstadt - FZD 5 Jun 11, 2022
New lateral movement technique by abusing Windows Perception Simulation Service to achieve DLL hijacking code execution.

BOF - Lateral movement technique by abusing Windows Perception Simulation Service to achieve DLL hijacking ServiceMove is a POC code for an interestin

Chris Au 190 Nov 14, 2022
This repo includes SVO Pro which is the newest version of Semi-direct Visual Odometry (SVO) developed over the past few years at the Robotics and Perception Group (RPG).

rpg_svo_pro This repo includes SVO Pro which is the newest version of Semi-direct Visual Odometry (SVO) developed over the past few years at the Robot

Robotics and Perception Group 1k Nov 29, 2022
Multi-sensor perception for autonomous vehicles

Multi-Sensor Fusion for Perception -- 多传感器融合感知 Maintained by Ge Yao, [email protected] Up & Running Overview 本Repo为基于ROS melodic @ Ubuntu 18.04的Mul

Ge Yao 4 Jun 11, 2022
FastDynamicCast - Fast dynamic cast in C++ for MSVC, outperforming the regular dynamic cast by up to 25 times

Fast dynamic cast This is a single header, dynamic cast implementation which outperforms the regular dynamic_cast by up to 25 times. Works on MSVC 201

tobspr 85 Oct 23, 2022