Pioneer P3-DX and ADstar planner

Today we all will be playing with Pioneer P3-DX robot. We'll learn how to program the robot to navigate in a grid world approach using the ADstar planning algorithm provided in the ROS SBPL package.

Before Starting the tutorial, you should know the basics of ROS.

This project was made at RISE LAB, IITM.

Package Link : P3DX_SBPL

Dependency : ROSARIA, roscpp


The Robot uses ADstar planner for navigation through the environment. The environment is a grid with each cell a square of 40cm by 40cm (can be changed). Initial environment is empty i.e. without obstacles as the P3DX moves toward its goal it keeps on updating its environment with the help of on-board eight front sonar sensors. The figure below will describe overall working.

Package overview

Package overview


1. ADstar : This is planner node

1.1 Subscribed topics :

sonar (P3DXsbpl/sonarmsg)

Sonar information is passed on this topic.

sbpl_wait ( P3DXsbpl/sbplmsg)

Subscribe to an acknowledgement signal for each step from P3DX driver node.

start_sbpl (P3DXsbpl/startmsg)

Width,height,start coordinates, end coordinates and obstacle threshold are published on this topic; only after receiving this planner starts.

1.2 Published topics :

sbpl_xy (P3DXsbpl/sbplmsg)

Publish robots present coordinated and new coordinates to driver node.

2. P3DX_motion : This is driver node for P3DX robot.

2.1 Subscribed topics :

sbpl_xy (P3DXsbpl/sbplmsg)

Receives robots present coordinated and new coordinates from planner node.

2.2 Published topics :

sonar (P3DXsbpl/sonarmsg)

Sonar information is passed on this topic.

sbpl_wait ( P3DXsbpl/sbplmsg)

Publish an acknowledgement signal after each step to P3DX planner node.

3 Message Types :

3.1 sbpl_msg :

bool sbpl_wait_flag
int8 sbpl_present_x
int8 sbpl_present_y
int8 sbpl_new_x
int8 sbpl_new_y
bool start_P3DX_motion
**3.2 sonar_msg :**
bool l         # weather obstacle is in left or not
bool fl         # weather obstacle is in front-left or not
bool f          # weather obstacle is in front or not
bool fr         # weather obstacle is in front-right or not
bool r          # weather obstacle is in right or not
int8 direction  # direction of the robot
**3.3 start_msg :**
int8 width         # width of grid
int8 height     # height of grid
int8 startx     # initial x coordinate of robot
int8 starty     # initial y coordinate of robot
int8 goalx      # x coordinate of goal
int8 goaly      # y coordinate of goal
char obsthresh  # obstacle threshold
**4. Launch File :** P3DX_sbpl.launch ### 5. Usage :
roslaunch P3DX_sbpl P3DX_sbpl.launch
rostopic pub -1 /start_sbpl P3DX_sbpl/start_msg - - 5 5 0 0 4 4 1
**NOTE :** The only concern should be about start\_sbpl topic and start\_msg type. Rest all topics,message types are used by the two nodes internally. **Planner will not start until it receives message on start_sbpl topic.** Initial direction of robot is East unless you have changed it in P3DX_motion.cpp. **6.** Direction Map The direction nomenclature i used is shown below.
Direction map

Direction map

See the working of this planner in the video below :

The goal coordinate of the robot is somewhere nearer to the blue basket.

Comments are welcomed :)