Click here to Skip to main content
14,304,886 members

Rodney - A Long Time Coming Autonomous Robot (Part 8)

Rate this:
5.00 (11 votes)
Please Sign up or sign in to vote.
5.00 (11 votes)
3 May 2019Apache
Eighth part in a series on a ROS (Robot Operating System) House Bot

Introduction

The Rodney Robot project is a hobbyist robotic project to design and build an autonomous house-bot using ROS (Robot Operating System). This article is the eighth in the series describing the project.

Background

In part 1, to help define the requirements for our robot, we selected our first mission and split it down into a number of Design Goals to make it more manageable.

The mission was taken from the article, Let's build a robot! and was: Take a message to... - Since the robot will [have] the ability to recognize family members, how about the ability to make it the 'message taker and reminder'. I could say 'Robot, remind (PersonName) to pick me up from the station at 6pm'. Then, even if that household member had their phone turned on silent, or was listening to loud music or (insert reason to NOT pick me up at the station), the robot could wander through the house, find the person, and give them the message.

The design goals for this mission were:

  • Design Goal 1: To be able to look around using the camera, search for faces, attempt to identify any people seen and display a message for any identified
  • Design Goal 2: Facial expressions and speech synthesis. Rodney will need to be able to deliver the message
  • Design Goal 3: Locomotion controlled by a remote keyboard and/or joystick
  • Design Goal 4: Addition of a laser ranger finder or similar ranging sensor used to aid navigation
  • Design Goal 5: Autonomous locomotion
  • Design Goal 6: Task assignment and completion notification

In the last part, we added the ROS Navigation Stack to give Rodney autonomous locomotion. The goals we gave the robot to navigate to were set using the visualisation tool rviz. In this part, we will add the mission code to navigate to different parts of the house and at each location to search for the known person to deliver the message to. The locations to visit will be loaded from a yaml file.

We will be updating the rodney and rodney_missions packages and adding a new package which enables us to add a LED and pushbuttons to the Raspberry Pi GPIO (General Purpose Input Output).

Smach Revisted

In part 4, we looked at the smach ROS package which is used to create state machines. We created a hierarchical state machine where we added a lower level state machine for the greeting mission (Mission 2). We wrote the code in such a way that it should be relatively simple to add new missions by adding lower level state machine for any new mission. In this part, we will add two new missions as lower level state machines. The first, Mission 1, will carry out our "Take a message to.." mission and the second, Mission 4, will be a "Go Home" mission which will be executed to send the robot back to a home location.

Image 1

Before we get to that, we will look at adding a bi-coloured LED, two pushbuttons and updating the rodney package to send the new mission data to the state machine.

GPIO

Although we have the touch screen, I want to add two pushbuttons to input commands to Rodney. Since the screen is the robot head, and can move, it means that it may not be in a suitable position for a user to interact with the robot. For this reason, we will add two pushbuttons to the robot hardware. Each button will have two functions, the function executed will depend on whether the robot is currently running a mission or not.

The first button identified as the black button will carry out the following functions. If no mission is running and the button is pressed, the robots head will move to a position which is suitable for a user input/output at the screen. If a mission is running and the button is pressed, a command will be sent to cancel the current mission.

The second button identified as the yellow button will carry out the following functions. If no mission is running and the button is pressed, a command will be issued to run the "Go Home" mission. This mission will navigate the robot to a known home location. Some missions will have a state which requires a user to acknowledge the robot. For example, when a verbal message is delivered by the robot, the user should acknowledge the receipt of the message. This acknowledgement will be the second function of the yellow pushbutton. If it is pressed when a mission is running, an acknowledge message will be sent to the state machine.

Along with the two pushbuttons, we will add a bi-coloured LED to the robot hardware. When the robot is not running a live mission, the LED will light green and when the robot is running a mission, it will light red.

Image 2

We could add the GPIO code required to the rodney package but in the interest of code reuse, we will add a new package to the project to handle the GPIO. Remember that you can write ROS code in a number of different languages and as we will make use of an existing Python library for accessing the Raspberry Pi GPIO, we will code this new node using Python.

Our ROS package for the node is called pi_io and is available in the pi_io folder. In this package, we are going to create a ROS service to change the state of the GPIO output lines. Although we haven't coded our own ROS service in any of the previous articles we have made use of an existing service to enable/disable the LIDAR. We will also create a new user defined message which will be published when one of the pushbuttons changes state.

The package contains all the usual ROS files and folders.

The msg folder contains the gpio_input.msg file which contains the definition of our user define ROS message.

uint8 index   # GPIO index starting at 0
bool value    # GPIO value

The message contains two parameters. The first is an index for which GPIO the message refers. The code will map this index to a GPIO pin. The second parameter is the current state of that pin.

The srv folder contains the gpio_output.srv file which contains the definition for our user defined ROS service. Services can be looked at as a remote procedure call, unlike ROS actions which we have covered in previous articles, services should return quickly as they are blocking.

uint8 index   # GPIO index starting at 0
bool value    # GPIO value
---
bool success

The message contains three parameters. The first is an index indicating which GPIO the value refers to. The code will map this index to a GPIO pin. The second parameter is the value to set that pin to. Our service will return a boolean success value. With the Raspberry Pi, we can't tell if the GPIO pin is set so we will always return true. With a board which allows you to read feedback for an output pin, we could return true only if the pin was set to the correct value. It is possible to set the return value of a service to empty by not entering anything after the "---" line.

The src folder contains the Python code for the node in the pi_io_node.py file. I'll now briefly describe the code.

The main function registers our node with ROS and creates an instance of the PiGpioNode class.

def main(args):
    rospy.init_node('pi_io_node', anonymous=False)
    rospy.loginfo("Rapberry Pi GPIO node started")    
    piio = PiGpioNode()

    try:
        rospy.spin()
    except KeyboardInterrupt:
        print('Shutting down')

if __name__ == '__main__':
    main(sys.argv)

The class constructor for the PiGpioNode maps the actual GPIO pins we will use to the index values used in the message and service. It then makes calls to the Pi GPIO Python library to state that we are referencing the GPIO by the GPIO pin number and not the physical pin numbering of the connector on the Pi. It sets which GPIO pins are outputs and which are inputs. For the inputs, it also states that the pin should be pulled down. When our pushbutton is pressed, it will pull the line high.

The input pins are also attached to an event which will result in the functions Input0HighCallback and Input1HighCallback being called when the respective input goes high.

The constructor then registers the service and the message topic with ROS. We also create two timers which will be used to debounce the pushbuttons.

