Trajectory Planner

Package to compute collision free trajectories for each agent.

The algorithm [CIT3] used is based on Distributed Model Predictive Control.

Usage

  1. Set start position and goals of each agent to compute trajectories(/set_planner_positions srv)
  2. Start trajectory solver (/plan_trajectories srv)
  3. Wait for trajectory solver to be done
  4. Start trajectory publishing (/pub_trajectories srv)
  5. Wait for trajectory publishing to be done

ROS Features

Subscribed Topics

None

Published Topics

/cfx/trajectory_goal(crazyflie_driver/Position)
Position of the CF on the trajectory, at each time step

Services

/set_planner_positions(crazyflie_charles/SetPositions)
Set position (start or goal) of each crazyflie. See TrajectoryPlanner.set_positions() for more information.
/plan_trajectories(std_srvs/Empty)
Start solver to find a trajectory for each crazyflie.
/pub_trajectories(std_srvs/Empty)
Call to start publishing all trajectories

Services Called

/traj_found(std_srvs/SetBool)

Service called once a trajectory is found.

data is True if valid trajectories are found.

data is false otherwise (collision, no solution in constraints).

/traj_done(std_srvs/Empty)
Service called once the last step of the trajectory is reached.

Parameters

~cf_list(str, default: [‘cf1’])

TrajectoryPlanner Class

class trajectory_planner_ros.TrajectoryPlanner(cf_list, solver_args)[source]

To plan trajectories of CFs

agents_dict = None

dict of str: Keys are the id of the CF. Items are a dict containing: Agent, trajectory_publisher, start_yaw

Type:dict of str
to_plan_trajectories = None

True if a trajectories are to be planned

Type:bool
trajectory_found = None

True if a trajectory has been found for current position

Type:bool
to_publish_traj = None

True if a trajectories are to be published

Type:bool
set_positions(srv_req)[source]

Set start position or goal of each agent

Parameters:srv_req (SetPositions) – List /w position type(start or goal) and positions of each agent
plan_trajectories(_)[source]

Start trajectory planning of each agent

start_publishing_srv(_)[source]

Start publishing CF trajectories

publish_trajectories()[source]

Publish computed trajectory

run_planner()[source]

Execute the correct method based on booleen states