PDA

View Full Version : An robot programming idea in Python



Pi Robot
07-08-2010, 10:51 AM
Over the last few weeks I have been learning Python and slowly migrating my Pi Robot C# code using PyDev, IronPython and Eclipse under Windows XP. Ultimately it might be nice to use ROS on Linux but for the moment I am stuck on Windows since I rely on the Serializer controller board which comes with a .Net library.

So I have implemented a simple idea that is a kind of poor man's ROS but without running a separate process for each sensor or actuator node and I was wondering what people think of this approach.

I start with a MessageBoard class that is used to store and retrieve sensor and actuator values. It looks like this:


import threading

class MessageBoard():
def __init__(self):
self.Messages = dict({})
self.messageLock = threading.Lock()

def pubValue(self, topic, value):
with self.messageLock:
self.Messages[topic] = value

def getValue(self, topic):
with self.messageLock:
if self.Messages.has_key(topic):
return self.Messages[topic]
else: return NoneAs you can see, this is basically a glorified dictionary of key-values pairs where the key is a label for the "topic" of the entry and the value is typically a sensor or actuator reading. For example, if you have a sonar sensor on the head of your robot, its topic label could be "SonarHead". To set this topic to the current reading from the sonar sensor, you would use:

myMessages = MessageBoard()
myMessages.pubValue("SonarHead", getSonarHeadReading())

where getSonarHeadReading() is a direct call to the sonar sensor as appropriate for your controller board. To then get the current sonar reading from the message board, you would use:

currentSonarHeadReading = myMessages.getValue("SonarHead")

I lock the getValue() and pubValue() methods since, depending on who you believe, Python dictionaries either are or are not thread-safe. (Some say they are atomic operations, others say not.) The advantage of using methods to retrieve/set the dictionary entries is that the methods could be changed at a later time to support socket read/writes or other such channels.

My robot uses a USB2Dynamixel controller with a bus of 11 Dynamixel AX-12 servos. So I run a separate thread that polls and sets various servo values by way of the MessageBoard:


import threading, time

class ActuatorThread(threading.Thread):
def __init__(self, myDynaNet, myActuators, myMessages):
threading.Thread.__init__(self)
self.finished = threading.Event()
self.daemon = True
self.myDynaNet = myDynaNet
self.myActuators = myActuators
self.myMessages = myMessages
self.Interval = 0.05 #50ms

def run(self):
while not self.finished.isSet():
self.myDynaNet.Synchronize()
for aName in self.myActuators.All:
self.aActuator = self.myActuators.All[aName]

self.myMessages.pubValue(aName + "_CurrentPosition", self.aActuator.getCurrentPosition())
self.myMessages.pubValue(aName + "_CurrentSpeed", self.aActuator.getCurrentSpeed())
self.myMessages.pubValue(aName + "_isMoving", self.aActuator.isMoving())
self.myMessages.pubValue(aName + "_CurrentLoad", self.aActuator.getCurrentLoad())
self.myMessages.pubValue(aName + "_CurrentTemperature", self.aActuator.getCurrentTemperature())

TorqueEnabled = self.myMessages.getValue(aName + '_TorqueEnabled')
CurrentTorqueEnabled = self.aActuator.getTorqueEnabled()
if TorqueEnabled != None and TorqueEnabled != CurrentTorqueEnabled:
self.aActuator.setTorqueEnabled(TorqueEnabled)

if (TorqueEnabled != False):
GoalPosition = self.myMessages.getValue(aName + '_GoalPosition')
CurrentGoalPosition = self.aActuator.getGoalPosition()
if GoalPosition != None and GoalPosition != CurrentGoalPosition:
self.aActuator.setGoalPosition(GoalPosition)

MovingSpeed = self.myMessages.getValue(aName + '_MovingSpeed')
CurrentMovingSpeed = self.aActuator.getMovingSpeed()
if MovingSpeed != None and MovingSpeed != CurrentMovingSpeed:
self.aActuator.setMovingSpeed(MovingSpeed)

if self.myMessages.getValue(aName + "_StopRequested"):
self.aActuator.stop()
self.myMessages.pubValue(aName + "_StopRequested", False)

time.sleep(self.Interval)

def stop(self):
print "Stopping servos ...",
self.finished.set()
self.join()
print "Done."As you can see, the ActuatorThread polls the servos every 50ms and either publishes current values to the MessageBoard, or uses new values from the MessageBoard to command the servos accordingly. For example, the method:

self.aActuator.getCurrentPosition()

is implemented as:


def getCurrentPosition(self):
return self.Dyn.CurrentPositionwhere the Dyn object is implemented by the Forest Moon library for now.

I have another thread running that communicates with RoboRealm and publishes its variable values to the MessageBoard like this:


import threading, time
import pi.nodes.services.RoboRealmAPI as RoboRealm
import pi.nodes.robots.pi_config as Config

class RoboRealmThread(threading.Thread):
def __init__(self, myMessages):
threading.Thread.__init__(self)
self.finished = threading.Event()
self.daemon = True
self.Interval = 0.05 # 50ms
self.myMessages = myMessages
self.RR = RoboRealm.RR_API()
self.RR.Connect("localhost")
width, height = self.RR.GetDimension()
Config.videoWidth = width
Config.videoHeight = height
self.halfWidth = int(width) / 2
self.halfHeight = int(height) / 2
print "Image Width: ", width, "Height: ", height

def run(self):
while not self.finished.isSet():
try:
COG_X = int(self.RR.GetVariable("COG_X")) - self.halfWidth
COG_Y = int(self.RR.GetVariable("COG_Y")) - self.halfHeight
Attention_Status = "Alert"
except ValueError:
COG_X = 0
COG_Y = 0
Attention_Status = "Waiting"
self.myMessages.pubValue("COG_X", COG_X)
self.myMessages.pubValue("COG_Y", COG_Y)
self.myMessages.pubValue("Attention_Status", Attention_Status)
time.sleep(self.Interval)

def stop(self):
print "Stopping RoboRealm thread ...",
self.finished.set()
self.join()
print "Done."
Using these two threads with a third HeadTracking thread, I am able to get really smooth and responsive head tracking of a colored object. The HeadTracking thread looks like this:


import threading, time
import pi.nodes.robots.pi_config as Config

class HeadTrack(threading.Thread):
def __init__(self, myActuators, myMessages):
threading.Thread.__init__(self)
self.finished = threading.Event()
self.daemon = True
self.myActuators = myActuators
self.myMessages = myMessages
self.Interval = 0.05 # 50ms
self.reactionTime = 0.25
self.lostFrameCount = 0
self.lostFrameCountMax = 25
self.kX = Config.videoFOV_X * Config.maxDynaServoInput / (Config.videoWidth * Config.maxDynaServoSpeed * self.reactionTime);
self.kY = Config.videoFOV_Y * Config.maxDynaServoInput / (Config.videoHeight * Config.maxDynaServoSpeed * self.reactionTime);
self.speedPanThreshold = 5
self.speedTiltThreshold = 5

def run(self):
while not self.finished.isSet():
if self.myMessages.getValue("Attention_Status") != "Alert":
self.lostFrameCount += 1
time.sleep(self.Interval)
if self.lostFrameCount > self.lostFrameCountMax:
self.myMessages.pubValue("HeadPan_GoalPosition", self.myActuators.HeadPan.StartPosition)
self.myMessages.pubValue("HeadTilt_GoalPosition", self.myActuators.HeadTilt.StartPosition)
continue

self.lostFrameCount = 0

servoInputPan = (int)(self.kX * self.myMessages.getValue("COG_X"))
servoInputTilt = (int)(self.kY * self.myMessages.getValue("COG_Y"));
servoInputPan = max(-Config.maxDynaServoInput, min(servoInputPan, Config.maxDynaServoInput))
servoInputTilt = max(-Config.maxDynaServoInput, min(servoInputTilt, Config.maxDynaServoInput))
if abs(servoInputPan) < self.speedPanThreshold: servoInputPan = 1
if abs(servoInputTilt) < self.speedTiltThreshold: servoInputTilt = 1