class PiGpioNode:
    def __init__(self):
        self.__output0 = 23       # GPIO Pin 23 is output index 0
        self.__output1 = 24       # GPIO Pin 23 is output index 1
        self.__input0 = 27        # GPIO Pin 27 is input index 0
        self.__input1 = 22        # GPIO Pin 22 is input index 1
        
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)    # Use GPIO pin number not the physical pin numbering
        
        GPIO.setup(self.__output0, GPIO.OUT)
        GPIO.setup(self.__output1, GPIO.OUT)
        GPIO.setup(self.__input0, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
        GPIO.setup(self.__input1, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
        
        # Currently we only publish message when input goes high
        GPIO.add_event_detect(self.__input0, GPIO.RISING, callback=self.Input0HighCallback)
        GPIO.add_event_detect(self.__input1, GPIO.RISING, callback=self.Input1HighCallback)
        
        self.__service = rospy.Service('gpio/output_cmd', gpio_output, self.OutputCommand)    
        self.__gpi_pub = rospy.Publisher('gpio/input_cmd', gpio_input, queue_size=3)
        
        # Timers for button bounce
        self.__input0_time = rospy.Time.now()
        self.__input1_time = rospy.Time.now()

The remainder of the class consists of the callback functions.

The first relates to the ROS service. When a call to the service is received on the gpio/output_cmd service, this callback will be called. From the index contained in the service call, it sets the GPIO output to the given value and returns True.

def OutputCommand(self, request):
    if(request.index == 0):
        gpo = self.__output0
    elif(request.index == 1):
        gpo = self.__output1

    GPIO.output(gpo, request.value)

    return True

The remaining callbacks are similar to each other and are called when one of the pushbuttons is pressed which causes the GPIO input pin in question to go high. If the pushbutton was not pressed in the last 200ms a message is created for the gpio/input_cmd topic, containing the index for the GPIO and a True value as in our case the callback will only be called when the input pin goes high. If your project is interested when the button is released, you could add a callback for this operation which sets the value in the message to false. The message is then published on the topic.

def Input0HighCallback(self, channel):
    # Check for button bounce
    current_time = rospy.Time.now()
    delta_time = (current_time - self.__input0_time).to_sec()    # Floating point seconds

    if delta_time > 0.200:
        input_cmd = gpio_input()
        input_cmd.index = 0
        input_cmd.value = True
        self.__gpi_pub.publish(input_cmd)

    self.__input0_time = current_time

def Input1HighCallback(self, channel):
    # Check for button bounce
    current_time = rospy.Time.now()
    delta_time = (current_time - self.__input1_time).to_sec()    # Floating point seconds

    if delta_time > 0.200:
        input_cmd = gpio_input()
        input_cmd.index = 1
        input_cmd.value = True
        self.__gpi_pub.publish(input_cmd)

    self.__input1_time = current_time

Below is an updated circuit diagram for Rodney which includes how the bi-coloured LED and pushbuttons are connected to the Raspberry Pi GPIO. A full size image of the circuit diagram is available in the drawings zip file.

Image 3

Note that I have made another change to the circuit since the last article. The Teensy is no longer powered from the Raspberry Pi. There is a track on the underside of the Teensy which can be cut to separate VIN from VUSB.

Updates to the Rodney Package

The rodney node will be responsible for setting the state of the LED and for monitoring the topic that publishes the state change of the pushbuttons. We also need to update it so that it can provide the mission data for the two new missions.

The first change will be to the rodney.launch file in the launch folder. This file configures and launches all the ROS nodes that make up the system.

We need to add the new pi_io node to the launch file so that the new code runs when we launch the robot code.

<node pkg="pi_io" type="pi_io_node.py" name="pi_io_node"/>

So that the robot can navigate to various locations in the house, we are going to provide it with a list of waypoints that will be stored in a yaml file. We are going to modify the rodney node so that the name of the file is passed to it at launch. To help us identify which waypoint file goes with which map, the waypoint file will have the same name as the map file with _patrol postfixed to the name. For example, the map my system loads by default is called second_floor, the file containing the waypoints that goes with this map is called second_floor_patrol.yaml. We will modify the lines of the launch file that launches the rodney node so that the file name is passed as an argument. The file name will follow the flag "-m".

<node pkg="rodney" type="rodney_node" name="rodney" 

 args="-m $(find rodney)/maps/$(arg map_file)_patrol.yaml" output="screen">
  <rosparam command="load" file="$(find rodney)/config/config.yaml"/> 
</node>

As you can see from the launch file snippet, the file containing the waypoints is stored in the map folder. Each waypoint in the file contains an x and y location and a direction the robot should face when it reaches the goal. This direction or orientation is a quaternion. In the "Using the Code" section, I'll show a handy way of working out what the position and orientation values are for any location on the map.

The waypoints should start at "w1", you can have as many waypoints as you like as long as they are consecutive. When searching for the person to deliver the message to, the robot will visit each waypoint in turn in the ascending and then the descending order. The file should also contain a home waypoint which the robot will navigate to when instructed to "Go Home".

An example of a waypoints file:

# Waypoints must start at w1 and be consecutive
# Also have an home location
w1:
  position:
    x: -0.328835725784
    y: -0.529747009277
  orientation:
    x: 0.0
    y: 0.0
    z: 0.273852223218
    w: 0.961771781577
w2:
  position:
    x: 1.31689155102
    y: -0.944578707218
  orientation:
    x: 0.0
    y: 0.0
    z: 0.732759099845
    w: 0.680488134793  
w3:
  position:
    x: 3.66307258606
    y: -0.040109038353
  orientation:
    x: 0.0
    y: 0.0
    z: 0.413215093458
    w: 0.910633453448
w4:
  position:
    x: 6.55329608917
    y: 1.04117441177
  orientation:
    x: 0.0
    y: 0.0
    z: 0.914737463209
    w: -0.404048726519
home:
  position:
    x: 0.0451934337616
    y: 0.0451934337616
  orientation:
    x: 0.0
    y: 0.0
    z: 0.224733375634
    w: 0.974420294265 

Now for the changes to the rodney_node.cpp file.

The main function now handles the parsing of the argument that contains the waypoint yaml file. This file name is passed to the constructor of the rodney_node class.

int main(int argc, char **argv)
{   
    ros::init(argc, argv, "rodney");
    
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");    
    std::string waypoints_filename;
    int opt;
    
    /* argument -m gives the file containing the patrol pose positions */
    while((opt = getopt(argc, argv, ":m:")) != -1)
    {
        switch(opt)
        {
            case 'm':
                waypoints_filename = std::string(optarg);                
                break;
        }
    }
        
    RodneyNode rodney_node(nh, nh_private, waypoints_filename);   
    std::string node_name = ros::this_node::getName();
    ROS_INFO("%s started", node_name.c_str());
	
    ros::Rate r(20); // 20Hz	    
    
    while(ros::ok())
    {
        rodney_node.sendTwist();
        rodney_node.checkTimers();
        
        ros::spinOnce();
        r.sleep();
    }
    
    return 0;    
}

There are a few minor changes to the constructor itself. It takes the waypoint filename and stores it in the member variable waypoints_filename_.

The constructor also subscribes to the new topic from the GPIO node gpio/input_cmd, registering the callback function to be called when a message is received on the topic.

// Raspberry Pi GPIO push buttons
gpio_sub_ =  nh_.subscribe("gpio/input_cmd", 5, &RodneyNode::gpioCallBack, this);

The final change in the constructor is to wait until the gpio/output_cmd service is available and then calls a new function missionNotRunning which sets the GPIO output to light the green part of the LED. Waiting for the service to become available stops us from calling the service before the pi_io node creates the service.

// Waiting for GPIO service to be available
ros::service::waitForService("gpio/output_cmd");
missionNotRunning();

The missionNotRunning and missionRunning functions are called when we switch between running a mission and not running a mission. They set the state mission_running_ member variable and set the required state of the bi-coloured LED using the service.

void RodneyNode::missionNotRunning(void)
{
    mission_running_ = false;
    
    // Red LED off, Green LED on
    ros::ServiceClient client = nh_.serviceClient<pi_io::gpio_output>("gpio/output_cmd");
    pi_io::gpio_output srv;
    
    srv.request.index = 0;
    srv.request.value = true;
    client.call(srv);
    
    srv.request.index = 1;
    srv.request.value = false;
    client.call(srv);
}
//---------------------------------------------------------------------------

void RodneyNode::missionRunning(void)
{
    mission_running_ = true;
    
    // Red LED on, Green LED off
    ros::ServiceClient client = nh_.serviceClient<pi_io::gpio_output>("gpio/output_cmd");
    pi_io::gpio_output srv;
    
    srv.request.index = 0;
    srv.request.value = false;
    client.call(srv);
    
    srv.request.index = 1;
    srv.request.value = true;
    client.call(srv);
}

The gpioCallBack function is called when a message is received on the gpio/input_cmd topic. It checks to see which GPIO signal went high and hence which button was pressed.

If the black button was pressed and a mission is running, it publishes the message to cancel the mission. If no mission is running, it publishes a message requesting that the head be moved to the user input position.

If the yellow button is pressed and a mission is running, it publishes the message to acknowledge a mission state. If no mission is running, it publishes a message requesting that mission 4, the Go Home mission, be conducted. Included in the message is the waypoint filename.

// callback for change in GPIO input
void RodneyNode::gpioCallBack(const pi_io::gpio_input::ConstPtr& msg)
{
    switch(msg->index)
    {
        case 0:
            // Black push button changed state
            // If mission running and button pressed, send cancel mission message          
            if((mission_running_ == true) && (msg->value == true))
            {
                // Cancel the ongoing mission
                std_msgs::Empty empty_msg;
                cancel_pub_.publish(empty_msg); 
            }
            // If mission not running and button pressed, move head to user input position
            else if((mission_running_ == false) && (msg->value == true))
            {
                std_msgs::String mission_msg;
                mission_msg.data = "J3^i^-";
                mission_pub_.publish(mission_msg);                            
            }
            break;  
            
        case 1:
            // Yellow push button 
            // If mission running and button pressed, send acknowledge mission stage message
            if((mission_running_ == true) && (msg->value == true))
            {
                // Acknowledge a mission step
                std_msgs::Empty empty_msg;
                ack_pub_.publish(empty_msg);                               
            }
            // If mission not running and button pressed, send mission 4 command
            else if((mission_running_ == false) && (msg->value == true))
            {                
                std_msgs::String mission_msg;
                mission_msg.data = "M4^"+ waypoints_filename_;
                
                mission_pub_.publish(mission_msg);                    
                missionRunning();                
                manual_locomotion_mode_ = false;                                                
            }
            
            break;
            
        default:
            break;             
    }
    
    last_interaction_time_ = ros::Time::now();
}

There is a change to the keyboardCallBack function. We currently use a keyboard on a remote machine when testing the robot to indicate that a mission should be conducted. If key '1' is pressed, we form a mission request message for mission 1 and publish the message. For this mission, the mission data consists of the waypoint filename, the ID of the person to deliver the message to and the text of the message to deliver. If key '4' is pressed, we create a mission 4 request similar to when the yellow button is pressed with no mission running.

// Start a mission
std_msgs::String mission_msg;
mission_msg.data = "M" + std::to_string(msg->code-keyboard::Key::KEY_0);

// Add on parameters for different missions
switch(msg->code)
{
    case keyboard::Key::KEY_1:
        // Mission 1 "Take a message to.."
        // "M1^patrol poses file|id of person to search for|message to deliver"
        mission_msg.data += "^" + waypoints_filename_ + "|1|Please walk Bonnie";
        break;

    case keyboard::Key::KEY_4:
        // Mission 4 "Go Home"
        // "M4^patrol poses file""
        mission_msg.data += "^" + waypoints_filename_;
        break;

    default:
        break;
}

mission_pub_.publish(mission_msg);

missionRunning();

There is one more change to the launch file which I have reluctantly made. I have noticed occasionally that when the Raspberry Pi is busy, for example calculating a navigational route, the LIDAR code stops producing the message with the laser scan data. Once it enters this state, it never recovers and keeps returning a timeout. Although the code is from a third party, we do have the source but I was keen to complete this part of the project and don't fancy spending the time looking into someone else's code at the moment. I have made a simple change to the code by forking the GitHub repository and changing the file node.cpp. The change results in raising a ROS Error message in the log and calling ros::shutdown() to kill the node. The changed code is available in the source zip file.

So that the node gets restarted under this error condition, I have added the respawn="true" to the rplidar_ros tag in the rodney.launch file.

<!-- The RPLidar and laser filter node
     Have created symbolic link for /dev/ttyUSBn to be rplidar -->
<node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" respawn="true" output="screen">
  <param name="serial_port" type="string" value="/dev/rplidar"/>
  <param name="serial_baudrate" type="int" value="115200"/>
  <param name="frame_id" type="string" value="laser"/>
  <remap from="scan" to="scan_filter_input"/>
</node>

It's not great, it's a bit of a hack, but since adding it the robot has always recovered on the odd occasion that it occurs. It could be that we are just asking too much of the Raspberry Pi. Rodney was always going to be a proto type and for my next robotic project, I may need to look at a more powerful single board computer.

Updates to the rodney_missions Package

We originally designed the rodney_missions node so that it would be easy to add new missions. We will make some minor changes to the rodney_missions_node.py file to accommodate these new missions and add new files take_message_to.py and go_home.py which will contain the lower level state machines.

Let's take a look at these two new state machines.

Mission 1 - "Take a message to.."

Image 4

This mission is what all the articles have been working towards. All of the code for this state machine is in the take_message_to.py file.

We enter this lower level state machine via the PREPARE_MISSION state. The mission data passed to this state contains the waypoint filename, the ID of the person to search for and the message to deliver. These parameters are separated by the '|' character so the first thing this state does is to spilt the data into the three components. It then loads the waypoint file and as we are going to navigate autonomously, it ensues that the LIDAR is enabled. If it successfully opened the file and read the waypoints, then it transits to the PREPARE_FOR_HEAD_MOVE state, otherwise if an error occurred, it aborts the mission and transits back to the higher state machine which we will call the root state machine.

The code for the PREPARE_MISSION state is held in the PrepareMission1 class.

# PREPARE_MISSION State. Prepares the mission by loading the waypoint file
class PrepareMission1(State):   
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['ready','error'],
                       input_keys=['mission_data'],
                       output_keys=['person_id','message','waypoints',
                       'current_waypoint','waypoint_direction','mission_status','start'])
        self.__helper_obj = helper_obj
        
    def execute(self, userdata):
        # Parse the mission data using '|' as the delimiter        
        # parameters[0] will contain the filename of the file containing the poses
        # parameters[1] will contain the id of the person to search for
        # parameters[2] will contain the message to deliver
        parameters = userdata.mission_data.split("|")
        
        userdata.person_id = parameters[1]
        userdata.message = parameters[2]
        
        # Load patrol poses from the given patrol file
        try:
            with open(parameters[0], 'r') as stream:
                try:                    
                    userdata.waypoints = yaml.load(stream)
                    userdata.current_waypoint = 0
                    userdata.waypoint_direction = True                    
                    userdata.mission_status = 'void' # This will be overwritten
                    next_outcome = 'ready'
                except:
                    rospy.logerr('Bad waypoint file')
                    userdata.mission_status = 'Bad waypoint file'
                    next_outcome = 'error'
        except:
            rospy.logerr("Can't open waypoint file")
            userdata.mission_status = "Can't open waypoint file"
            next_outcome = 'error'                    
        
        # Ensure the Lidar is enabled
        self.__helper_obj.LidarEnable()
        
        # Indicate that when we start scanning for faces that it is a new scan
        userdata.start = True   
    
        return next_outcome   

The PREPARE_FOR_HEAD_MOVE state is responsible for calculating the next head position as the head/camera is moved to scan the robots field of view. If there are still head positions to scan, it will transit to the MOVE_HEAD state, otherwise it will transit to the DEFAULT_HEAD_POSITION state.

The code for the PREPARE_FOR_HEAD_MOVE state is held in the PrepareToMoveHead class.

# PREPARE_FOR_HEAD_MOVE State. Prepares for the next head position
class PrepareToMoveHead(State):
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['scan_complete','move_head'],
                       input_keys=['start_in'],
                       output_keys=['start_out','user_data_absolute',
                                    'user_data_pan','user_data_tilt'])
        self.__helper_obj = helper_obj
                                                             
    def execute(self, userdata):
        # Is this the start of a new head scan
        if userdata.start_in == True:
            userdata.start_out = False
            scan_complete = False
            # get the camera start position (pan min and tilt max)
            position_request_pan,  position_request_tilt = self.__helper_obj.CameraToStartPos()
        else:            
            scan_complete, position_request_pan, position_request_tilt = 
                                           self.__helper_obj.CameraToNextPos()
        
        # Set up user data that will be used for goal in next state if scan not complete
        userdata.user_data_absolute = True
        userdata.user_data_pan = position_request_pan
        userdata.user_data_tilt = position_request_tilt        

        if scan_complete == True:
            next_outcome = 'scan_complete'
        else:
            next_outcome = 'move_head'
        
        return next_outcome 

