114 lines
4.1 KiB
Python
114 lines
4.1 KiB
Python
|
"""
|
||
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
||
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
||
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
||
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
||
|
|
|
||
|
|
This program is free software; you can redistribute it and/or modify
|
||
|
|
it under the terms of the GNU General Public License as published by
|
||
|
|
the Free Software Foundation; either version 2 of the License, or
|
||
|
|
(at your option) any later version.
|
||
|
|
|
||
|
|
This program is distributed in the hope that it will be useful,
|
||
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
GNU General Public License for more details.
|
||
|
|
|
||
|
|
You should have received a copy of the GNU General Public License
|
||
|
|
along with this program; if not, write to the Free Software
|
||
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||
|
|
|
||
|
|
If you wish to redistribute ARIA under different terms, contact
|
||
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
||
|
|
robots@mobilerobots.com or
|
||
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
||
|
|
"""
|
||
|
|
from AriaPy import *
|
||
|
|
import sys
|
||
|
|
|
||
|
|
# Shows how to add a task callback to ArRobot's synchronization/processing cycle
|
||
|
|
#
|
||
|
|
# This program will just have the robot wander around, it uses some aance
|
||
|
|
# routines, then just has a constant velocity. A sensor interpretation task callback is invoked
|
||
|
|
# by the ArRobot object every cycle as it runs, which records the robot's current
|
||
|
|
# pose and velocity.
|
||
|
|
#
|
||
|
|
# Note that tasks must take a small amount of time to execute, to adelaying the
|
||
|
|
# robot cycle.
|
||
|
|
#
|
||
|
|
# Callbacks are done differently in Python than in C++. In the Aria C++
|
||
|
|
# library, a class called ArFunctor is used to store
|
||
|
|
# a function pointer, and an instance of an object. Different ArFunctor
|
||
|
|
# subclasses are used for functions with different arguments, and for global
|
||
|
|
# functions which are not object methods. Instead, in the Python wrapper, you
|
||
|
|
# can simply pass a function (whether defined globally, or bound to an object
|
||
|
|
# if defined as a method, or created on the fly as a 'lambda' expression)
|
||
|
|
# whenever the Aria API calls for an ArFunctor*. The most common use is shown
|
||
|
|
# below: a method bound to an object is passed directly as a sensor interpretation
|
||
|
|
# task.
|
||
|
|
|
||
|
|
|
||
|
|
class PrintingTask:
|
||
|
|
|
||
|
|
# Constructor. Adds a sensor interpretation task to the given robot object.
|
||
|
|
def __init__(self, robot):
|
||
|
|
self.myRobot = robot
|
||
|
|
robot.addSensorInterpTask("PrintingTask", 50, self.doTask)
|
||
|
|
|
||
|
|
# This method will be called by ArRobot as a sensor interpretation task
|
||
|
|
def doTask(self):
|
||
|
|
print "x %6.1f y %6.1f th %6.1f vel %7.1f mpacs %3d" % (self.myRobot.getX(), self.myRobot.getY(), self.myRobot.getTh(), self.myRobot.getVel(), self.myRobot.getMotorPacCount())
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
Aria_init()
|
||
|
|
|
||
|
|
parser = ArArgumentParser(sys.argv)
|
||
|
|
robot = ArRobot()
|
||
|
|
con = ArRobotConnector(parser, robot)
|
||
|
|
|
||
|
|
if not con.connectRobot():
|
||
|
|
print "Could not connect to robot, exiting"
|
||
|
|
Aria_logOptions()
|
||
|
|
Aria_exit(1)
|
||
|
|
|
||
|
|
|
||
|
|
sonar = ArSonarDevice()
|
||
|
|
|
||
|
|
# This object encapsulates the task we want to do every cycle.
|
||
|
|
# Upon creation, it puts a callback functor in the ArRobot object
|
||
|
|
# as a 'user task'.
|
||
|
|
pt = PrintingTask(robot)
|
||
|
|
|
||
|
|
# actions used to wander
|
||
|
|
recover = ArActionStallRecover()
|
||
|
|
aFront = ArActionAvoidFront()
|
||
|
|
constantVelocity = ArActionConstantVelocity("Constant Velocity", 400)
|
||
|
|
|
||
|
|
|
||
|
|
robot.addRangeDevice(sonar)
|
||
|
|
|
||
|
|
if not con.connectRobot(robot):
|
||
|
|
print "Could not connect to the robot."
|
||
|
|
Aria_exit(2)
|
||
|
|
print "Connected to the robot. (Press Ctrl-C to exit)"
|
||
|
|
|
||
|
|
|
||
|
|
# turn on the motors, turn off amigobot sounds
|
||
|
|
robot.comInt(ArCommands.ENABLE, 1)
|
||
|
|
robot.comInt(ArCommands.SOUNDTOG, 0)
|
||
|
|
|
||
|
|
# add the wander actions
|
||
|
|
robot.addAction(recover, 100)
|
||
|
|
robot.addAction(aFront, 50)
|
||
|
|
robot.addAction(constantVelocity, 25)
|
||
|
|
|
||
|
|
# Start the robot process cycle running. Each cycle, it calls the robot's
|
||
|
|
# tasks. When the PrintingTask was created above, it added a new
|
||
|
|
# task to the robot. '1' means that if the robot connection
|
||
|
|
# is lost, then ArRobot's processing cycle ends and self call returns.
|
||
|
|
robot.run(1)
|
||
|
|
|
||
|
|
print "Disconnected. Goodbye."
|
||
|
|
|