Trajectory planner

This planner can calculate movement trajectory from start to goal positions on a ros::OccupancyGrid using A* or BFS algorithms for a robot primitive that can perform a set of simple movements.

Features

2D planning

Planning works on a 2D occupancy grid (ros::OccupancyGrid), which is just a 2D array, where each cell represents a small area and can be in one of three states: occupied, free, unknown. It is a good map data structure for small wheel platforms and simple walking robots. You can get it from Rtabmap, hector_mapping or gmapping SLAM algorithms.

rospy based

This package is based on ROS and built using Python 2, so you don't need to compile it. To run the package, move it into catkin workspace and run planning on predefined map: roslaunch trajectory_planner_py static_planning.launch

Description

How planner works

The planner is searching in the state space, where State is a vector of position and orientation of a robot. The Robot is a rectangular primitive with width and height parameters and a set of simple moves, which are described as vectors (length, dtheta) and represent rotation and moving forward/backward. New states are derived from the previous ones by applying move transformations.

We also check intermediate sub states on collisions with obstacles by simulating moves with a small step.

To start planning we need a map, start and goal positions, robot parameters and a set of available moves. Then we can get a tree of states by applying moves to the start state and repeating this for its "children". We use A* or BFS to search for a goal state in that tree. I got intuition for A* implementation from this nice publication.

ROS API

Subscribed topics

• initialpose (geometry_msgs/PoseWithCovarianceStamped)
  Start pose for planner. You can set initialpose from RVIZ
• goal (geometry_msgs/PoseStamped)
  Goal pose for planner

Published topics

• trajectory (visualization_msgs/MarkerArray)
  Array of robot's poses

Disclaimer

This package is in development now and may contain bugs.