The MOVE_HEAD state is a special type of state called a SimpleActionState. We covered these type of states in part 4 of the article. This action conducted is to move the head to the position set by the PREPARE_FOR_HEAD_MOVE state. On completion of the action, the state machine will transit to the SCAN_FOR FACES state. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The SCAN_FOR_FACES state is again a SimpleActionState. This time, the action called is part of the face recognition package and will check the next image from the camera for known subjects. On completion of the action, the state machine will transit to the CHECK_FOR_SUBJECT state. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The CHECK_FOR_SUBJECT state checks the results from the previous state to see if the ID of the person the robot is searching for matches anyone in the results. If the subject was seen, then the message is delivered by creating and publishing a ROS topic to the speech and animated head nodes. The state machine will then transit to the WAIT_FOR_USER state. If the subject in question was not seen, then it will transit back to PREPARE_FOR_HEAD_MOVE.

The code for the CHECK_FOR_SUBJECT state is held in the CheckForSubject class.

# CHECK_FOR_SUBJECT state. Checks the results from the previous state to see 
# if person searching for was found
class CheckForSubject(State):
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['seen','not_seen'],
                       input_keys=['person_id','message','seen_ids','seen_names'])
        self.__helper_obj = helper_obj

    def execute(self, userdata):
        next_outcome = 'not_seen' 
                
        # Was anyone see?  
        if len(userdata.seen_ids) > 0:                     
            # at least one person was seen
            for idx, val in enumerate(userdata.seen_ids):               
                if str(val) == userdata.person_id:                    
                    # We have found who we were looking for
                    next_outcome = 'seen'                                                    
                    rospy.loginfo("I have found %s, delivering message", 
                                   userdata.seen_names[idx])
                    greeting = 'Hello ' + userdata.seen_names[idx] + 
                               ' I have a message for you'
                    
                    # Speak greeting
                    self.__helper_obj.Speak(greeting, greeting)
                    
                    rospy.sleep(2.0)
                    
                    # Speak message
                    self.__helper_obj.Speak(userdata.message, userdata.message)         
            
        return next_outcome

