This planner is able to calculate movement trajectory on ros::OccupancyGrid for robot, using set of allowed moves for it. Given map, start and goal positions and possible movements, planner will try to find the way from start to goal positions, using one of available algorithms (A* and BFS implemented while now).

Planning works on 2D occupancy grid, which you can get from Rtabmap, hector_mapping or gmapping SLAM algorithms. OccupancyGrid is just 2D array, where each cell represent small area of real world and can be in one of three states: occupied, free, unknown. Using ros::OccupancyGrid is a good way for small wheel platforms or walking robots.

This package based on ROS.
Due to Python 2 as main language you don't need to compile package.
Just move it into catkin workspace and run planning on predefined map: roslaunch trajectory_planner_py static_planning.launch

Basic idea of planner - search in state space, where State is a vector which represents position and orientation of a robot. Robot is a rectangular primitive with specified width and height. Robot also has some movements, which it can do. Each movement is a vector (length, dtheta), which can represent movements such as rotation and moving forward/backward. When we apply movement to one of available states (for example start state), we can get new state with different position and rotation.

Of course, on the way of this movement must not be any obstacles. This is how we can get new state. It can be done by verifying substates when "simulating" yet another move.

So to start planning we should have map, start and goal positions, robot parameters and set of available moves for it. Then we can get huge tree of states by applying moves to start state and then to new states and etc.

Now we need algorithm to effectively find state, which is the nearest to goal state. Current implementation of A* is not brilliant, but seems to be easy to understand.

I got an idea of how to implement A* using this nice publication.

Architecture of this package is quite easy, so don't be scared to change something in source code ;)

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

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

This package is in development now. So be careful when using it. Some important things, like time limitation of path finding were not implemented yet.