Behavior tree

In this part we will begin our tutorial based on python smach. You will learn 3 basically used behavior tree algorithm---serial, concurrent over synchronous and preemption, which are connected with later tutorials person-follower and object-pick-place. notice this tutorial will utilize the fix-point movement program and fix-angle rotation program in [Tutorial on the odometer and its simple applications], the python interface in move_base will be used as well.

What is behavior tree

Behavior tree which is named finite-state machine. By calling subroutines, behavior tree can complete some complex behavior(e.g basic person follower program). The robot can make certain behaviors through the information from the sensor(e.g robot arm interruption due to sensor tested danger) . Due to the different sequence of finite-state in the compiler, making it abundant during the process of robot development, and more challenging as well. In the following tutorial we will use python smach framework and talk about 3 common ways to program the robot behavior tree, which is used in reality.

Serial behavior tree

The serial behavior tree is the simplest way to write a behavior tree, which simply determines the next state to be reached based on the output of the current state run. Although simple, serial behavior trees are sufficient for many application scenarios. In this section, we will first write a simple behavior tree to control a robot that walks wirelessly in a loop on a cube with a 4-meter-long edge. Then, to be more relevant to real-world applications, we will also use the interface provided by move_base to implement a simple version of the patrol function.

Walk around the square

Flowchart of walking around a square program:

start=>start: program begin
end=>end: program end
turn_right=>operation: right turn 90°
turn_left=>operation: left turn 90°
move_4_meters=>operation: forward 4 meter

start->turn_right
turn_right->move_4_meters
move_4_meters->turn_left
turn_left->move_4_meters

Demo start flow with the corresponding effect:

• 1 Input demo start commands

roslaunch handsfree_smach demo_smach_serial_square.launch

The stage screen appears on the desktop. The small blue cube is the robot, which is in the stage coordinate system at position (-4.0, -4.0), facing in the positive direction of the Y-axis: smach_demo

• 2 Check through the stage window, the blue cube is turing right in the stage window. When finished, the cube begins to circle around the 4 meters width square. The output is shown above. If you want to stop the program, just shutdown the terminal.

Simple patrol

Since the actual application environment requires the robot to have a wall barrier function, it is difficult to apply the edge-walking program written in Section 2-1 in a real-world scenario. robot movement control. The implementation steps can be divided into two parts: obtaining the target points of interest and writing the behavior tree based on the list of target points, as shown below (before learning this section, we strongly recommend you to complete the radar building tutorial:

Part I - Patrol "Point of Interest" Selection

• 1 According to Radar mapping we have made a map in the patrol area. When finished, it will be saved as in folder handsfree_stage/world as .bmp file. Open the folder handsfree_stone in the same folder(or you can rebuild a new file with .world in the end and paste the corresponding letters from handsfree_stone.world), then change some parameters in the world file.

• 2 Open stage virtual stage(if you use your .world file, remember to change your address in launch, and match the size with the parameter in rviz map), you can browse this paper for details.

• roslaunch handsfree_stage stage_handsfree_room.launch
  

  The stage window is shown, stage has loaded the saved map in step1 and a blue robot successfully.

• 3 Run node map_server and node amcl(they have been included in move_base_amcl.launch), rviz is opened to receive topic at the same time.

  roslaunch handsfree_stage move_base_amcl_stage.launch
  

  If run successfully, the terminal and rviz are shown as fellow.

  

• 4 According to the real location of robot in stage, we can set the initial location of amcl algorithm.(you can browse Navigation) for details). After initialization, open a new terminal, input command:

  rostopic echo /amcl_pose
  

  when finished, the amcl-based optimal location algorithm will be shown

  Drag the robot to the "point of interested", the amcl-based optimal algorithm will be computed when the robot arrive at the destination.

Second step-a simple patrol

In previous tutorials, a few of "point of interested" have been selected according to request. This time we will use the provided python interface in move_base system and make a simple patrolled program,which is based on python smach framework and preselected "point of interested".

• 1 The algorithm of this part is based on 2-1, if you are not familiar with python interface in move_base, browse this tutorial

• 2 The demo of this period is shown below(make sure smach_patrol.py have executable access)

  roslaunch handsfree_smach demo_smach_patrol.launch
  

  If run successfully, stage and rviz will show the following interface.

• 3 Switch to stage and rviz, you will find move_base is making path planing. In stage, robot is running the first step.

Concurrent synchronization behavior tree

Please browse official tutorials if you want more information about Concurrent State Machine, which is provided by python smach.

Explanation about concurrent synchronization

In consideration of the fact that there are some readers who have not studied operating system-related courses, the following is a simple example to explain the meaning of "synchronization"