The WAIT_FOR_USER state waits in this state until an acknowledge message is received indicating that the user acknowledges the receipt of the spoken message or until a period of 60 seconds elapses. If the acknowledge is received, then the state machine will transit to the root state machine. If the 60 seconds elapse, then the state machine will transit back to the SCAN_FOR_FACES state. The state can also be pre-empted which would result in the transit back to the root state machine.

The code for the WAIT_FOR_USER state is held in the WaitForMsg class.

# WAIT_FOR_USER state. Waits for the ack message for a set time
class WaitForMsg(State):
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['ack','timeout','preempted'],
                       output_keys=['mission_status'])
        
        self.__helper_obj = helper_obj
                               
        self.__mutex = threading.Lock()
        self.__msg = None
        self.__subscriber = rospy.Subscriber('/missions/acknowledge', 
                            Empty, self.callBack, queue_size=1)
        
    def callBack(self, msg):
        # Indicate a message was received
        self.__mutex.acquire()
        self.__msg = msg
        self.__mutex.release()
        
    def execute(self, userdata):
        # Clear any previous ack message
        self.__mutex.acquire()
        self.__msg = None
        self.__mutex.release()
    
        # Give the user 60 seconds to acknowledge the message
        timeout = rospy.Time.now() + rospy.Duration.from_sec(60.0)
        message_arrived = False
        preempted = False
        while rospy.Time.now() < timeout and message_arrived == False and preempted == False:
            # Check to see if message arrived            
            self.__mutex.acquire()
            if self.__msg is not None:
                # Message has arrived
                message_arrived = True
            self.__mutex.release()
            
            # Check mission was cancelled
            if self.preempt_requested():
                self.service.preempt()
                preempted = True
             
        # Why did the loop terminate   
        if preempted == True:
            # Report with voice that the mission was cancelled
            message = 'Mission one cancelled'
            self.__helper_obj.Speak(message, message)            
            # set the mission status
            userdata.mission_status = 'Take a message mission cancelled'
            next_outcome = 'preempted'
        elif message_arrived == True:
            userdata.mission_status = 'Take a message mission complete'
            next_outcome = 'ack'            
        else:
            next_outcome = 'timeout'
            
        return next_outcome

