Trajectory planner
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.