Kevin J. Walchko
created 20 Oct 2017
Now we are going to start getting the Roomba to move. In order to do that, we need to program the robot correctly. Students usually are lazy and write a lazy software flow for their robot and then don't understand why things crash or don't work like they want. Once we understand the Create 2 API and program a modular/logical software flow, we will develop a simple algorithm to get our robot to do obstacle avoidance.
In [1]:
%matplotlib inline
from __future__ import print_function
from __future__ import division
import numpy as np
The Robot Operating System (ROS) is a flexible framework for writing robot software. It is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms.
Why? Because creating truly robust, general-purpose robot software is hard. From the robot's perspective, problems that seem trivial to humans often vary wildly between instances of tasks and environments. Dealing with these variations is so hard that no single individual, laboratory, or institution can hope to do it on their own.
As a result, ROS was built from the ground up to encourage collaborative robotics software development. For example, one laboratory might have experts in mapping indoor environments, and could contribute a world-class system for producing maps. Another group might have experts at using maps to navigate, and yet another group might have discovered a computer vision approach that works well for recognizing small objects in clutter. ROS was designed specifically for groups like these to collaborate and build upon each other's work.
roscore as the central pub/sub broker, moving large amounts of data around in messages (i.e., 3D lidar point clouds, large images, etc) can introduce unnecessary delays and CPU overheadThis class will not throw you into the deep end with ROS. Instead we are just using Python, but you can take these ideas and use them when you build a complex robot.
multiprocessing)Python, like C/C++ and most other languages support both threads and processes
events, queues, and namespaces ... you cannot share global variablesIf you are interested in doing something like this, google: python multiprocessing and a bunch of examples will come up.
The example below, creates a new process that uses a shared namespace with other processes. This bridges the gap between threads (which share global variables) and processes that typically don't.
#!/usr/bin/evn python
import multiprocessing
class MyFancyClass(object):
def __init__(self, name):
self.name = name
def do_something(self):
proc_name = multiprocessing.current_process().name
print 'Doing something fancy in %s for %s!' % (proc_name, self.name)
def worker(q):
obj = q.get()
obj.do_something()
# you have to have this if statement here
if __name__ == '__main__':
queue = multiprocessing.Queue()
p = multiprocessing.Process(target=worker, args=(queue,))
p.start()
queue.put(MyFancyClass('Fancy Dan'))
# Wait for the worker to finish
queue.close()
queue.join_thread()
p.join()
#!/usr/bin/evn python
import multiprocessing
def producer(ns, event):
ns.value = 'This is the value'
ns.array = [1,2,3] # for dynamic objects, you need to assign them to update the namespace
ns.fail.append(1) # does not update the global namespace! Do the above method
event.set() # let the other process know it is ok
def consumer(ns, event):
try:
value = ns.value
except Exception, err:
print 'Before event, consumer got:', str(err)
event.wait()
print 'After event, consumer got:', ns.value
if __name__ == '__main__':
mgr = multiprocessing.Manager()
namespace = mgr.Namespace()
namespace.fail = [] # this is setup to show you a failure
event = multiprocessing.Event() # use for communications between processes
p = multiprocessing.Process(target=producer, args=(namespace, event))
c = multiprocessing.Process(target=consumer, args=(namespace, event))
c.start()
p.start()
c.join()
p.join()
The output is:
$ python multiprocessing_namespaces.py
Before event, consumer got: 'Namespace' object has no attribute 'value'
After event, consumer got: This is the value
So we have already talked about mobile robots, coordinate systems, body frames, etc. Now we are going to talk about how do we command the robot and get it to go where we want.
from pycreate2 import Create2
# Create a Create2.
port = "path-to-serial-port"
bot = Create2(port)
# Start the Create 2
bot.start()
# Put the Create2 into 'safe' mode so we can drive it
# This will still provide some protection, when it senses danger it will stop
bot.safe()
# directly set the motor speeds ... go forward
bot.drive_direct(100, 100) # inputs for motors are +/- 500 max
time.sleep(2)
# turn in place, CW
bot.drive_direct(200,-200)
time.sleep(2)
Let's start off simple. Let's define a couple of states and transitions between them. Understand there is more than one way to do this, but we will give you an example.
Start: The initial state. On start-up, the robot will initialize and setup some things, then immediately tranision the Wander state.
Wander: When the robot enters into this state it starts to wander around.
Let's maybe try to make it a little more robust. Now we create a couple more transitions.
Actually you can write a rather professional, modular, and clean architecture with Python. Remember to always set up your system properly and tear it down when you shutdown. Killing your software with Ctrl-C (essentially causing it to crash) is sloppy and can leave you in a bad state.
Think about this. Your program just commanded the robot to go forward at full speed. Then you quit your program. There was never a command sent to the robot to stop ... oops. So try to think about this when you program your robot. There are some safety things I have put in the code so you don't do anything too stupid, but I have not robustly tested the Roomba software either.
The following software suggestions are primarily functional programming. However, I have also mixed in some class programming too. Feel free to program however you want.
This basically holds the functions (or classes if you prefer) that tell your robot what to do. There is a function for each state.
#!/usr/bin/env python
from __future__ import print_function, division
def Start(robot):
# setup things for our robot
robot.bot.camera = Camera('pi')
robot.bot.camera.init(window=(640,480))
# stuff happens here
return 'STATE_WANDER'
def ObstacleAvoid(robot):
# for our simple 2D robot, this function isn't too complex
# read sensors
sensors = robot.bot.get_sensors()
# stuff happens here
next_state = 'STATE_WANDER'
return next_state
def Wander(robot):
# remember, this could be really complex code
# stuff happens here
return 'STATE_OBSTACLE_AVOID'
def Stop(robot):
# shutdown things for our robot
return 'STATE_OBSTACLE_AVOID'
# this simple hash is just a simple way to keep track of everything
# it is not necessary to do this, but can help to keep things organized.
StateArray = {
STATE_START: Start,
STATE_OBSTACLE_AVOID: ObstacleAvoid,
STATE_WANDER: Wander,
STATE_STOP: Stop
}
# Now you can access these using:
#
# StateArray[STATE_START](robot)
#
# Or, if you hate hashes, use an array like this:
#
# StateArray = [Start, ObstacleAvoid, Wander, Stop]
#
# Thn you could access your functions like this:
#
# StateArray[0](robot)
#
# Note, in this method, you have to keep track of what index 0, 1, 2, ... is.
# Either way will give you the same thing, but the first is more readable
Notice the interface for each of these is the same robot argument. This is nice, because it leads to modular code! I can easily add new states without changing any other function or code because all of the states take the same inputs. This is good design, but it takes planning to do this.
Here is your robot file that acts like the actual state machine and determines how you transition to the next state based off of outputs from the previous state.
#!/usr/bin/env python
from __future__ import print_function, division
from states import StateArray
from pycreate2 import Create2 # Roomba driver
from nxp_imu import IMU # imu driver
from time import sleep
class MyRobot(object):
"""
This is a super simple class to hold things together.
Unfortunately you are not trained like the rest of the world to
program with classes, so I want to keep this simple. However, if
you have any talent, then please try to do this properly. In 1993
at Univ of Florida we spent half of the semester in our C++ class
learning Object Oriented Programming (OOP). If you took the data
structures class, then you learned Java ... all class based.
"""
bot = None
camera = None
imu = None
def __init__(self, port):
# this function is automatically run when I call MyRobot()
# It sets up everything I need
self.bot = Create2(port)
def __del__(self):
# this function is automatically called when MyRobot() goes
# out of scope or the program is ended
self.bot.safe()
self.bot.close()
self.camera.close()
self.imu.close()
if __name__ == "__main__":
port = "/dev/ttyUSB0" # serial port path, change as appropriate for your robot
robot = MyRobot(port)
current_state = 'STATE_START'
try:
while True:
current_state = StateArray[current_state](robot)
sleep(0.1) # run this loop at 10 Hz forever or until Ctrl-C is pressed
except KeyboardInterrupt:
print('User hit Ctrl-C, robot shutting down\n\nBye ...')
# depending on how you program things, you might have to tell the robot
# to stop
Now, for whatever reason, I broke up my state function into one python file (like a library) and my actual run-time loop into another. You don't have to do that. I try to keep things organized and as stuff gets more complex, I break them out into different files so I don't have just one python file with 1000 lines of code in it.
Ok, so you did some obstacle avoidance in ECE 382. Depending on how well you did (or how much help you got from your friends) you maybe made it through the maze. Basically, we will start there.
The block Turn seems simple ... yes it is. We are starting simple. Later when we do path planning, this could be much more complex (you will see). Think how complex it would be if we were using quadcopters and we detected an obstacle in 3D space as opposed to our Roomba who lives on a 2D floor. Remember, we are starting simple like ECE382 robot maze.
pycreate library has some simple examples for you to follow.pycreate reference link above, what type of sensors are available for you to access?
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.