The DEFAULT_HEAD_POSITION state is again a SimpleActionState. It will call an action to move the head/camera to the default position. When the robot moves, we want the head to face forward. Once the head move is complete, it will transit to the PREPARE_TO_MOVE state. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The PREPARE_TO_MOVE state is responsible for extracting the next waypoint form the list of waypoints. If it reaches the end of the list, it will start traversing the list in the opposite direction. From the waypoint data, it constructs the nav goal ready for the next state, MOVE.

The code for the PREPARE_TO_MOVE state is held in the PrepareToMove class.

# PREPARE_TO_MOVE State. Reads the next waypoint to move the base to
class PrepareToMove(State):
    def __init__(self):
        State.__init__(self, outcomes=['done'],
                       input_keys=['waypoints','current_waypoint_in','waypoint_direction_in'],
                       output_keys=['current_waypoint_out',
                                    'destination','waypoint_direction_out','start']) 
                       
    def execute(self, userdata):
        # Extract the next waypoint but which way along the waypoints are we travelling
        if userdata.waypoint_direction_in == True:
            # Incrementing along the waypoints
            next_waypoint = userdata.current_waypoint_in + 1
            waypoint_id = 'w' + str(next_waypoint)
                    
            # If next waypoint does not exist (reached end of list), 
            # we need to work backwards along the list
            if not waypoint_id in userdata.waypoints:
                next_waypoint = userdata.current_waypoint_in - 1
                userdata.waypoint_direction_out = False
                # Allow for only one waypoint
                if next_waypoint == 0:
                    next_waypoint = 1
                    
                # next waypoint updated so update the waypoint_id string
                waypoint_id = 'w' + str(next_waypoint)
        else:
            # Decrementing along the waypoints
            next_waypoint = userdata.current_waypoint_in - 1
            
            # If next point is now zero, we have reached the start of list and 
            # we need to work forwards along the list
            if next_waypoint == 0:
                next_waypoint = 1
                userdata.waypoint_direction_out = True
                
            waypoint_id = 'w' + str(next_waypoint)
                              
        # Copy the waypoint data in to a PoseStamped message and userdata                            
        waypoint = userdata.waypoints[waypoint_id]
        
        target_pose = PoseStamped()
            
        target_pose.header.frame_id = 'map'
        target_pose.pose.position.x = waypoint['position']['x']
        target_pose.pose.position.y = waypoint['position']['y']
        target_pose.pose.position.z = 0.0
        target_pose.pose.orientation.x = waypoint['orientation']['x']
        target_pose.pose.orientation.y = waypoint['orientation']['y']
        target_pose.pose.orientation.z = waypoint['orientation']['z']
        target_pose.pose.orientation.w = waypoint['orientation']['w']
            
        userdata.destination = target_pose
        userdata.current_waypoint_out = next_waypoint
        
        # Once we reach the new destination we will start 
        # a new head scan so reset flag here
        userdata.start = True
        
        return 'done'

The MOVE state is again a SimpleActionState. This time, the action conducted is to navigate to the pose set from the waypoint data. On completion of the action, the state machine will transit to the PREPARE_FOR_HEAD_MOVE state. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The remaining code for this state machine is contained in the Mission1StateMachine class. It constructs the state machine from the previously described classes. There are also a number of functions that are called on completion of the Simple Action States. These are mostly concerned with reporting if the action was cancelled or aborted.

# Child (derived) class. Parent class is StateMachine
class Mission1StateMachine(StateMachine):
    def __init__(self, helper_obj):
        StateMachine.__init__(self, outcomes=['complete','preempted','aborted'], 
                    input_keys=['mission_data'], output_keys=['mission_status'])

        self.__helper_obj = helper_obj
        
        with self:
            # This state will prepare for the mission
            StateMachine.add('PREPARE_MISSION',
                             PrepareMission1(self.__helper_obj),
                             transitions={'ready':'PREPARE_FOR_HEAD_MOVE','error':'aborted'})
            
            # Set up action goal for deafult head position            
            default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos()
            head_goal = point_headGoal()
            head_goal.absolute = True
            head_goal.pan = default_position_pan
            head_goal.tilt = default_position_tilt
                             
            # Add the default camera position state. 
            # Which moves the head to the default position
            StateMachine.add('DEFAULT_HEAD_POSITION',
                             SimpleActionState('head_control_node',
                                               point_headAction,
                                               goal = head_goal,
                                               result_cb = self.defaultHeadPositionComplete,
                                               output_keys=['mission_status']),                                               
                             transitions={'succeeded':'PREPARE_TO_MOVE',
                                          'preempted':'preempted','aborted':'aborted'}) 
                             
            # The next state prepares for each nav goal request
            StateMachine.add('PREPARE_TO_MOVE',
                             PrepareToMove(),
                             transitions={'done':'MOVE'},
                             remapping={'current_waypoint_in':'current_waypoint',
                                        'current_waypoint_out':'current_waypoint',
                                        'waypoint_direction_in':'waypoint_direction',
                                        'waypoint_direction_out':'waypoint_direction'})
                                        
            # This state uses an Action to move the robot to the required goal
            StateMachine.add('MOVE',
                             SimpleActionState('move_base',
                                               MoveBaseAction,
                                               goal_slots=['target_pose'],
                                               result_cb = self.moveComplete,
                                               output_keys=['mission_status']),
                             transitions={'succeeded':'PREPARE_FOR_HEAD_MOVE', 
                                          'preempted':'preempted', 'aborted':'aborted'},
                             remapping={'target_pose':'destination'})
                             
            # This state will calculate the next head/camera position
            StateMachine.add('PREPARE_FOR_HEAD_MOVE',
                             PrepareToMoveHead(self.__helper_obj),
                             transitions={'scan_complete':'DEFAULT_HEAD_POSITION',
                                          'move_head':'MOVE_HEAD'},
                             remapping={'start_in':'start','start_out':'start'})
                             
            # This state will call the action to move the head/camera
            StateMachine.add('MOVE_HEAD',
                             SimpleActionState('head_control_node',
                                               point_headAction,
                                               goal_slots=['absolute','pan','tilt'],
                                               result_cb = self.moveHeadComplete,
                                               output_keys=['mission_status']),
                             transitions={'succeeded':'SCAN_FOR_FACES',
                                          'preempted':'preempted','aborted':'aborted'},
                             remapping={'absolute':'user_data_absolute',
                                        'pan':'user_data_pan','tilt':'user_data_tilt'})
                             
            # This state will call the action to scan for faces on the image from the camera
            StateMachine.add('SCAN_FOR_FACES',
                             SimpleActionState('face_recognition',
                                               scan_for_facesAction,
                                               result_slots=['ids_detected','names_detected'],
                                               result_cb = self.scanForFaceComplete,
                                               output_keys=['mission_status']),                
                             transitions={'succeeded':'CHECK_FOR_SUBJECT',
                                          'preempted':'preempted','aborted':'aborted'},
                             remapping={'ids_detected':'seen_ids',
                                        'names_detected':'seen_names'})
                             
            # This state will check the results of the scan and decide on the next action
            StateMachine.add('CHECK_FOR_SUBJECT',
                             CheckForSubject(self.__helper_obj),
                             transitions={'seen':'WAIT_FOR_USER',
                                          'not_seen':'PREPARE_FOR_HEAD_MOVE'})
                             
            # This state will wait for the acknowledge message or will timeout 
            # if the message is not received in a set time
            StateMachine.add('WAIT_FOR_USER',
                             WaitForMsg(self.__helper_obj),
                             transitions={'ack':'complete','timeout':'SCAN_FOR_FACES',
                                          'preempted':'preempted'})
            
                                                          
    def defaultHeadPositionComplete(self, userdata, status, result):
    
        self.__helper_obj.CameraAtDefaultPos(userdata, status, result)
        
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 1 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Take a message mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission number 1 aborted'
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Take a message mission failed during move head action'
            
    def moveComplete(self, userdata, status, result):
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 1 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Take a message mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission number 1 aborted'
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Take a message mission failed during base move action'
            
    def moveHeadComplete(self, userdata, status, result):
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 1 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Take a message mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission one aborted'
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Take a message mission failed during head move'
            
    def scanForFaceComplete(self, userdata, status, result):
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 1 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Take a message mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission one aborted'
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Take a message mission failed during face scan'

