Cargo-ANTS Path Adaptor

CHOMP implementation is based on source code from trychomp which is based on CHOMP.

Build and Run GUI:

You need CMake, Eigen v3, and GTK+ v2 (only the GUI).

git clone https://github.com/j3sq/ROS-CHOMP.git chomp
mkdir chomp/build
cd chomp/build
cmake ..
make
./chomp-shell

Usage:

void generatePath(VectorXd const &qs, VectorXd const &qe, VectorXd &xi, MatrixXd const &obs);

• qs : Start point (x,y) as a 2x1 vector.
• qe : End point (x,y) as a 2x1 vector.
• xi : Generated chomp trajectory points (x₁,y₁,x₂,y₂,...,x_N,y_N) as 2*Nx1 vector. N is currently fixed to 20.
• obs : A matrix of disk like obstacles of size Kx3. Each row is (x,y,R) of the obstacle.

Notes:

• If the function is called with a non empty trajectory xi, the function will use the provided trajectory as an initial guess for the chomp algorithm.
• demo/minimal_program.cpp shows a basic example of using chomp

ROS Support:

You need Cargo-ANTS for messages definitions. Place the project folder inside (path_to_ros_workspace)/src and then build by executing catkin_make.
For testing:

roscore
//on a separate terminal
rosrun cargo_ants_path_adaptor path_adaptor
//on a separate terminal
rostopic echo /trajectory  
//on a separate terminal. Replace x0,y0 and x1,y1 (below) by coordinates of start and end points respectively.
rostopic pub -1 /path_planner cargo_ants_msgs/Path '{mode: 0, container: name , goals: [{gx: x0, gy: y0, gth: 0, dr: 0, dth: 0},{gx: x1, gy: g1, gth: 0, dr: 0, dth: 0}]}'
//on a separate terminal. Replace o0,o0,R0 and o1,y1,R1 (below) by coordinates x,y,Radius of obstacles
rostopic pub -1 /obstacles cargo_ants_msgs/ObstacleMap '{obstacles:[origin: {ox : x0, oy : y0 , oth : R0}, origin:{ox : x1, oy : y1, oth : R1}]}'