This repo contains the code for 2 examples of the ICRA 2021 submission.

A standalone version of the solver can be found by following the link Solver only. This version needs way less dependencies (e.g. no ros) and doesn't contain the examples of the paper.

Dependencies

ros

The planner is intergated as a ros node. It has been developped with ros kinetic but should be compatible with merely any recent ros distro. Ros installation

gazebo

Car dynamic is simulated with Gazebo. Gazebo installation

osqp

We use the QP solver OSQP as a baseline (see paper). OSQP installation

other dependencies

In addition, the following packages are necessary: gnuplot, libjsoncpp-dev, libx11-dev, liblapack-dev, libf2c2-dev, libeigen3-dev, libglew-dev, freeglut3-dev. They can be installed by calling sudo apt install PACKAGE_NAME.

Build

Clone the repository and its submodules into your catkin workspace:

git clone --recursive git@github.com:ControlTrees/icra2021.git

This will clone the code related to the examples and two submodules (https://github.com/ControlTrees/rai, and https://github.com/ControlTrees/tamp).

install gazebo models

The car, obstacle and pedestrian gazebo models are part of the repository and must be copied to the gazebo folder containing all models. This gazebo folder is typicall under ~/.gazebo/models.

cd icra2021/lgp_car_models
cp -r * ~/.gazebo/models

build RAI

The rai submodule doesn't use cmake as build system, it has to be build separatly.

cd ../control_tree_car/externals/rai
make

The build can take a few minutes.

build ros nodes

The rest of the code can be built as standard ros nodes using catkin_make in the source directory of the catkin workspace.

catkin_make

Execute tests

Open a terminal in the folder corresponding to the control_tree_car package in build folder of the catkin workspace. This is typically ${CATKIN_WORKSPACE}/build/icra_2021/control_tree_car where CATKIN_WORKSPACE is the root of the catkin workspace.

The tests can be launched using ctest:

ctest .

Launch examples

Pedestrian example

Open two terminals in the catkin workspace.

In the first terminal, type the following command, it will launch the planner and the visualization RViz.

roslaunch control_tree_car pedestrian.launch

In the second terminal, type the following command, it will launch the simulator.

gzserver src/icra_2021/lgp_car_gazebo_plugin/world/pedestrian_4.world

Obstacle avoidance example

Open two terminals in the catkin workspace.

In the first terminal, type the following command, it will launch the planner and the visualization RViz.

roslaunch control_tree_car obstacle_avoidance.launch

In the second terminal, type the following command, it will launch the simulator.

gzserver src/icra_2021/lgp_car_gazebo_plugin/world/obstacle_avoidance_2.world