For more information on ROS state machines, see part 4 of the article and the documentation for the smach ROS package

Mission 4 - "Go Home"

Image 5

All of the code for this state machine is in the go_home.py file.

We enter this lower level state machine via the PREPARE_MISSION state. The mission data passed to this state contains the waypoint filename. It loads the waypoint file, constructs the nav goal for the home waypoint and as we are going to navigate autonomously, it ensures that the LIDAR is enabled. If it read the file and found a home waypoint in the file, it then transits to the DEFAULT_HEAD_POSITION state, otherwise if an error occurred, it transits back to the root state machine.

The code for the PREPARE_MISSION state is held in the PrepareMission4 class.

# PREPARE_MISSION State. Prepares the mission by loading the waypoint file
class PrepareMission4(State):   
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['ready','error'],
                       input_keys=['mission_data'],
                       output_keys=['destination','mission_status'])
        self.__helper_obj = helper_obj
        
    def execute(self, userdata):
        # mission_data will contain the filename of the file 
        # containing the waypoints including the home waypoint        
        
        # Ensure the Lidar is enabled
        self.__helper_obj.LidarEnable()
        
        # Load the waypoints
        try:
            with open(userdata.mission_data, 'r') as stream:
                try:                    
                    waypoints = yaml.load(stream)
                    next_outcome = 'ready'
                except:
                    rospy.logerr('Bad waypoint file')
                    userdata.mission_status = 'Bad waypoint file'
                    next_outcome = 'error'
        except:
            rospy.logerr("Can't open waypoint file")
            userdata.mission_status = "Can't open waypoint file"
            next_outcome = 'error'                    
        
        if next_outcome == 'ready':
            # Check the home waypoint exists
            if 'home' in waypoints:            
                # Copy the home waypoint data in to a PoseStamped message and userdata                            
                waypoint = waypoints['home']
        
                target_pose = PoseStamped()
            
                target_pose.header.frame_id = 'map'
                target_pose.pose.position.x = waypoint['position']['x']
                target_pose.pose.position.y = waypoint['position']['y']
                target_pose.pose.position.z = 0.0
                target_pose.pose.orientation.x = waypoint['orientation']['x']
                target_pose.pose.orientation.y = waypoint['orientation']['y']
                target_pose.pose.orientation.z = waypoint['orientation']['z']
                target_pose.pose.orientation.w = waypoint['orientation']['w']
            
                userdata.destination = target_pose                              
 
            else:
                rospy.logerr('No home waypoint in file')
                userdata.mission_status = 'No home waypoint in file'
                next_outcome = 'error'                  
    
        return next_outcome

The DEFAULT_HEAD_POSITION state is a SimpleActionState. It will call an action to move the head/camera to the default position. Again, when the robot moves, we want the head to face forward. Once the head move is complete, it will transit to the MOVE state. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The MOVE state is again a SimpleActionState. This time, the action conducted is to navigate to the home pose set from the waypoint data. On completion of the action, the state machine will transit back to the root state machine. The action can also be cancelled or aborted which would result in a transit back to the root state machine.

The remaining code for this state machine is contained in the Mission4StateMachine class. It constructs the state machine from the previously described classes. There are also a number of functions that are called on completion of the Simple Action States. These are mostly concerned with reporting if the action was cancelled or aborted.

# Child (derived) class. Parent class is StateMachine
class Mission4StateMachine(StateMachine):
    def __init__(self, helper_obj):
        StateMachine.__init__(self, outcomes=['complete','preempted','aborted'], 
                              input_keys=['mission_data'], output_keys=['mission_status'])
        
        self.__helper_obj = helper_obj
        
        with self:
            # This state will prepare for the mission
            StateMachine.add('PREPARE_MISSION',
                             PrepareMission4(self.__helper_obj),
                             transitions={'ready':'DEFAULT_HEAD_POSITION','error':'complete'})
            
            # Set up action goal for deafult head position            
            default_position_pan, default_position_tilt = self.__helper_obj.CameraDefaultPos()
            head_goal = point_headGoal()
            head_goal.absolute = True
            head_goal.pan = default_position_pan
            head_goal.tilt = default_position_tilt
                             
            # Add the default camera position state. Which moves the head to the default position
            StateMachine.add('DEFAULT_HEAD_POSITION',
                             SimpleActionState('head_control_node',
                                               point_headAction,                                               
                                               goal = head_goal,
                                               result_cb = self.defaultHeadPositionComplete,
                                               output_keys=['mission_status']),                               
                             transitions={'succeeded':'MOVE',
                                          'preempted':'preempted','aborted':'aborted'})                             
                                                    
            # This state uses an Action to move the robot to the required goal
            StateMachine.add('MOVE',
                             SimpleActionState('move_base',
                                               MoveBaseAction,
                                               goal_slots=['target_pose'],
                                               result_cb = self.moveComplete,
                                               output_keys=['mission_status']),
                             transitions={'succeeded':'complete', 
                                          'preempted':'preempted', 'aborted':'aborted'},
                             remapping={'target_pose':'destination'})                             
                             
                             
    def defaultHeadPositionComplete(self, userdata, status, result):
    
        self.__helper_obj.CameraAtDefaultPos(userdata, status, result)
        
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 4 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Go home mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission 4 aborted'                   
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Go home mission failed during move head action'            
                                     
    def moveComplete(self, userdata, status, result):
        if status == GoalStatus.PREEMPTED:
            # Report with voice that the mission was cancelled
            message = 'Mission number 4 cancelled'
            self.__helper_obj.Speak(message, message)
            
            # set the mission status
            userdata.mission_status = 'Go home mission cancelled'
            
        elif status == GoalStatus.ABORTED:
            # Report with voice that the mission was aborted
            message = 'Warning mission 4 aborted'
            self.__helper_obj.Speak(message, message + ':(')
            
            # set the mission status
            userdata.mission_status = 'Go home mission failed during base move action'
            
        elif status == GoalStatus.SUCCEEDED:
            userdata.mission_status = 'Go home mission complete'

