Section 5.2 Player/Stage Integration

The previous section presented a working example where a GSSP program was used to control ROS's turtlesim simulator. We saw how a TCP controller can be used to allow the human to override the robot's default behavior. In this section, we will run this exact same program on a different robot simulator - Player/Stage. Instead of controlling the turtle in turtlesim, we will control the pioneer robot in Player/Stage. To achieve this, we do not need to modify the GSSP program and the TCP controller. We only need to use a different driver. This shows how GSSP programs can be robot-independent.

Player/Stage

To run this example, you will have to install Player/Stage with python bindings for the C client library. This requires that you have swig and python developer libraries installed before compiling Player/Stage. Also ensure that your python path is configured correctly so that you can import playerc.

Driver

In the driver folder, you will find PlayerStageController.py, which is the driver we will be using. We will go through the code in this file.

import math
from playerc import *

First we import the math library and the playerc library. playerc allows our program to talk to Player/Stage.

class Driver():
    def __init__(self, address = 'localhost', port=6665):
        c = playerc_client(None, address, port)
        if c.connect() != 0:
            raise playerc_error_str()
        
        self.conn = c
        
        position2d = playerc_position2d(c, 0)
        if position2d.subscribe(PLAYERC_OPEN_MODE) != 0:
            raise playerc_error_str()
        
        self.position2d = position2d
        self.currentState = {}
        
        self.velocity = 1.0
        self.yawrate = 2.0

We create a Driver class according to the format described in Section 3.2 Creating Robot Drivers. In the constructor, we create a connection to the simulator via TCP. We will be using self.position2d throughout the driver to get the position of the robot and set the velocity of the robot. We initialize self.currentState to be an empty dictionary which we will use to store the current state of the robot. self.velocity is the default linear velocity, and self.yawrate is the default angular velocity.

    def getState(self):
        r = self.conn.read()
        
        self.currentState['Position X'] = self.position2d.px
        self.currentState['Position Y'] = self.position2d.py
        self.currentState['Velocity'] = self.velocity
        
        return self.currentState

getState gets the current state of the robot by calling self.conn.read(), and then stores it in self.currentState. It then returns self.currentState to the meta-controller.

    def changeState(self, stateChanges):
        if len(stateChanges.keys()) == 0:
            self.position2d.set_cmd_vel(0.0, 0.0, 0.0, 1)
        
        if stateChanges.has_key('Velocity'):
            self.velocity = stateChanges['Velocity']
            self.position2d.set_cmd_vel(self.velocity, 0.0, self.position2d.pa, 1)
        
        if stateChanges.has_key('Position X') or stateChanges.has_key('Position Y'):
            toX = 0
            toY = 0
            
            if not stateChanges.has_key('Position X'):
                toX = self.position2d.px
            else:
                toX = stateChanges['Position X']
                
            if not stateChanges.has_key('Position Y'):
                toY = self.position2d.py
            else:
                toY = stateChanges['Position Y']

            self._moveTo(toX, toY)

changeState accepts the requested state changes from the meta-controller as argument and translates them into robot-specific commands.

if len(stateChanges.keys()) == 0 checks to see if the set of requested state changes is empty. If so, it tells the robot to stop moving. Namely, it sets the robot's linear and angular velocity to zero.

if stateChanges.has_key('Velocity') checks to see if the meta-controller has requested a change in the robot's velocity. If yes, we update the default linear velocity with self.velocity = stateChanges['Velocity']. Then we tell the robot to travel at the new linear velocity while maintaining the current angular velocity.

if stateChanges.has_key('Position X') or stateChanges.has_key('Position Y') checks to see if a change in Position X or Position Y is requested. If the meta-controller requests a change in only one of these variables, we use the robot's current value for the other variable. This is what the two if-else blocks do. toX and toY indicates the position we ultimately want to move the robot to. We then call the _moveTo function, giving it toX and toY. The _moveTo function computes the angular velocity that is necessary to steer the robot to the desired x y location. It then tells the robot to move at that angular velocity. The implementation of _moveTo is an exercise in geometry and we will not go through it here.

    def cleanup(self):
        self.position2d.unsubscribe()
        self.conn.disconnect()

We close the connection to Player/Stage in the cleanup function.

GSSP Program

We will be running turtle.gssp located in the data folder. This is the same program described in the previous section. Please refer to the previous section with the same heading for a description of turtle.gssp.

TCP Controller

The TCP controller is also described in the previous section. Please refer to the previous section with the same heading for a description of the TCP controller.

Running the Example

To run the example, we need to first start Player/Stage. In the ext folder, there is a folder called playerStage. Inside it, you will find empty.cfg, which defines an empty world with no obstacles. In a terminal, go into this directory, and execute the following commands.

player empty.cfg

You should see Player/Stage start up.

Assuming that you have started the GUI and have opened turtle.gssp, start the meta-controller from the GUI. Make sure that you choose PlayerStageController.py as the driver. Section 3.3 Running the Meta-controller describes how to do this. The robot should then start following waypoint set w1 as shown below.

If you wish, you can attach the GUI to the meta-controller to visualize the position of the robot in the GUI. See Section 3.3 Running the Meta-controller for details on attaching to the meta-controller.

Now let us start the TCP controller. In a terminal, go into the ext/tcp-human-control directory, and run the following command.

python main.py

You should see the following.

Try to tell the robot to go north by typing w and then enter. Feel free to drive the robot in other directions. Typing c and then enter will cause the robot to go back to following w1.

Right now, there is a known issue with the PlayerStageController.py driver. Right after the Player/Stage simulator starts up, the driver can connect to the simulator just fine. However, if you stop the meta-controller, and then start it again, the driver will not be able to connect to the simulator. In this case, you would have to restart Player/Stage before the driver can connect again. We will try to resolve this issue in future releases of GSSP.