if servoInputPan < 0: self.myMessages.pubValue("HeadPan_GoalPosition", self.myActuators.HeadPan.Max)
else: self.myMessages.pubValue("HeadPan_GoalPosition", self.myActuators.HeadPan.Min)

if servoInputTilt > 0: self.myMessages.pubValue("HeadTilt_GoalPosition", self.myActuators.HeadTilt.Min)
else: self.myMessages.pubValue("HeadTilt_GoalPosition", self.myActuators.HeadTilt.Max)

self.myMessages.pubValue("HeadPan_MovingSpeed", min(Config.maxSafeDynaServoSpeedInput, max(Config.minDynaServoInput, abs(servoInputPan) + 1)))
self.myMessages.pubValue("HeadTilt_MovingSpeed", min(Config.maxSafeDynaServoSpeedInput, max(Config.minDynaServoInput, abs(servoInputTilt) + 1)))
time.sleep(self.Interval)

def stop(self):
print "Stopping HeadTrack thread ...",
self.finished.set()
self.join()
print "Done."In any event, that's a lot of detail, but my main question is what people think of this general "Message Board" approach. I realize this is not at all a new concept but it's one that seems to sit well in my head so I am curious what others are doing along similar lines.

Thanks!
patrick

lnxfergy
07-08-2010, 01:33 PM
I've done something similar in the past -- about 18mo ago I was working on a project I called Pybotix. Once I found ROS, I basically shifted my efforts into working on ROS powered bots, but, for historical reference: There was a "core" program which comprised of three parts: a World State, an Event Router, and a Service Manager.

The World State was similar to your dictionary -- except instead of just having a dictionary, the namespace consisted of a hierarchy of actual device drivers and wrappers that grouped them. For instance, REX had 4 sonar sensors, each was a small "sensor" class which was told where to get it's data and how to format the data. The 4 sonars were then put into a "deviceGroup" which was called "sonars", these were then put into a "sensors" device groups along with the "ir" and "encoders" groups. There was a bit of odd-ball code to allow easy introspection and interesting usage of calling objects to get values/etc.

The Event Router was intended to route asynchronous callbacks and stuff -- and never really got implemented. The Service Manager handled synchronous requests. It's usage is similar to services in ROS.

With all of the World State objects and each Service, they could be loaded as either local instances of the class, or as an XML-RPC connection to a service/object somewhere else. I never really used distributed World State, but I did typically run services such as speech recognition on different machines and tied them using the XML-RPC connection.

When I found ROS, I realized, I was doing a very similar thing -- except they had 20x the manpower, and had a 100x better implementation. Something I realized after several months playing/developing this system was that the sensor/data management is one of the easier aspects -- the services/actions (as ROS calls them) are where it gets really dicey -- because it gets very asynchronous fast.

There's still some awful documentation out there: http://www.pybotix.com/ until the domain name expires.

-Fergs

Pi Robot
07-08-2010, 07:36 PM
Dang! You've put my little effort to shame with Pybotix! Looks like a really comprehensive effort. However, I agree that going forward with ROS is a good idea and my only goal right now is to keep my robot going in a way that might be compatible with ROS a little later on. Of course, my biggest hurdle to using ROS immediately is that it doesn't run under Windows. I am actually a Linux admin by day so I'm a big fan, but there are a pile of useful robotics tools on Windows that I'm not ready to give up like RoboRealm, the Serializer board and the neural network libraries from AForge.Net. I realize I have OpenCV to eventually take the place of RoboRealm, and once you get your ROS code done I guess I'll have to buy an Arbotix to replace the Serializer. :veryhappy:

--patrick

lnxfergy
07-08-2010, 08:26 PM
Dang! You've put my little effort to shame with Pybotix! Looks like a really comprehensive effort. However, I agree that going forward with ROS is a good idea and my only goal right now is to keep my robot going in a way that might be compatible with ROS a little later on. Of course, my biggest hurdle to using ROS immediately is that it doesn't run under Windows. I am actually a Linux admin by day so I'm a big fan, but there are a pile of useful robotics tools on Windows that I'm not ready to give up like RoboRealm, the Serializer board and the neural network libraries from AForge.Net. I realize I have OpenCV to eventually take the place of RoboRealm, and once you get your ROS code done I guess I'll have to buy an Arbotix to replace the Serializer. :veryhappy:

--patrick

Honestly, Pybotix was a semester-length project. I had started building REX over winter break, needed an AI/Robotics project for a spring course, and decided that fleshing out that software would be a good idea. At the time I had this vision of building a "social robot" out of Rex, of course, I instead ended up spending the entire semester building an infrastructure and never got to the "interesting" part.