Changes to the Root State Machine

All that's left is to make some changes to the rodney_missions_node.py file. This contains the root state machine which now needs to include the lower state machines each as a state.

We already had code to return the default head position but since we have also added a job to set the head to the user input angle, we need to define this position. This means we need to add some minor changes to the rodney_missions_node.py file for this functionality.

The MissionsHelper class constructor now includes code to read the angle used for user input position from the parameter server. Remember that ROS uses radians for angle values.

self.__user_input_position_pan = rospy.get_param("~head/user_position/pan", 0.0)
self.__user_input_position_tilt = rospy.get_param("~head/user_position/tilt", -0.5)

The class also includes a new function to return these values.

def UserInputPosition(self):
    return self.__user_input_position_pan, self.__user_input_position_tilt

The PREPARE state of the root state machine now needs to include the code not only to set the user input angle when requested but to transit to the new lower level state machines if requested. The complete code for the PREPARE state now looks like this.

# The PREPARE state
class Prepare(State):
    def __init__(self, helper_obj):
        State.__init__(self, outcomes=['mission1','mission2',
                       'mission4','done_task','head_default','move_head'], 
                       input_keys=['mission'],
                       output_keys=['mission_data','start','user_data_absolute',
                                    'user_data_pan','user_data_tilt'])
        self.__helper_obj = helper_obj
    
    def execute(self, userdata):        
        # Based on the userdata either change state to the required mission or 
        # carry out single job userdata.mission contains the mission or single 
        # job and a number of parameters separated by '^'
        retVal = 'done_task';
        
        # Split into parameters using '^' as the delimiter
        parameters = userdata.mission.split("^")
        
        if parameters[0] == 'M1':
            # Mission 1 to search for a known person and deliver a message
            # parameter[1] contains filename of file containing poses for the patrol,
            # the id of the person to search for and the message to deliver            
            userdata.mission_data = parameters[1]
            retVal = 'mission1'        
        elif parameters[0] == 'M2':
            # Mission 2 is scan for faces and greet those known, there are no
            # other parameters with this mission request
            userdata.start = True
            retVal = 'mission2'
        elif parameters[0] == 'M4':
            # Mission 4 is go home. parameter[1] contains filename of file containing poses
            userdata.mission_data = parameters[1]
            retVal = 'mission4'
        elif parameters[0] == 'J1':
            # Simple Job 1 is play a supplied wav file and move the face lips           
            # Publish topic for speech wav and robot face animation
            self.__helper_obj.Wav(parameters[1], parameters[2])
        elif parameters[0] == 'J2':
            # Simple Job 2 is to speak the supplied text and move the face lips
            # Publish topic for speech and robot face animation
            self.__helper_obj.Speak(parameters[1], parameters[2])
        elif parameters[0] == 'J3':
            # Simple Job 3 is to move the head/camera. This command will only
            # be sent in manual mode.
            # parameters[1] will either be 'u', 'd', 'c', 'i' or '-'
            # parameters[2] will either be 'l', 'r' or '-'
            # Check for command
            if 'c' in parameters[1]:
                # Move to default position
                retVal = 'head_default'
            elif 'i' in parameters[1]:
                # Move to user input position. This position is a good position 
                # for user input at the screen
                pan_position, tilt_position = self.__helper_obj.UserInputPosition()
                userdata.user_data_absolute = True # This will be a actual position move
                userdata.user_data_pan = pan_position
                userdata.user_data_tilt = tilt_position
                retVal = 'move_head'
            else:                
                relative_request_pan, relative_request_tilt = 
                     self.__helper_obj.CameraManualMove(parameters[1]+parameters[2])                
                # Set up user data that will be used for goal in next state
                userdata.user_data_absolute = False # This will be a relative move
                userdata.user_data_pan = relative_request_pan
                userdata.user_data_tilt = relative_request_tilt
                retVal = 'move_head'                
        elif parameters[0] == 'J4':
            # Simple job to toggle the LIDAR on/off
            self.__helper_obj.ToggleLidar()

        return retVal

The remaining changes are to the RodneyMissionNode class simply add the new lower level state machines to the root state machine.

This is an example of adding a new state machine to the root:

# ------------------------- Sub State machine for mission 1 ---------------------
# Create a sub state machine for mission 1 - take a message to
self.__sm_mission1 = missions_lib.Mission1StateMachine(self.__missions_helper)
           