Assume that there are three events A, B and C. Event C needs the data generated in events A and B. Therefore, event C can only be executed after events A and B have been executed, while events A and B are executed concurrently, and it is not possible to determine which event will end first. In this case, we need a mechanism to ensure that event C is executed only when events A and B are executed and data is output. This mechanism, synchronization, can be achieved by using a system-provided mutual exclusion lock or a semaphore to synchronize multiple threads.

Use concurrent synchronization behavior tree

There are many cases for concurrent synchronization behavior tree. For better understanding, in this period we will use concurrent state Machine interface, which is provided by python smach. We will control both two robot concurrently by the pre-programed fixed-point movement program.

Concurrent control of two robots program flow chart:

start=>start: Robot 0,1 in stage
end=>end: over
robot_1_3m_robot_2_1m=>operation: Robot 0 moves 3 meters, robot 1 moves 1 meter
robot_1_1m_robot_2_3m=>operation: Robot 0 moves 1 meters, robot 1 moves 3 meters
robot_1_n3m_robot_2_n1m=>operation: Robot 0 moves -3 meters, robot 1 moves -1 meter
robot_1_n1m_robot_2_n3m=>operation: Robot 0 moves -1 meter, robot 1 moves -3 meters
start->robot_1_3m_robot_2_1m->robot_1_1m_robot_2_3m->robot_1_n3m_robot_2_n1m
robot_1_n3m_robot_2_n1m->robot_1_n1m_robot_2_n3m->robot_1_3m_robot_2_1m

notice Robot 0 and robot 1 move in the same speed and at the same time(which is called concurrent process). If one of them arrive the destination, it should wait until the other one arrive the destination, then the next procedure can be executed.

Demo launch process and corresponding effect:

• 1 Open terminal and run the following command, start stage and concurrent state program.

  roslaunch handsfree_smach demo_smach_concurrence.launch
  

  In the stage interface, tow stage robots is shown, both faced to Y direction.

Observe the motion of the two robots: Both of them move for a while, then one of them stop, they do not move together until the other one arrive at the destination.(Each stop can be recognized as a concurrent synchronization.

Preemptive behavior tree

Preemptive behavior tree is the most complicated method of the 3 behavior trees, but it symbolizes the robot decision-making method---Change the current behavior according to environment. Before this part, you'd better have a good understanding of concurrent synchronization tree and get familiar with most of the official tutorials provided by python smach.

Operation mechanism of preemptive behavior tree

You would not be confused with preempt if you have browse ros actionlib. It means interrupt current executed program and response the preemptive request(notice it doesn't means the force interruption from operate system, but by the program itself). The preemptive procedure provided by python smach function in the same way. When a state is done, it can decide whether to preempt or not. After preemption, it can respond automatically.

Use preemptive behavior tree

The period of preemptive behavior tree will be mentioned in the following Person-Follower and Object-Pick-Place. For better understanding, this tutorial will be carried on based on period 2-1. A "Laser inspection" willed be added to detect the obstacle. The "hold on " instruction will be sent when the obstacle show out in a certain range. It will not circle around square until the obstacles remove.

Seizure behavior tree example program flow chart:

start=>start: assume a robot and a movable block in stage
end=>end: demo done
move=>operation: robot circle around suqare
stop=>operation: robot stop
laser_detect_move=>condition: obstacle?
laser_detect_stop=>condition: obstacle?
start->move
move->laser_detect_move
laser_detect_move(no)->move
laser_detect_move(yes)->stop
stop->laser_detect_stop
laser_detect_stop(no)->move
laser_detect_stop(yes)->stop

Result of the demo:

• 1 Check the laser node(Please make sure laser_detection.py have the executed access):

  rosrun handsfree_smach laser_detection.py
  

  If run successfully, there won't have any output in the terminal. Now execute step2.

• 2 Run the following command, start stage virtual platform and preemptive state procedure:

  roslaunch handsfree_stage demo_smach_preemption.launch
  

  Stage interface show up, the blue cube represents the robot and the red one is the movable obstacle.

  

  If you run successfully, the blue cube begins to circle around square in the stage interface opened in the first step. Use mouse to move the red cube in front of blue cube about 0.5 to 1 meter, it will stop. The procedure will be carried on when the red cube remove. The result is shown below.

  

Test behavior tree

This period could make you better understand behavior tree. Base on a practical application, we have provided four simple simulation. The program does not provide smach_viewer provided by ROS.(Next graph)

It is necessary to learn smach visual tools for much more complicated state transmitted condition when codding behavior tree. You can add a few of codes and check the running state in smach_viewer. For more details please browse official tutorials