As for moving forward with ROS -- it does have a distributed nature -- in theory, the Python client is basically built on just python + XML-RPC. I imagine that with some work, you could interface Roborealm + any other software running on a Windows box over an ethernet connection to the linux box (I know I'm going to have to do something similar in our lab at school, as we have a very high-end, but Windows-only, speech recognizer we want to tie into the bots). Of course, having a bot who has half his brain on a Windows box, and half on a Linux box, could lead to serious split-personality issues....

-Fergs

lnxfergy
07-08-2010, 08:46 PM
One other thing i just thought of, specifically of interest to you: have you seen omniclops? http://code.google.com/p/omniclops/

I've played a bit with it -- haven't re-assembled my setup though, and the first one was pretty shabby. It would still need the filter&flood, but you might be able to get your omni-directional vision back up and running pretty quickly under linux using some of that.

-Fergs

LinuxGuy
07-08-2010, 09:02 PM
I am actually a Linux admin by day so I'm a big fan, but there are a pile of useful robotics tools on Windows that I'm not ready to give up like RoboRealm, the Serializer board and the neural network libraries from AForge.Net.
The Serializer board is not tied to Windows or any specific library. It accepts text based commands through a serial type interface, executes them, and returns results through the same interface.

8-Dale

LinuxGuy
07-08-2010, 09:05 PM
I've done something similar in the past -- about 18mo ago I was working on a project I called Pybotix. Once I found ROS, I basically shifted my efforts into working on ROS powered bots, but, for historical reference: There was a "core" program which comprised of three parts: a World State, an Event Router, and a Service Manager.
I saw this thread and started looking at ROS. I want to see if I can build it on my BeagleBoard. If this is possible and works, then it may open up a wider possibility for BeagleBoard based robotics.

8-Dale

lnxfergy
07-08-2010, 09:17 PM
I saw this thread and started looking at ROS. I want to see if I can build it on my BeagleBoard. If this is possible and works, then it may open up a wider possibility for BeagleBoard based robotics.

8-Dale

There is a limited BeagleBoard port already I believe -- take a look around for it, possibly in the ros.org newsfeed. One of the issues is that while roscore may run on your non-intel hardware, much of the other stuff however may not be as portable from regular PC hardware (using OpenCV, Intel Thread Building Blocks, etc). Most users are running a modern Ubuntu system for their ROS work.

-Fergs

Pi Robot
07-08-2010, 09:21 PM
The Serializer board is not tied to Windows or any specific library. It accepts text based commands through a serial type interface, executes them, and returns results through the same interface.

8-Dale

Of course you are right! I forgot that I occasionally HyperTerm into the firmware prompt where you can issue all the basic sensor and motor commands. Of course, I've gotten spoiled by the .Net library and the convenience of Intellisense when using Visual Studio...

--patrick

Pi Robot
07-08-2010, 09:28 PM
As for moving forward with ROS -- it does have a distributed nature -- in theory, the Python client is basically built on just python + XML-RPC. I imagine that with some work, you could interface Roborealm + any other software running on a Windows box over an ethernet connection to the linux box (I know I'm going to have to do something similar in our lab at school, as we have a very high-end, but Windows-only, speech recognizer we want to tie into the bots). Of course, having a bot who has half his brain on a Windows box, and half on a Linux box, could lead to serious split-personality issues....

-Fergs

Yeah, that had occurred to me too. But, like you said, I'm not particularly crazy about running two machines and two OSes. I guess I could use VMware and then I would need only one machine.


One other thing i just thought of, specifically of interest to you: have you seen omniclops? http://code.google.com/p/omniclops/ And thanks for the link. I like the work Bob Mottram has done (which is a lot!).

iBot
07-09-2010, 09:45 AM
I've been reading this thread carefully and with interest to gain some idea how to better structure some of my code for re-use on multiple platforms.

I am confused when you say the actuator thread polls every 50ms. You sleep for 50ms, but also make a lot of uncached bus transactions, which would seem to slow the poll rate down to a fraction of your target.

My fear is that all these abstractions can cause one to lose the control freak capability which I am used to. I hope I am missing something.

Pi Robot
07-09-2010, 10:45 AM
Great question! I'm not sure I have the right answer so I hope others will chime in if I have this wrong. I have my AX-12+ servos set to "Synchronized" mode which I *think* means that reads and writes to the bus are cached until I do the DynaNet.Synchronize() operation every 50ms. However, I am just now reviewing the Forest Moon documentation and this is what it says about Synchronized mode:


For a Dynamixel in Synchronized mode, any change to the GoalPosition or MovingSpeed is only recorded in the cache. Then when 'DynamixelNextwork.Synchronize' is called, all position and speed data for all changed and Synchronized Dynamixels will be sent at once in one SyncWrite instruction.I wonder if this means that my other calls to read the current temperature, current position, and so on are *not* cached? I'll look at the Forest Moon source but does anyone know the answer off hand?

iBot
07-09-2010, 10:53 AM
My understanding is that the synchronize uses the sync_write and there is no sync_read. The f_m library defines the cached registers for read, and they do not seem to be the ones you use. At a couple of ms per bus read, that adds up.

Pi Robot
07-09-2010, 11:07 AM
This is really good news because it means I'm wasting piles of milliseconds the way I'm doing it now. I'm reading through the FM source code and documentation and my brain isn't working. Can you tell me how I would do a "cached read" of the register values such as temperature and current position? The source code seems to indicate that these values are never cached, but then there is a ReadAll() method that I suspect I should use...

Pi Robot
07-09-2010, 11:33 AM
Ah, I think I see now. Here is my guess on how to modify my ActuatorThread code, just showing an example of reading the current position. (I'm away from my robot until the end of the day so I can't try it out yet):


import clr
clr.AddReferenceToFileAndPath('..\External Libraries\Dynamixel.dll')
from Dynamixel import Dynamixel, Register

def run(self):
while not self.finished.isSet():
Dynamixel.ReadAll()
for aName in self.myActuators.All:
self.aActuator = self.myActuators.All[aName]
self.myMessages.pubValue(aName + "_CurrentPosition", self.aActuator.getCurrentPosition())

self.myDynaNet.Synchronize()
time.sleep(self.Interval) And then my getCurrentPosition() method becomes:


def getCurrentPosition(self):
return Dynamixel[Register.CurrentPosition]Does that look right?

iBot
07-09-2010, 11:33 AM
I guess this is my point, that building on top of libraries and architectures is good, but it makes great demands on trust and communication on capability of those components. This where this old nuts and bolts guy struggles !

There does seem some value to reading multiple registers in one transaction if they are fairly contiguous. Delays on the USB2Dynamixel are heavier on transaction overhead than data transfer time.
Also thing like temperature need only be read every second or so. Some values like load are only updated every 200 ms, so need not be read so fast. Also you don't seem to use some of the data you read.
I moved to high speed USB 2.0 to dynamixel to reduce transaction time by factor of 8, but it has some impact on processor load.
It.would be good to hear what rate you actually get.

Pi Robot
07-09-2010, 11:59 AM
Great points. And if it weren't for your "nuts and bolts" approach, I would have blindly stumbled along making all these separate register calls. I'll post back how these changes affect the performance of my robot hopefully this weekend. In the meantime, it is amazing how smooth and responsive the current head tracking is even with all the inefficiency in my code.

lnxfergy
07-09-2010, 12:03 PM
Here's my take out it all: your PC isn't going to do realtime easily, use realtime coprocessors.

My robots use an ArbotiX as a coprocessor to the PC. The ArbotiX handles realtime interpolation of the servos, closed loop control of the motors, and anything else that has to be realtime. My Python-based ROS driver queries it occasionally (configurable, I tend to use 30hz, but i've been able to get it up to 50hz) to fill the /joint_states topic under ROS. ROS has several functions related to rates of loops. Using their python "rate" object my joint_state loop runs right at my desired 30hz (+/- 1%) regardless of how much time it takes to carry out individual commands -- how they do it, I have no idea, but it works. I know the rate -- because I can use ROS's tools (rostopic in this case) to see how often a particular topic is published to...

To get up to 50hz, I'm using a sync_read command from the PC to the ArbotiX -- as it's the FTDI driver (and it's buffering) which causes most of the latency we see. The ArbotiX then queries individual servos, builds a big response packet, and sends it back.

-Fergs

LinuxGuy
07-09-2010, 01:49 PM
Here's my take out it all: your PC isn't going to do realtime easily, use realtime coprocessors.
This is exactly why I am going to use one or more slave processors, even with my BeagleBoard. Well, I also believe strongly in the distributed processing model and using the right processors for given tasks that need to be done.


My robots use an ArbotiX as a coprocessor to the PC. The ArbotiX handles realtime interpolation of the servos, closed loop control of the motors, and anything else that has to be realtime.
At present, I am hoping to use an ARC-32 board for real time processing for servos and sensors, probably with an Arduino or Sanguino type board for some dedicated tasks. One of those tasks might be continuous scanning with a front mounted pan/tilt that has two sensors.


My Python-based ROS driver queries it occasionally (configurable, I tend to use 30hz, but i've been able to get it up to 50hz) to fill the /joint_states topic under ROS. ROS has several functions related to rates of loops. Using their python "rate" object my joint_state loop runs right at my desired 30hz (+/- 1%) regardless of how much time it takes to carry out individual commands -- how they do it, I have no idea, but it works. I know the rate -- because I can use ROS's tools (rostopic in this case) to see how often a particular topic is published to...
I am very intrigued by ROS, and am continuing to look closer at it. Since I am planning to write all the control software for W.A.L.T.E.R. and A.S.T.R.I.D. in Python, ROS may fit very well for what I want to do. I just have to get my head more into ROS and figure out how everything has to work to use it.

8-Dale

RobotAtlas
07-09-2010, 09:44 PM
I'm using a mixture of Windows and Linux (actually QNX) for development in my day job and I'd say if you don't absolutely have to do it - don't do it.
Windows it great, but much more limiting when you want to make the brains for your robot.
FitPC2 is about the smallest hardware you need to run any Windows (except maybe Win CE or Win Mobile).

And for our walking robots FitPC2 is already kind of heavy.

I think ROS is just the thing we all should adopt, so that we can combine our efforts and not reinvent the wheel again and again.

RobotAtlas
07-09-2010, 09:49 PM
> When I found ROS, I realized, I was doing a very similar thing -- except they had 20x the manpower, and had a 100x better implementation.

Fergs, I think you got those numbers reversed. ;)
Maybe because you are too modest about your coding skills and also underestimate how amny people are involved in ROS now.

lnxfergy
07-09-2010, 10:38 PM
I think ROS is just the thing we all should adopt, so that we can combine our efforts and not reinvent the wheel again and again.

Well, ROS hasn't really avoided the reinventing the wheel aspect -- for instance, I know of at least 4 separate labs that have released ROS wrappers for ARToolkit. They all effectively do the same thing -- but with quite different interfaces.

It's also probably not for everyone. It's really targeted at Ubuntu systems. C++/Python are the main languages (although there is beta-level support for Java and a few other languages). The biggest thing is: the learning curve is a bit steep -- especially given what *most* hobbyists are really doing. There is a lot of very good documentation -- but it's going to take most people quite a bit of time to really get up and running well.


> When I found ROS, I realized, I was doing a very similar thing -- except they had 20x the manpower, and had a 100x better implementation.

Fergs, I think you got those numbers reversed. ;)
Maybe because you are too modest about your coding skills and also underestimate how amny people are involved in ROS now.

At the time that I found ROS, there weren't nearly as many people working on it-- but yeah, today there are a lot (I have 3 people just at my little lab using ROS on robots).

-Fergs

RobotAtlas
07-09-2010, 11:02 PM
> I know of at least 4 separate labs that have released ROS wrappers for ARToolkit.

That's unavoidable. Those 4 labs probably started at around the same time, or, more likely, just ported their existing projects to ROS. But you, Mike, now know about all 4 of them and can compare them.
And, the best part - you are not about to write the 5th, are you?
Over time, chances are at most 2 out of those 4 projects will survive and the rest will switch over or just die.

> At the time that I found ROS, there weren't nearly as many people working on it-- but yeah, today there are a lot
See, and this is just what happened in 18 months.

But please, let's continue this type of conversation on another thread I started a few minutes ago.

lnxfergy
07-09-2010, 11:11 PM
That's unavoidable. Those 4 labs probably started at around the same time, or, more likely, just ported their existing projects to ROS. But you, Mike, now know about all 4 of them and can compare them.
And, the best part - you are not about to write the 5th, are you?

Actually, our lab uses a modified version of Reactivision (a tool similar to ARToolkit), so we may... hehe

Pi Robot
07-10-2010, 08:49 AM
My understanding is that the synchronize uses the sync_write and there is no sync_read. The f_m library defines the cached registers for read, and they do not seem to be the ones you use. At a couple of ms per bus read, that adds up.

Thanks to iBot, I have modified my Dynamixel polling loop to use the ReadAll() method in the Forest Moon library instead of reading each register individually. I then read the cached values when publishing a particular value to the message board. Below are the results in terms of the fastest polling rate I can obtain comparing this new method versus my old method. The hardware setup I have is:

* USB2Dynamixel controller in TTL mode connected to a USB 2.0 port on a DLink USB hub
* USB port Latency Timer set to 1 msec down from default 16 msec
* 11 AX-12+ servos
* Windows XP SP3 on an Acer Core 2 Duo 3 Ghz PC
* Iron Python 2.6
* PySerial 2.4

My polling loop queries Current Position, Current Speed, Current Load, Current Temperature, Moving status, Torque Enable status, Moving Speed and Goal Position. It also sets Moving Speed, Goal Position and Torque Enable if these values have changed. See code block below for details.

NOTE: I do not use time.sleep() at all in the loop anymore so I am driving it as fast as it will go.

RESULTS: Average time to complete one polling loop across all 11 servos

Old Method (not using cached values): 66 msec (16 Hz)
New Method (using ReadAll): 22 msec (45 Hz)

BTW, these numbers hold up even when using RoboRealm in a second thread to head track a colored object at 30 frames per second.

Here now is the new polling loop:


class ActuatorThread(threading.Thread):
def __init__(self, myDynaNet, myActuators, myMessages):
threading.Thread.__init__(self)
self.finished = threading.Event()
self.daemon = True
self.myDynaNet = myDynaNet
self.myActuators = myActuators
self.myMessages = myMessages
self.Count = 0

def run(self):
clock = 0
while not self.finished.isSet():
start = time.clock()
for aName in self.myActuators.All:

self.aActuator = self.myActuators.All[aName]

self.aActuator.Dyn.ReadAll()

self.myMessages.pubValue(aName + "_CurrentPosition", self.aActuator.getCurrentPosition())
self.myMessages.pubValue(aName + "_CurrentSpeed", self.aActuator.getCurrentSpeed())
self.myMessages.pubValue(aName + "_isMoving", self.aActuator.isMoving())
self.myMessages.pubValue(aName + "_CurrentLoad", self.aActuator.getCurrentLoad())
self.myMessages.pubValue(aName + "_CurrentTemperature", self.aActuator.getCurrentTemperature())

TorqueEnabled = self.myMessages.getValue(aName + '_TorqueEnabled')
CurrentTorqueEnabled = self.aActuator.getTorqueEnabled()
if TorqueEnabled != None and TorqueEnabled != CurrentTorqueEnabled:
self.aActuator.setTorqueEnabled(TorqueEnabled)

if (TorqueEnabled != False):
GoalPosition = self.myMessages.getValue(aName + '_GoalPosition')
CurrentGoalPosition = self.aActuator.getGoalPosition()
if GoalPosition != None and GoalPosition != CurrentGoalPosition:
self.aActuator.setGoalPosition(GoalPosition)

MovingSpeed = self.myMessages.getValue(aName + '_MovingSpeed')
CurrentMovingSpeed = self.aActuator.getMovingSpeed()
if MovingSpeed != None and MovingSpeed != CurrentMovingSpeed:
self.aActuator.setMovingSpeed(MovingSpeed)

if self.myMessages.getValue(aName + "_StopRequested"):
self.aActuator.stop()
self.myMessages.pubValue(aName + "_StopRequested", False)

self.Count += 1

self.myDynaNet.Synchronize()

delta = time.clock() - start
clock += delta
print "Current Time:", delta, "Ave Time:", clock / self.Count

def stop(self):
print "Stopping servos ...",
self.finished.set()
self.join()
print "Done."
And I read the cached values like this:


def getCurrentPosition(self):
return self.Dyn.Item[Register.CurrentPosition]

---
Pi Robot
http://www.pirobot.org (http://www.pirobot.org/)

iBot
07-10-2010, 04:49 PM
I did some testing too, and would agree that you have now have the best performance.
Basically a read transaction, or any transaction with a reply takes of the order of 2 ms. This is mainly driven by the latency in the full speed USB driver in the PC and the latency value (1ms) in the FTDI chip of the USB2Dynamixel for the reply. So best as you do, to do it all in one go with a readall().
The writes are best using sync writes, both because they talk to many devices, and they have no reply. Sync writes take of the order of 1 ms.
I have tried my high speed multiport FTDI FT4232H under XP with dissapointing results. I changed the latency in the registry to 0 to reduce the latency value from 1ms to 125us. The FTDI tries to send every 125us which overwealms the OS, and the overall performance is actually slower than 1ms setting despite a faster bus transaction. Multiport is an advantage, but would require a rewrite of the library to support the overlapped transactions.
Based on what you have found, the F-M library might be simplified greatly if the proven inefficient parts were dropped.
To go any faster coupling of Dynamixels to PC requires an external processor to concentrate the individual servo data to a single message as proposed above, but I hope the performance now meets you needs.
And your architecture is still intact. Great.

Pi Robot
07-10-2010, 07:55 PM
Very cool. And many thanks again for your suggestions. I can't believe I'm getting 3x the polling rate after implementing your suggestions. Head tracking now seems almost instantaneous.

Pi Robot
07-10-2010, 08:54 PM
Well, ROS hasn't really avoided the reinventing the wheel aspect -- for instance, I know of at least 4 separate labs that have released ROS wrappers for ARToolkit. They all effectively do the same thing -- but with quite different interfaces.

It's also probably not for everyone. It's really targeted at Ubuntu systems. C++/Python are the main languages (although there is beta-level support for Java and a few other languages). The biggest thing is: the learning curve is a bit steep -- especially given what *most* hobbyists are really doing. There is a lot of very good documentation -- but it's going to take most people quite a bit of time to really get up and running well.
-Fergs

I believe Fergs has already seen this article because he made a comment at the bottom of it. :veryhappy: In the meantime, I think this write-up gives you a good taste of what it's like working with ROS, open source and multiple independent developers.

From I Heart Robotics - Testing ROS USB Cameras

http://www.iheartrobotics.com/2010/05/testing-ros-usb-camera-drivers.html

In the meantime, I wonder if Fergs would be willing to list his set of pros and cons so far when working with ROS?

Pi Robot
07-11-2010, 08:53 PM
Just to throw another iron in the fire: has anyone looked at Urbi from Gostai? It is now open source, runs on Linux, Windows and MacOS X, and has support for a number of popular robot platforms including Robotis. The home page is at:

http://www.gostai.com/

lnxfergy
07-12-2010, 11:22 AM
In the meantime, I wonder if Fergs would be willing to list his set of pros and cons so far when working with ROS?

Tiny bit of background: I've been running ROS for about 2-3 months now, on our University bots (iCreate + laptop, and Nelson (http://www.cs.albany.edu/%7Eferguson/nelson2.jpg)), and on my own bots (a ArbotiX+FitPC rover, and ArbotiX+FitPC based Issy3). Most people by now probably have noticed that I love Python, so ROS is quite easy for me to attach my existing code onto. It also has very good support for C++, and beta-level support for Java/Octave and several other languages.

Which brings me to an interesting point that I like -- ROS is ROS-agnostic. The current paradigm is to take your existing code and put a thin ROS wrapper around it. Each piece becomes a node in the runtime graph. We've got a lot of code in our lab (control stuff, emotional systems for Nelson, little visual routines), and it's been a breeze to move the stuff over into an ROS system -- far easier than it was to hook them onto our old framework (Tekkotsu) originally.

That said -- ROS really is targeted at experienced programmers. It's a massive system, and although it's well documented, most of the documentation assumes a certain level of experience, I'd say the equivalent of an Undergraduate Minor in CS, or several years of programming experiance is a pre-requisite to really develop inside ROS (a less experienced person may be able to setup a basic system and run existing code, but any real development will take some knowledge). It's a very powerful framework, especially because of how many new modules keep getting released (as open source) into the community.

-Fergs

Pi Robot
07-12-2010, 01:42 PM
Thanks Fergs. This is very helpful information. Regarding Python, I too am a fan and a fairly new convert. I spent the last 3 years programming Pi Robot in C# but I wanted to move to something cross-platform and compatible with ROS. I was actually amazed how easy it was to pick up and PySerial actually seems to do better with my Serializer and USB2Dynamixel than C#'s serial interface.

I've played with ROS on my Ubuntu machine enough to control a Rovio and to get the Georgia Tech Robotis library going. But so far I haven't experienced a "Eureka" moment where I thought, "wow, this is really the way to go." By contrast, the use of list comprehensions alone made it worth the effort to convert my C# code to Python--it was immediately apparent I would use them everywhere.

I really like the ROS publish/subscribe model. And I appreciate the ability to plug into development efforts being made by others such as navigation, coordinate transformations and robotic arm control. But since these projects are open source, you could also just use the algorithms without having to use the entire ROS framework. So I guess I am wondering, what specific aspects of ROS do you feel have helped your own programming efforts in a way that you couldn't get by running your own Python modules within a single application for a given robot?

Sorry to keep pelting you with questions, but I feel you're at a unique intersection point between the University/ROS world and hobby robotics so it's great to hear your thoughts.

--patrick

lnxfergy
07-12-2010, 04:23 PM
... But so far I haven't experienced a "Eureka" moment where I thought, "wow, this is really the way to go."
...
But since these projects are open source, you could also just use the algorithms without having to use the entire ROS framework. So I guess I am wondering, what specific aspects of ROS do you feel have helped your own programming efforts in a way that you couldn't get by running your own Python modules within a single application for a given robot?

I'm not sure there's any particular aspect that couldn't be done without ROS -- except that community part. Once your projects reach a certain amount of complexity, you really can't go it alone anymore. Being able to share code quickly and seamlessly with others becomes a must. If you're comfortable with hacking into individual open source projects to connect them with your own code, then ROS really isn't going to be too hard to deal with. Yes, you'll spend time learning ROS -- but it'll save you time later when you don't have to learn every little nook and cranny of every package you want to use (if you were to integrate them into your own program). There's also the language aspect -- ROS already supports Python/C++, it's just a matter of time before Java support is fully implemented, and I wouldn't be shocked to eventually see a Mono client library as well. Not having to fight with the languages that other people want to write their code in is another strength of ROS.

I've been using several 3rd party packages, but the thing I love most about ROS (which is really a by-product of it being a distributed architecture) is that I can restart individual nodes without reloading the whole robot -- this quickly speeds up development time. The introspection abilities provided by rostopic and other tools makes debugging inter-module communication fairly straight forward -- at it's core, that's all ROS really is, a standardized distributed computing model, that's highly tuned for performance AND debugging. And it's stable -- while some packages (such as PointCloudPerception2) are under active development, most of the core, and many other useful packages, are very stable -- their API and access methods are not changing anytime soon (the same cannot be said for many other frameworks).

I think another reason that I'm putting my efforts towards ROS is that it appears to have more staying power than most. Willow Garage has a lot of funding, they've spent a lot, they don't look to be slowing down. Having seen a variety of projects/frameworks come along only to disappear a short time later, I think ROS is going to last. I don't think I'm alone in this train of though -- prior to the PR2 Beta program, they really hadn't done a ton of advertising or promoting, yet numerous research labs had already adopted ROS.

-Fergs

Pi Robot
07-12-2010, 07:50 PM
Yeah, that all makes good sense and I think I have come to the same conclusions which is why I've gone as far as I have to try ROS out. Of the 3rd party packages you have used, which would you recommend as the easiest to start off with? I mentioned earlier I have already tried a Rovio package and the GT Robotis package. Have you successfully connected to a USB webcam for example?

Thanks!

lnxfergy
07-12-2010, 08:04 PM
Yeah, that all makes good sense and I think I have come to the same conclusions which is why I've gone as far as I have to try ROS out. Of the 3rd party packages you have used, which would you recommend as the easiest to start off with? I mentioned earlier I have already tried a Rovio package and the GT Robotis package. Have you successfully connected to a USB webcam for example?

I've been using:


Brown University's Create package - works pretty well, does better odometry than I thought the Create was capable of.
GT's Robotis package - it's more of an API package -- still needs some programming to do anything.
Brown's ARToolkit package - briefly played with it to check how well ARToolkit fiducials could be detected (from what range).
Brown's teleop_twist_keyboard package - nice easy way to drive around any robot that accepts "cmd_vel" messages (The ArbotiX ROS driver handles cmd_vel for both walkers and small wheeled bots).

I haven't used any of the webcam interfaces yet because we have our own vision pipeline that's been developed over the past couple semesters. We're also running this on 900Mhz netbooks -- so I'm a bit concerned about the overhead of running image messages from node to node within ROS.

-Fergs

Pi Robot
07-12-2010, 10:05 PM
Thanks again Fergs. Now I will stop bugging you for awhile so you can get back to your ROS package for the ArbotiX. :happy:

LinuxGuy
07-14-2010, 08:49 AM
There's still some awful documentation out there: http://www.pybotix.com/ until the domain name expires.
Is it possible to get the source code for PyBotix? I would at least like to look at the code. Is it 100% Python code?

8-Dale

lnxfergy
07-14-2010, 08:52 AM
I think another reason that I'm putting my efforts towards ROS is that it appears to have more staying power than most.

And apparently, the DARPA concurs with me -- in the DARPA solicitation for "Maximum Mobility and Manipulation (M3)", the following appears under the guidelines for software developed:

"´╗┐To encourage interoperability, all applicable software developed under the M3 program must be compatible with ROS: the Robot Open Source framework"

-Fergs

lnxfergy
07-15-2010, 12:20 AM
Is it possible to get the source code for PyBotix? I would at least like to look at the code. Is it 100% Python code?

8-Dale

It's available on my personal SVN server (http://svn.blunderingbotics.com/) -- but I wouldn't use it for anything but a glance over, it's not that well documented since development was quite rushed, and it's definitely not maintained -- nor is it likely to be revived. It's actually the second rewrite, if you back up the revision you can see where most files were replaced in about March/April 2009 with this ground-up mid-semester rewrite.

The core code is entirely Python -- but it leverages several non-python packages as services (for instance, on Linux it used Festival and OpenCV. I believe Connor at one point created service wrappers for RoboRealm and MS SAPI under Windows).

-Fergs

Connor
07-15-2010, 12:59 AM
It's available on my personal SVN server (http://svn.blunderingbotics.com/) -- but I wouldn't use it for anything but a glance over, it's not that well documented since development was quite rushed, and it's definitely not maintained -- nor is it likely to be revived. It's actually the second rewrite, if you back up the revision you can see where most files were replaced in about March/April 2009 with this ground-up mid-semester rewrite.

The core code is entirely Python -- but it leverages several non-python packages as services (for instance, on Linux it used Festival and OpenCV. I believe Connor at one point created service wrappers for RoboRealm and MS SAPI under Windows).

-Fergs

I'm sad to see pybotix go away... I was hoping to use it with Argos, and since it was python, it was for the most part, cross platform, as Argos was running windows. I did write a service wrapper for MS SAPI, I don't remember where I got with the RoboRealm wrapper. Since then, Mech Warfare took over, and Argos's main board died, so, I've not got to play around with it much. After I finish up my current projects, I hope to bring Argos back to live, and look at pybotix or ROS and see what I can do. The biggest thing I was looking for from PyBotix, was Fergy's implementation of navigation and localization. I'm not sure how far he got with that.

Thanks, Connor

Pi Robot
07-27-2010, 07:43 AM
Just to throw another iron in the fire: has anyone looked at Urbi from Gostai? It is now open source, runs on Linux, Windows and MacOS X, and has support for a number of popular robot platforms including Robotis. The home page is at:

http://www.gostai.com/

Looks like Urbi and ROS now play together:

http://www.willowgarage.com/blog/2010/07/22/urbi-open-source-now-integrated-ros

--patrick

jes1510
07-28-2010, 03:22 PM
Personally I'm a fan of a client/server over sockets approach when it comes to dealing with peripherals. That allows outside access to the readings or allows you to pump in data from some other source. I've had a lot of success with this approach in the past.

For example, one script reads sensor data and pumps it to another script that decides what to do with it.

MikeG
07-28-2010, 10:11 PM
I absolutely agree Jes1510

lnxfergy
07-28-2010, 10:28 PM
Have you successfully connected to a USB webcam for example?

I had not last time, but over the past week or so, I've been working with usb_cam (from the Bosch drivers). Haven't had any issues across half a dozen cameras. I've also been using compressed_image_transport, and have been really amazed at the framerate that I'm getting viewing the image remotely from a 900Mhz Celeron on a fairly cruddy network.

I've also been using ar_pose from CCNY as a quick and dirty localization technique for our robots in the lab. It's getting the images from usb_cam.

-Fergs

Pi Robot
07-29-2010, 10:53 AM
I had not last time, but over the past week or so, I've been working with usb_cam (from the Bosch drivers). Haven't had any issues across half a dozen cameras. I've also been using compressed_image_transport, and have been really amazed at the framerate that I'm getting viewing the image remotely from a 900Mhz Celeron on a fairly cruddy network.

I've also been using ar_pose from CCNY as a quick and dirty localization technique for our robots in the lab. It's getting the images from usb_cam.

-Fergs

Very cool! Can you report what kind of frame rates you are getting? Also, are you using OpenCV yet on this setup and, if so, what kind of frame rates do you get when using, for example, an edge filter on a 320x240 image? And how much CPU does this consume?

When you have time, I also have a couple more ROS questions. First, have you tried setting up rospy on a Windows machine? (I understand you still need to run roscore on a Linux box). And second, in your ROS implementation for the arbotiX, are you running a separate process for each servo to publish its position? So an 18-servo robot maps to 18 processes (or more?)?

Thanks!
patrick

lnxfergy
07-29-2010, 11:29 AM
Very cool! Can you report what kind of frame rates you are getting? Also, are you using OpenCV yet on this setup and, if so, what kind of frame rates do you get when using, for example, an edge filter on a 320x240 image? And how much CPU does this consume?

Most of the time I'm seeing 30fps output from the usb_cam driver -- on either of my 900mhz celeron eee books, or my 1.6GHz Atom-based dell netbook. Haven't tried a basic edge detector to see how that affects it (I'll have one of the guys in the lab try that out today or tomorrow).


First, have you tried setting up rospy on a Windows machine? (I understand you still need to run roscore on a Linux box).

No, I haven't -- I avoid windows as much as possible .... :veryhappy:


And second, in your ROS implementation for the arbotiX, are you running a separate process for each servo to publish its position? So an 18-servo robot maps to 18 processes (or more?)?

It would never be multiple processes -- but it can be multiple threads. That's of course handled internally by rospy, but it's configurable to broadcast a single joint position on a topic (like the GT version), but the default is to publish a single "joint_states" topic, which has all servos, by name (more PR2-like, and personally, I think a better design paradigm, it's rare you want to know one servo state).

The ArbotiX ROS stuff should be out very shortly -- we're finally in final testing, and I should have version 0.1 out this weekend (the odometry support for closed loop motor control is still not quite working, but servo control and cmd_vel motor control is there, it's mostly documentation that needs finalizing before release). At the same time, we'll have the first Alpha release of PyPose/NUKE 2.0 (with mammal quad support, biped support won't debut until a slightly later alpha build. Will also include a number of other updates, such as NUKE-ROS sketch export, and live servo update in the pose tab).

One of things you'll notice when I get the docs up, is that the ArbotiX node is almost too configurable. There's about half a dozen things that can be turned on or off at startup (closed loop motor control [and whether or not we can translate laterally, such as on a NUKE bot], broadcast single joints, start services to move single joints, broadcast point clouds using the poor man's lidar setup, etc).

-Fergs

Pi Robot
07-29-2010, 11:51 AM
Many thanks Fergs, that all makes sense. Looking forward even more to your ROS beta release.

--patrick

Pi Robot
07-29-2010, 06:11 PM
Personally I'm a fan of a client/server over sockets approach when it comes to dealing with peripherals. That allows outside access to the readings or allows you to pump in data from some other source. I've had a lot of success with this approach in the past.

For example, one script reads sensor data and pumps it to another script that decides what to do with it.

I played around with sockets in Python for the first time today and I see what you mean--very nice! I have a few questions since this is new stuff to me. I set up one script (code shown below) to play the role of a message server that receives data from client nodes. Client nodes "publish" their data to the message server. I am borrowing the code samples from the Python docs found at:

http://docs.python.org/library/socketserver.html

My questions are:


What do you think of this particular server code for a robot message server?
Does the server code shown below automatically terminate each thread after receiving a message from a client?
Is the opening and closing of the connection between client and server for each value published such a good idea? Or is there a better way to keep the connection open, especially if the client is publishing many times per second?
How are you coding your client/server? (If you can share.)


Here is the server code:


import SocketServer, threading, time

class ThreadedTCPRequestHandler(SocketServer.BaseRequest Handler):
def handle(self):
data = self.request.recv(1024)
cur_thread = threading.currentThread()
response = "%s: %s" % (cur_thread.getName(), data)
print response
#self.request.send(response)

class ThreadedTCPServer(SocketServer.ThreadingMixIn, SocketServer.TCPServer):
pass

if __name__ == "__main__":
# Port 0 means to select an arbitrary unused port
HOST, PORT = "127.0.0.1", 50007

server = ThreadedTCPServer((HOST, PORT), ThreadedTCPRequestHandler)
ip, port = server.server_address

# Start a thread with the server -- that thread will then start one
# more thread for each request
server_thread = threading.Thread(target=server.serve_forever)
# Exit the server thread when the main thread terminates
server_thread.setDaemon(True)
server_thread.start()
print "Server loop running in thread:", server_thread.getName()

# Stay up forever
while 1:
time.sleep(0.01)And here is how a client node publishes to the server:


def publish(self):
self.value = self.getValue()
HOST = '127.0.0.1' # The remote host
PORT = 50007 # The same port as used by the server
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.connect((HOST, PORT))
self.sock.send(self.value)
self.sock.close()

jes1510
07-29-2010, 08:12 PM
class Server (Thread) :
def run ( self):
global channel
global details
global sock
sock = socket.socket ( socket.AF_INET, socket.SOCK_STREAM )
# sock.settimeout(5)

sock.bind ( ( '', port ) )
# sock.setblocking(0)
incomingIP = ''
incomingPort = ''

while 1 :

sock.listen ( 100 )
try :

channel, details = sock.accept()
# self.sendData(channel, "connected")
incomingIP, incomingPort = details
channelFile = channel.makefile()

print
print "We have opened a connection with" + str(details)
incomingCommand = channelFile.readline ()
incomingCommand = incomingCommand[:-1]
incomingCommand = incomingCommand.lower()
print ("Got '"+ incomingCommand + "' from " + str(incomingIP) + " on inComing Port " + str(incomingPort))
print

command = incomingCommand.split(" ")
hardware = command[0]


if hardware == "sensors" :
sensorChannel = channel
self.Sensors(sensorChannel)



The above code will open a socket and listen for something coming in. The first thing you would send would be an identifier of what you want to talk to. In the above case it would be "sensors". That will spawn a new thread with a connection to the sensors class. The sensors thread will stay alive until you close the socket on the client side. The thread will die after the socket is closed. Once the thread is open it can stay alive and provide 2 way communications.



class Sensors(Thread) :
def __init__(self,connection):
Thread.__init__(self)
self.connection=connection

def run(self) :
sensorsConnection = self.connection
sensorsFile = sensorsConnection.makefile()

while 1 :

sockData = sensorsFile.readline()
if not sockData :
print "Killing Sensors Thread"
sensorsConnection.close()
break

sockData = sockData[:-1]

#try :
line = sockData.split(" ")

Pi Robot
07-30-2010, 01:04 PM
Thanks jes1510 for the sample code--this really helps me understand the client/server socket stuff better. My goal is slightly different than your setup as I am trying to emulate the ROS message server (in a very rudimentary form), but in a way that will allow me to migrate to ROS at a later date with minimal effort. So my server will accept topic/message pairs from client nodes. I think I have figured out how to keep the connection open from the client nodes using a separate server thread for each client. The code I have working so far is as follows. Note that I am not doing anything with the messages at this point--just printing them out from the server:

For the server:


import socket, threading

class listen_thread(threading.Thread):
def __init__(self, conn):
threading.Thread.__init__(self)
self.daemon = True
self.conn = conn

def run(self):
while 1:
data = self.conn.recv(1024)
if not data: break
print data
self.conn.close()

HOST = '' # Symbolic name meaning all available interfaces
PORT = 50007 # Arbitrary non-privileged port
threads = dict({})
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(50)
print "Server now listening on port", PORT, "..."
while 1:
conn, addr = s.accept()
print 'Connected by', addr
threads[addr] = listen_thread(conn)
threads[addr].start()And for client Nodes:


import threading, time, socket

SOCK_HOST = '127.0.0.1'
SOCK_PORT = 50007

class Node(object):
""" Top Level Node Class """

def __init__(self, name="", uri=""):
self.name = name
self.uri = uri

class publisher(threading.Thread):
def __init__(self, topic, type, name, rate):
threading.Thread.__init__(self)
self.finished = threading.Event()
self.daemon = True
self.topic = topic
self.type = type
self.name = name
self.rate = rate
self.interval = 1.0 / rate

self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.sock.connect((SOCK_HOST, SOCK_PORT))

def getValue(self):
self.value = None
return self.value

def publish(self):
self.value = self.getValue()
self.sock.send(self.topic + ":" + self.value)

def run(self):
while not self.finished.isSet():
self.publish()
time.sleep(self.interval)

def stop(self):
print "Stopping node", self.name, "...",
self.sock.close()
self.finished.set()
try:
self.join()
except:
pass
finally:
print "Done."And finally, here is an example where two client nodes publish their messages to the server at different rates:


import time
import pi.nodes.node_sockets as Node

class pub1(Node.publisher):
def getValue(self):
return "Hello"

class pub2(Node.publisher):
def getValue(self):
return "Good-bye"

p1 = pub1("topic1", int, "Topic 1", 5)
p2 = pub2("topic2", int, "Topic 2", 15)

p1.start()
p2.start()

time.sleep(5)

p1.stop()
p2.stop(

lnxfergy
07-30-2010, 02:47 PM
My goal is slightly different than your setup as I am trying to emulate the ROS message server (in a very rudimentary form), but in a way that will allow me to migrate to ROS at a later date with minimal effort.

Honestly, I wouldn't bother mimicking their network setup -- just keep a very good level of abstraction between your "logic" code that does stuff and the connection code. One of the important facets of ROS is the idea of ROS-agnostic code -- write it how you would have otherwise, then put a thin ROS wrapper around it. Because of this ability, lots of code has been ported to ROS (hence the explosion in available packages, if you can wrap around it, you can use it in ROS).

-Fergs

Pi Robot
07-30-2010, 03:09 PM
Honestly, I wouldn't bother mimicking their network setup -- just keep a very good level of abstraction between your "logic" code that does stuff and the connection code. One of the important facets of ROS is the idea of ROS-agnostic code -- write it how you would have otherwise, then put a thin ROS wrapper around it. Because of this ability, lots of code has been ported to ROS (hence the explosion in available packages, if you can wrap around it, you can use it in ROS).

-Fergs

Yeah, that sounds like good advice. When you have a moment, it would be great to see a simple example of what you mean by putting a thin ROS wrapper around an existing piece of code. This would also help me better understand the distinction between logic and connection code so I can proceed in the right direction.

lnxfergy
07-30-2010, 03:18 PM
Yeah, that sounds like good advice. When you have a moment, it would be great to see a simple example of what you mean by putting a thin ROS wrapper around an existing piece of code. This would also help me better understand the distinction between logic and connection code so I can proceed in the right direction.

Ok, I would not recommend using this code yet (as it's still changing a bit here and there, especially the odometry/cmd_vel, and how joints are set), but the ArbotiX ROS wrappers are in SVN: http://code.google.com/p/vanadium-ros-pkg/source/browse/

You can see there is an original src/arbotix/arbotix.py which consists of the actual serial driver and a number of routines (this started as the PyPose driver.py, and will eventually be back-migrated into PyPose before it goes V2.0). Then in bin/arbotix-ros.py we have the ROS aspects. This wraps the arbotix code, adding all the topic/message stuff. It's not the cleanest split between logic/networking, but it does work.

Pay no attention to the src/arbotix/gp_lidar.py stuff because it's an absolute mess of ROS/non-ROS code and is still under active development.

-Fergs

Pi Robot
07-30-2010, 05:14 PM
Thanks Fergs--this is great. I'll browse through some of your code and see if it makes sense to me.

--patrick

lnxfergy
08-04-2010, 03:29 PM
Just as a heads up: ROS's new cturtle release has a wiki page about running rospy on Winders computers: http://www.ros.org/wiki/cturtle/Installation/Windows

-Fergs

Pi Robot
08-05-2010, 06:29 PM
Just as a heads up: ROS's new cturtle release has a wiki page about running rospy on Winders computers: http://www.ros.org/wiki/cturtle/Installation/Windows

-Fergs

Thanks Fergs. If I'm not mistaken, this is the same page that was up for the Box Turtle distribution. In any case, I haven't tried it yet as I am making progress moving everything over to Linux instead...

--patrick

Pi Robot
08-07-2010, 08:42 AM
Ok, I would not recommend using this code yet (as it's still changing a bit here and there, especially the odometry/cmd_vel, and how joints are set), but the ArbotiX ROS wrappers are in SVN: http://code.google.com/p/vanadium-ros-pkg/source/browse/

etc.

-Fergs

Hey Fergs,

I've made some partial headway understanding your ROS arbotiX code which looks great so far. I have one conceptual question about the way ROS does things that I'm hoping you can clarify.

If all one wanted to do was publish the joint states, I'm guessing one could just write a publisher node that simply reads the appropriate register values and publishes them at the desired frequency.

However, in the arbotiX case (as well as with my current USB2Dynamixel setup), we also need to create ROS services to set the goal positions and velocities of the servos, and I see where you have defined the callbacks to do this.

My question is this: in my experience (and probably yours too), sending packets to the Dynamixel bus is not "thread safe". In other words, if I have two or more threads trying to access the bus at the same time, I can get timeouts, lock-ups or weird movement behavior. So how does your code prevent the publisher part (running continuously at 10Hz in your source at the moment) from getting tripped up by incoming service requests to set servo positions and speeds? Or does ROS somehow manage this behind the scenes?

Hope that makes sense!

lnxfergy
08-07-2010, 11:38 AM
Thread safety is all the way down at the bottom. Every read/write eventually is handled by the "execute" function within the ArbotiX class. There's a mutex protection inside execute -- this makes sure we don't send crap data. It does however have one side effect: if we set a high joint_state publishing rate (say 30-40hz) we've noticed it sometimes tends to starve the setting services of access to the bus.

I'm considering that existing code in SVN to be v0.1, it's fully working, but fairly undocumented and has some lag in the "set/relax" services. v0.2 (which I'm currently working on), fixes this problem. In v0.2, when you call a service to set a joint, the request is put on the queue of joints to update -- which is then handled in the main loop (thus there's no weird mutex sleep waiting issues, because access to the queue is very fast, as opposed to slow serial transactions). This aspect is currently about 90% done -- just undergoing testing.

The second item I'm adding is that we'll have a new service structure -- by default, you won't have dozens of "set, get, relax" services running, instead there will be the joint_state publishing and a single "setJoints" service that takes a JointState message type allowing you to set multiple joints at once -- say, an entire arm, or the pan&tilt of the head. You can also relax joints by sending 0 for effort in the JointStates message. There is also still a parameter "single_services" that can be set true to turn back on the dozens of "set,get,relax" services.

This is of course all very low-level stuff -- I'm eventually planning to introduce a way to spin up Joint Trajectory Controllers within the ArbotiX-ROS stuff. This would be more in-line with the way that high-end ROS robots, like PR2, do arm and head control, which should allow users to use the high-level move_arm infrastructure (if they have IK services available for their particular configuration).

-Fergs

Pi Robot
08-07-2010, 08:10 PM
Ah, that's brilliant! (At least to me--it's probably obvious to you.) I've been using the Forest Moon Dynamixel library that I have had converted into Python and it doesn't seem to have a single point of entry into the serial stream so it's not as easy to wrap it in a lock.

In the meantime, I will spend more time studying your SVN 0.1 code which is now making even more sense.

Thanks!

Pi Robot
08-10-2010, 06:41 PM
I put together a little write-up of some of the stuff we've been talking about on this thread and I thought some of you might get a kick (or a groan) out of it. I posted it at:

http://www.pirobot.org/blog/0013/

--patrick

lnxfergy
08-10-2010, 08:35 PM
I put together a little write-up of some of the stuff we've been talking about on this thread and I thought some of you might get a kick (or a groan) out of it. I posted it at:

http://www.pirobot.org/blog/0013/

--patrick

My thoughts:

Thread Safety -- there's a whole other side to this. Even if you're thread safe (two threads can't mess with the values at the same time), there's a possibility of things going wrong (one thread/node sets the value, then the other sets it, and again, so that you get oscillation between two controllers, neither reaching their goal). You really don't want more than one node controlling the base velocity or arm joint positions. ROS introduces "actions" to cover this sort of event -- a node requests a particular goal be reached (through an action interface), and than can poll the state of the action.

Remote Messaging Over A Network -- while it is nice to be able to add additional processors to the mix, I find that the best part of the distributed approach of ROS is that when debugging code I can leave 90% of the robot infrastructure running, and just restart the node(s) that I'm working on currently. This saves a lot of time in bigger systems. The other big advantage is that we can write code in differently languages (C++ for heavy lifting, python for quick prototyping, etc).

Other stuff -- I've been working with a number of other packages lately. With the release of cturtle, we now have binaries of the executive_smash (http://www.ros.org/wiki/executive_smach) stack -- it's pretty cool for writing state machine executives inside ROS.

One other note on code specifics: in Python, it is generally faster to write code that "asks for forgiveness rather than asking for permission", so:



if topic in MessageBoard:
return MessageBoard[topic]
else: return None
would typically be written as:


try:
return MessageBoard[topic]
except:
return None
If "topic" doesn't exist, an exception will be thrown. Exception handling is very fast and streamlined in Python. In your other version, we have to search through all the keys once to see if topic is MessageBoard, and then search through again to find the correct index to return (since code isn't compiled, it doesn't hang onto the index).

-Fergs

Pi Robot
08-10-2010, 08:57 PM
Great comments! And thanks for the tip on using a try-except block instead of keyword lookup. (I updated the web page with your version.) Yeah, I was looking at the ROS action stuff a little while ago. Like you say, the thread-locking is just part of the equation. I also have action priorities at a higher level of abstraction and a mechanism for "attention" that helps to prevent shared resources being accessed simultaneously by more than one node. But I will look at ROS actions in more depth when I get a chance.

lnxfergy
08-23-2010, 01:08 PM
The other ROS thread made me find this one again, and I thought I would post a quick ArbotiX package update. V0.2 should be out in a few days (like Wednesday or Thursday) and is the first release I consider stable enough for people to start using.

This release introduces the notion of a lightweight core plus numerous controllers. The core part of the program is the same old arbotix wrapper which provides thread-safe access to the serial port and publishes servo positions over the joint_state topic. On top of this core, we dynamically launch a series of controllers and/or sensors (based on the configuration at startup time). Controllers that will be in the 0.2:


base_controller - controller for a differential drive mobile base, wraps the low-level motor and encoder functions of the ArbotiX into a module that subscribes to "cmd_vel" to get it's velocity commands, and which publishes odometry information and transforms. This allows a small, cheap ArbotiX powered base to run with the navigation stack! (assuming you have adequate sensory)
nuke_controller - same as base_controller, but provides holonomic control of a NUKE-powered walker.
joint_controller - controls servos. This is really where all the testing/work has gone recently. We eventually tracked down that the huge delay/lag was the use of service-based set functions. Thus, joint_controller uses a topic-based set function, it subscribes to "cmd_joints" (of message type JointState) and updates the servos.

There will also be experimental support for the Poor Man's Lidar (http://www.showusyoursensors.com/2010/08/ros-and-other-ramblings.html) (although there is more work to be done for future releases, it is primarily in sensor smoothing and publication refinement to better support costmap generation, and thus the interface is mostly stable). Looking towards later releases, I'm working on a series of more advanced controllers that take joint trajectories rather than just joint settings (and work over the more standard node_ns/command topic). The current roadmap is on the wiki: http://code.google.com/p/vanadium-ros-pkg/wiki/RoadMap

-Fergs

Pi Robot
08-23-2010, 06:21 PM
This is all very exciting and I look forward to your 0.2 release.

--patrick