# Now add the sub state machine for mission 1 to the top level one
StateMachine.add('MISSION1', 
                 self.__sm_mission1, 
                 transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'})

We must also make a call to pre-empt the lower level state machine as it is running when a message to cancel a mission is received. The CancelCallback function now looks like this:

# Callback for cancel mission message
def CancelCallback(self, data):
    # If a sub statemachine for a mission is running then request it be preempted
    if self.__sm_mission1.is_running():
        self.__sm_mission1.request_preempt() 
    elif self.__sm_mission2.is_running():
        self.__sm_mission2.request_preempt() 
    elif self.__sm_mission4.is_running():
        self.__sm_mission4.request_preempt() 

Robot Hardware

A full size image of the current circuit diagram is available in the diagrams zip folder along with a full size copy of the image from rqt_graph showing all the nodes and topics.

A complete bill of material for the project so far is available here.

In part 1 of the article, I referenced the Ubiquity Robot Image which I use on the Raspberry Pi. Instructions on how to install the image, install extra software and configure it for the project are available here.

Using the Code

As usual, I'll run the code on the robot hardware and run the test tools and manual control nodes on a Linux PC. I'll refer to this PC as the workstation in the details below.

Building the ROS Packages on the Pi (Robot Hardware)

If not already done, create a catkin workspace on the Raspberry Pi and initialize it with the following commands:

$ mkdir -p ~/rodney_ws/src
$ cd ~/rodney_ws/
$ catkin_make

Copy the packages face_recognition, face_recognition_msgs, head_control, imu_calib, pan_tilt, pi_io, rodney, rodney_missions, ros-keyboard, rplidar-ros, servo_msgs, speech, tacho_msgs and thunderborg into the ~/rodney_ws/src folder.

Build the code with the following commands:

$ cd ~/rodney_ws/ 
$ catkin_make

Check that the build completes without any errors.

You will also need to compile and download the sketch to the Teensy 3.5.

Building the ROS Packages on the Workstation

On the workstation, we want to run the keyboard, joystick and heartbeat nodes so that we can control the actual robot hardware remotely.

Create a workspace with the following commands:

$ mkdir -p ~/test_ws/src 
$ cd ~/test_ws/ 
$ catkin_make

Copy the packages rodney, joystick, and ros-keyboard into the ~/test_ws/src folder, and then build the code with the following commands:

$ cd ~/test_ws/ 
$ catkin_make

Check that the build completes without any errors.

Tips

When running ROS code and tools on a workstation and the Raspberry Pi, there can be a lot of repeat typing of commands at a number of terminals. In the next sections, I have included the full commands to type but here are a few tips that can save you all that typing.

On the Raspberry Pi, to save typing "source devel/setup.bash", I have added it to the .bashrc file for the Raspberry Pi.

$ cd ~/
$ nano .bashrc

Then add "source /home/ubuntu/rodney_ws/devel/setup.bash" to the end of the file, save and exit.

When running test code and tools on the workstation, it also needs to know where the ROS master is so I have added the following to the .bashrc file for the workstation.

alias rodney='source ~/test_ws/devel/setup.bash; \
export ROS_MASTER_URI=http://ubiquityrobot:11311'

Then, by just typing "rodney" at a terminal, the two commands are run and a lot of typing is saved.

You can also save some typing as some ROS tools support TAB completion. For example, type rosrun rosserial_ and then press the tab key to auto complete rosrun rosserial_python.

Waypoints

How's your knowledge of quaternions? Are you happy to set the orientation values of each waypoint to match the direction you want the robot to face when it reaches the waypoint? If you answered "brilliant" and "yes" to those questions, then go ahead and create your waypoint file. If on the other hand, like me, you would like some help, then follow this next bit.

We are going to start the robot code and rviz and use the navigation tools in rviz to set a nav goal just like we did in the last article in this series.

On the robot hardware, run the following commands to start all the current nodes in the system. I'm going to use my default map but you can set the map by adding "map_file:=my_map_filename" to the end of the roslaunch command.

$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch

On the workstation, run the following commands to start the remote control node:

$ source test_ws/devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ roslaunch rodney remote.launch

A small window whose title is "ROS keyboard input" should be running. When entering keyboard strokes, ensure the small window has the focus.

Give this window the focus and press 'm' on the keyboard to set the robot to manual mode so that is does not move in the steps below.

On the workstation, run the following commands in another terminal to start rviz:

$ source test_ws/devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ roslaunch rodney rviz.launch

On the workstation, run the following commands in another terminal:

$ rostopic echo /move_base/goal

Now select a waypoint position by clicking the "2D Nav Goal" button, click/drag the large green arrow on the map just like setting a goal.

The goal values will be displayed by the rostopic echo terminal. Copy the values to your waypoint file. Repeat for each waypoint you want to add to your file. Don't forget to set a home waypoint in the file.

Image 6

Shutdown the robot and workstation code.

Testing

Test Setup

On the robot hardware, run the following commands to start all the current nodes in the system. I'm going to use my default map but you can set the map by adding "map_file:=my_map_filename" to the end of the roslaunch command. This time, your waypoint file should also get loaded.

$ source rodney_ws/devel/setup.bash
$ roslaunch rodney rodney.launch

On the workstation, run the following commands to start the remote control node:

$ source test_ws/devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ roslaunch rodney remote.launch

A small window whose title is "ROS keyboard input" should be running. When entering keyboard strokes, ensure the small window has the focus.

On the workstation, run the following commands in another terminal to start rviz:

$ source test_ws/devel/setup.bash 
$ export ROS_MASTER_URI=http://ubiquityrobot:11311 
$ roslaunch rodney rviz.launch

User Input Position

Check that the bi-coloured LED illuminates green as the robot is not running a mission.

Press the black pushbutton, the head should move to the user input position.

Localise the Robot

Before we can let the robot loose, we still need to manually localise the robot like we did in the last article. I hope to add some self localising in the future but for now, configure rviz to display the robot model or base_link axis, the laser scan, map and pose estimates. Also ensure that the map is the fixed frame.

We can see from the display that the laser scan does not match the map, the pose estimates are spread around. So before we give the robot a navigational goal, we need to improve its localisation.

Image 7

The first operation we will carry out is to give the robot an improved localisation using rviz. Click the "2D Pose Estimate" button, estimate the real location and pose of the robot and click/drag the large green arrow on the map to set the initial pose. You can keep doing this until you get the laser scan close to matching the map.

We now have a good initial pose but the pose estimates are still out. We can improve these by driving the robot around in manual mode. Spinning on the spot is a good manoeuvre to conduct. Whilst moving the robot, you should see the pose estimates converging on the robots position in rviz.

Go Home

From the previous steps, the robot will still be in manual mode so teleop the robot so it is in a different location to the home waypoint.

Now press the yellow pushbutton. The bi-coloured LED illuminates red and the robot should navigate itself to the home location.

Cancel a Mission

Put the robot in manual mode and teleop away from the home position. Again press the yellow pushbutton but this time before the robot reaches the home location, press the black button to cancel the mission. The robot should stop moving and the bi-coloured LED should illuminate green.

Take a Message to

Well here, we are just about to test every bit of code we have written for our robot!

The test code in the rodney package for this mission is going to result in the robot searching for the person who is ID number '1' in your face recognition subjects. That was set back way in part 2 of these articles. The message delivered is also going to ask that person to take our family dog for a walk. I guess you may have spotted that and changed the ID and or message by now if building your own robot.

Give the ROS keyboard input window the focus and pressing the "1" key (not on the numeric keypad). The robot should visit each waypoint in turn and at each waypoint, move its head searching for the person who is ID number '1'.

When the subject is found, the message will be delivered. Sixty seconds after the message is delivered, if no action is taken, the robot will resume the search by first checking to see if the subject it still in the current camera image. Once the message is delivered for a second time, press the yellow button to acknowledge the message.

Points of Interest

We have now almost completed our initial idea for this robotics project. In this next part which I also expect to be the last part, I'll look at:

  • Assigning the mission and mission data from a web browser, which will complete Design Goal 6
  • Run mission 2, the greeting mission, on power up
  • See if we can get the robot to do a self localisation manoeuvre

Whilst I include the current version of all the code under development in the source zip file, the code is also available at my GitHub Site. Each package is in its own repository. Please feel free to Fork the repositories for your own projects make updates and if you feel any changes will be helpful to myself or others, then create a Pull Request.

History

  • 2019/05/03: Initial release

License

This article, along with any associated source code and files, is licensed under The Apache License, Version 2.0

Share

About the Author

Phil Hopley
Software Developer (Senior)
United Kingdom United Kingdom
Having spent the last 40 years as an engineer, 19 years as a test and commissioning engineer and 21 years as a software engineer, I have now retired to walk the Cumbrian fells and paddle the coast and lakes. When the weather is inclement I keep my hand in with robotic software and hardware. Over the years I have used Code Project to help me solve some programming issues so now with time on my hands it’s time to put something back into the Code Project.

Comments and Discussions

 
QuestionMessage Closed Pin
3-May-19 9:32
memberMember 143541993-May-19 9:32 
QuestionThis series is epic Pin
Sacha Barber3-May-19 0:42
mvaSacha Barber3-May-19 0:42 
PraiseRe: This series is epic Pin
Phil Hopley4-May-19 7:11
professionalPhil Hopley4-May-19 7:11 

General General    News News    Suggestion Suggestion    Question Question    Bug Bug    Answer Answer    Joke Joke    Praise Praise    Rant Rant    Admin Admin   

Use Ctrl+Left/Right to switch messages, Ctrl+Up/Down to switch threads, Ctrl+Shift+Left/Right to switch pages.

Article
Posted 3 May 2019

Stats

4.1K views
176 downloads
6 bookmarked