This website uses browsing/session and functional cookies to ensure you get the best experience. Learn More

Making a new device in YARP

From Wiki for iCub and Friends
Jump to: navigation, search

Contents


Other YARP device tutorials:

Specifically for interfacing with motors, here are some useful examples:

  • In YARP source code:
    • See example/dev/fake_motor.cpp
    • See src/modules/fakebot, a small simulated robot (has simulated motor control plus vision)
    • See implementation of test_motor device (src/libYARP_dev/include/yarp/dev/TestMotor.h)

The Scenario

Let's assume we have an existing library or piece of code for communicating with a robot arm we just bought, and we now want to control that arm from YARP. We could always do this by making some ports and deciding on some messages ourselves, but let's suppose we want to make our arm have the same interface as existing motor controlboards in YARP ("esd", "jrkerr", "dimax", the simulator, etc)

Since we don't have any actual hardware for this tutorial, we'll make a "fake" robot arm that has just three degrees of freedom, is controlled by position, and moves instantaneously to setpoints. To see how dependencies on external libraries are handled, we'll make the fake arm code depend on "libxml2", a library for reading XML files. This dependency is meant to stand in for a dependency on a vendor supplied library for your hardware.

Let's assume our arm has the following interface:

 // Start the motors, configured from an XML file
 extern void xmlarm_start(const char *configFile);
 // Shut down the motor interface
 extern void xmlarm_stop();
 // Drive the motors towards the given setpoints
 extern void xmlarm_set(double pos0, double pos1, double pos2);
 // Check the position of the given motor
 extern double xmlarm_get(int index);

We'll assume that this interface is in a header file "xml_arm.h" (complete contents given later on this page) with a dummy implementation in "xml_arm.cpp" (again, contents given later on this page, the details are not important).

First step: get a test program compiling with CMake

YARP's system for bundling devices into libraries depends on CMake, for reasons of portability and flexibility. So the first step to making a new device in YARP is simply to get an existing test program compiling with CMake.

Here's a simple test program for xml_arm (we'll put it in "test_arm.cpp"):

 int main() {
   xmlarm_start("startup.xml");
   xmlarm_set(10,20,30);
   printf("Moved to %g %g %g\n", 
           xmlarm_get(0), xmlarm_get(1), xmlarm_get(2));
   xmlarm_stop();
 }

The test program needs an xml configuration file (put it in "startup.xml"):

 <?xml version="1.0"?>
 <myarm>
   <pos0>5</pos0>
   <pos1>5</pos1>
   <pos2>5</pos2>
 </myarm>

Let's suppose this test program came with a Makefile, or a project file, or whatever. We'll need to translate this to CMake. For example, here's a Makefile we might have:

%.o: %.cpp
   g++ -c -I/usr/include/libxml2 $< -o $@

xml_arm: xml_arm.o test_arm.o
   g++ $^ -o xml_arm -lxml2

clean:
   rm -f xml_arm.o test_arm.o xml_arm

(if you copy/paste this, you'll need to fix some spaces to be tabs - but you don't need to use this makefile) Here is the CMakeLists.txt file this would translate to:

PROJECT(xml_arm)
INCLUDE_DIRECTORIES(/usr/include/libxml2)
LINK_LIBRARIES(xml2)
ADD_EXECUTABLE(test_arm test_arm.cpp xml_arm.cpp xml_arm.h)

After running cmake and compiling, we get a program "test_arm", which when run gives:

Starting position is 5 5 5
Moved to 10 20 30

Second step: break test program into a library and an executable

The next step to making a YARP device is to introduce in our CMake project a library that contains all the code we want to wrap (and not any test code). This will do the trick:

PROJECT(xml_arm)
INCLUDE_DIRECTORIES(/usr/include/libxml2)
LINK_LIBRARIES(xml2)
ADD_LIBRARY(xml_arm xml_arm.cpp xml_arm.h)
LINK_LIBRARIES(xml_arm)
ADD_EXECUTABLE(test_arm test_arm.cpp)

Now after running cmake and compiling, we get a library called "xml_arm" (e.g. libxml_arm.a on linux) and a program called "test_arm" as before that links that library. It runs, giving the same result as before.

Third step: implement the yarp::dev::DeviceDriver interface (open, close)

So far, we've done nothing involving YARP, we've just been getting set up in CMake. Now, we need to decide what YARP device "interfaces" we will implement.

We need to add a class that implements the "yarp::dev::DeviceDriver" interface. This is very simple, it means we should have an open and a close method. The open method is a good point to set up configuration options.

Let's start a file "YarpXmlArm.h" holding the YARP wrapper for our arm:

#include <stdio.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include "xml_arm.h"
class YarpXmlArm : public yarp::dev::DeviceDriver
{
public:
  bool open(yarp::os::Searchable& config) {
    printf("Opening YarpXmlArm\n");
    yarp::os::ConstString filename = 
      config.check("xml",
		   yarp::os::Value("startup.xml"),
		   "xml file holding initial positions").asString();
    xmlarm_start(filename.c_str());
    return true;
  }

  bool close() {
    printf("Closing YarpXmlArm\n");
    xmlarm_stop();
    return true;
  }
};

We map DeviceDriver::open and DeviceDriver::close to xmlarm_start and xmlarm_stop. The "config.check" line looks for an option called xml (default value: startup.xml) that is used to find the initial xml config file. (The "Searchable" interface used here seems a little strange; it is designed so that options for devices can be loaded in YARP from the command line, from files, across the network, from GUIs, etc.)

Now let's make a simple test program called "test_yarp_arm.cpp":

#include <stdio.h>
#include "YarpXmlArm.h"
using namespace yarp::os;
using namespace yarp::dev;
int main(int argc, char *argv[]) {
  YarpXmlArm arm;
  Property p;
  p.fromCommand(argc,argv);
  // Check YARP API to our device.
  // All devices should be usable as a yarp::dev::DeviceDriver.
  // We use a pointer just to really show the DeviceDriver methods
  // are implemented.
  DeviceDriver *dev = &arm;
  dev->open(p);
  // Can't do anything with the device yet
  dev->close();
}

And update our CMakeLists.txt file to compile it:

PROJECT(xml_arm)
FIND_PACKAGE(YARP)
INCLUDE_DIRECTORIES(/usr/include/libxml2)
LINK_LIBRARIES(xml2)
ADD_LIBRARY(xml_arm xml_arm.cpp xml_arm.h YarpXmlArm.h)
LINK_LIBRARIES(xml_arm)
ADD_EXECUTABLE(test_arm test_arm.cpp)
ADD_EXECUTABLE(test_yarp_arm test_yarp_arm.cpp)

When we run cmake and compile we get a test program test_yarp_arm. We run it and see:

Opening YarpXmlArm
Starting position is 5 5 5
Closing YarpXmlArm

We should be able to run "./test_yarp_arm --xml another.xml" to use a different startup file. We'll test this later.

Fourth step: implement other yarp::dev::I<MumbleMumble> interfaces

The next step is to expose the capabilities of your device using interfaces that exist in YARP for similar devices.

  • For camera sources, a common interface is yarp::dev::IFrameGrabberImage and sometimes yarp::dev::IFrameGrabberControls
  • For motors, common interfaces include yarp::dev::IPositionControl, yarp::dev::IVelocityControl, yarp::dev::IEncoders, ...
  • See lists of interfaces in the "device interfaces" section of the YARP module reference page.

For our arm, we will implement yarp::dev::IPositionControl (position control of motors) and yarp::dev::IEncoders (reading encoder values). Here we go:

#include <stdio.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include "xml_arm.h"

class YarpXmlArm : public yarp::dev::DeviceDriver,
                   public yarp::dev::IPositionControl,
                   public yarp::dev::IEncoders
{
public:
   bool open(yarp::os::Searchable& config) {
   printf("Opening YarpXmlArm\n");
   yarp::os::ConstString filename = 
     config.check("xml",
                  yarp::os::Value("startup.xml"),
                  "xml file holding initial positions").asString();
   xmlarm_start(filename.c_str());
   return true;
 }

 bool close() {
   printf("Closing YarpXmlArm\n");
   xmlarm_stop();
   return true;
 }

 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////
 //// IPositionControl interface
 ////

 virtual bool getAxes(int *ax) {
   *ax = 3;
   printf("YarpMyArm reporting %d axes present\n", *ax);
   return true;
 }
 virtual bool setPositionMode() {
   return true;
 }
 virtual bool positionMove(int j, double ref) {
   return false;
 }
 virtual bool positionMove(const double *refs) {
   xmlarm_set(refs[0],refs[1],refs[2]);
   return true;
 }
 virtual bool relativeMove(int j, double delta) {
   return false;
 }
 virtual bool relativeMove(const double *deltas) {
   return false;
 }
 virtual bool checkMotionDone(int j, bool *flag) {
   return false;
 }
 virtual bool checkMotionDone(bool *flag) {
   return false;
 }
 virtual bool setRefSpeed(int j, double sp) {
   return false;
 }
 virtual bool setRefSpeeds(const double *spds) {
   return false;
 }
 virtual bool setRefAcceleration(int j, double acc) {
   return false;
 }
 virtual bool setRefAccelerations(const double *accs) {
   return false;
 }
 virtual bool getRefSpeed(int j, double *ref) {
   return false;
 }
 virtual bool getRefSpeeds(double *spds) {
   return false;
 }
 virtual bool getRefAcceleration(int j, double *acc) {
   return false;
 }
 virtual bool getRefAccelerations(double *accs) {
   return false;
 }
 virtual bool stop(int j) {
   return false;
 }
 virtual bool stop() {
   return false;
 }

 ////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////
 ////
 //// IEncoders interface
 ////

 virtual bool resetEncoder(int j) {
   return true;
 }
 virtual bool resetEncoders() {
   return false;
 }
 virtual bool setEncoder(int j, double val) {
   return false;
 }
 virtual bool setEncoders(const double *vals) {
   return false;
 }
 virtual bool getEncoder(int j, double *v) {
   return false;
 }
 virtual bool getEncoders(double *encs) {
   encs[0] = xmlarm_get(0);
   encs[1] = xmlarm_get(1);
   encs[2] = xmlarm_get(2);
   return true;
 }
 virtual bool getEncoderSpeed(int j, double *sp) {
   return false;
 }
 virtual bool getEncoderSpeeds(double *spds) {
   return false;
 }
 virtual bool getEncoderAcceleration(int j, double *spds) {
   return false;
 }
 virtual bool getEncoderAccelerations(double *accs) {
   return false;
 }
};

There's a lot of methods in these interfaces! To start, we make most of them return false, which means "not implemented yet", and then add just a few - in this case getAxes, positionMove (the variant that moves all axes at the same time), and getEncoders. For a real implementation, you should implement as much as you can. It is fine to leave methods returning false that don't make sense for your hardware.

Now we update our test program (test_yarp_arm.cpp) to try some of our methods:

#include <stdio.h>
#include "YarpXmlArm.h"
using namespace yarp::os;
using namespace yarp::dev;

int main(int argc, char *argv[]) {
 YarpXmlArm arm;
 Property p;
 p.fromCommand(argc,argv);

 DeviceDriver *dev = &arm;
 IPositionControl *pos = &arm;
 IEncoders *enc = &arm;

 dev->open(p);
 int n = 0;
 pos->getAxes(&n);
 printf("arm has %d axes\n", n);
 double x[3];
 enc->getEncoders(x);
 printf("arm encoder readings are %g %g %g\n", x[0], x[1], x[2]);
 dev->close();
}

And when we run this we see:

Opening YarpXmlArm
Starting position is 5 5 5
YarpMyArm reporting 3 axes present
arm has 3 axes
arm encoder readings are 5 5 5
Closing YarpXmlArm

Fifth step: add our device to a bundle of devices

Almost there! Now say we want to add our device into the collection of devices included with YARP (we could also keep it separate, see documentation, but we won't do this in this tutorial).

  • Make a new directory in "src/modules" of the YARP source code. For example, make "src/modules/xml_arm". Copy everything we have made so far there.
  • Change our CMakeLists.txt file in two ways. We turn off compilation of test code if a special variable called COMPILE_DEVICE_LIBRARY" is turned on. And we call a method called "PREPARE_DEVICE" to declare the name of our device, its header file, its class in C++, and any standard network wrapper that can be placed around it. Our CMakeLists.txt file becomes:
PROJECT(xml_arm)
FIND_PACKAGE(YARP)
INCLUDE_DIRECTORIES(/usr/include/libxml2)
LINK_LIBRARIES(xml2)
IF (COMPILE_DEVICE_LIBRARY)
  PREPARE_DEVICE(xml_arm TYPE YarpXmlArm INCLUDE YarpXmlArm.h 
                 WRAPPER controlboard)
ENDIF (COMPILE_DEVICE_LIBRARY)

ADD_LIBRARY(xml_arm xml_arm.cpp xml_arm.h YarpXmlArm.h)

IF (NOT COMPILE_DEVICE_LIBRARY)
  LINK_LIBRARIES(xml_arm)
  ADD_EXECUTABLE(test_arm test_arm.cpp)
  ADD_EXECUTABLE(test_yarp_arm test_yarp_arm.cpp)
ENDIF (NOT COMPILE_DEVICE_LIBRARY)

If we configure, compile and run, everything should remain unchanged so far.

Now edit the CMakeLists.txt file in "src/modules" and add the following line:

 ADD_SUBDIRECTORY(xml_arm)

Add it in the place where there is a lot of similar lines. Now run the cmake GUI (ccmake on linux) in the directory "src/modules". You should see an option "ENABLE_yarpmod_xml_arm". Turn this on, and compile. You should see xml_arm.cpp being compiled, and a library called "yarpmod" created, along with a program called "yarpmoddev". Run:

 yarpmoddev --list

You should see in the list:

 Device "xml_arm", C++ class YarpXmlArm, wrapped by "controlboard"

Now run:

 yarpmoddev --device xml_arm

and you should see something like:

 $ ./yarpmoddev --device xml_arm
 Subdevice xml_arm
 Opening YarpXmlArm
 yarpdev: created device <xml_arm>.  See C++ class YarpXmlArm for documentation.
 yarp: Port /controlboard/rpc:i active at tcp://192.168.242.1:10002
 yarp: Port /controlboard/command:i active at tcp://192.168.242.1:10012
 yarp: Port /controlboard/state:o active at tcp://192.168.242.1:10022
 YarpMyArm reporting 3 axes present
 YarpMyArm reporting 3 axes present
 yarpdev: created wrapper <controlboard>.  See C++ class ServerControlBoard for documentation.
 Server control board starting
 yarpdev: device active in background...

You can now read encoder values from /controlboard/state:o with "yarp read ... /controlboard/state:o", and send commands (just "get axes" and "set poss (5 10 15)" will work) with yarp rpc or yarp write ("yarp rpc /controlboard/rpc:i")

If you now reconfigure YARP itself with CMake, and choose to compile devices, you'll find an option for the xml_arm device. Turn it on, and yarpdev will list your device. You will also be able to create your device with the PolyDriver class.

See http://eris.liralab.it/yarpdoc/add_a_device.html for other ways to compile devices if you want to use them without recompiling YARP.

What if there are not any good interfaces for my device in YARP?

  • Make some up! Someone has to be the first.
  • You won't get free remote access to your device if we haven't written a network wrapper for it ("controlboard", "grabber", etc). So just make your own ports and send messages. If this works well, it can be converted to a network wrapper.

xml_arm.h

// This is an example of an arm motor interface, as part of a tutorial 
// on how to wrap motor interfaces for use with YARP.

// This motor interface is very simple (and fake, since we don't
// have any actual external hardware).

// There are just three motors, which can be controlled just with
// position control, and movement to setpoints is instantaneous.

// Just to make things a little more realistic, we introduce an
// external library dependency; in this case on "libxml2", a library
// for reading xml files.  Normally, a motor interface would have
// a dependency on the library or device API used to actually speak
// to the motors.  This dependency should be handled just as we will
// see libxml2 handled.

//   --paulfitz

// Initialize the motor interface, going to the initial positions
// specified in the named configuration file.  An example of the
// contents of such a file:
//   <?xml version="1.0"?>
//     <myarm>
//     <pos0>5</pos0>
//     <pos1>10</pos1>
//     <pos2>15</pos2>
//   </myarm>
extern void xmlarm_start(const char *configFile);

// Shut down the motor interface
extern void xmlarm_stop();

// Drive the motors towards the given setpoints
extern void xmlarm_set(double pos0, double pos1, double pos2);

// Check the position of the given motor
extern double xmlarm_get(int index);

xml_arm.cpp

#include "xml_arm.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <libxml/xmlreader.h>

#include <string>
using namespace std;

static double xmlarm_pos[3] = {0, 0, 0};

xmlNodePtr getChildNode(xmlNodePtr p, const char *name, 
			xmlNodePtr last = NULL){
  if(p == NULL || name == NULL) return NULL;
  for(p=(last==NULL)?p->children:last->next; p!=NULL; p=p->next){
    if(p->name && (strcmp((char*)p->name,name) == 0)){
      return p;
    }
  }
  return NULL;
}

string getValue(xmlNodePtr p) {
  return (char*)xmlNodeGetContent(p);
}

void xmlarm_start(const char *configFile) {
  xmlDoc *doc = NULL;
  xmlNode *root_element = NULL;
  
  LIBXML_TEST_VERSION;

  doc = xmlReadFile(configFile, NULL, 0);
  
  if (doc == NULL) {
    printf("error: could not parse file %s\n", configFile);
    exit(1);
  }

     
  /*Get the root element node */
  root_element = xmlDocGetRootElement(doc);
  
  xmlNodePtr part = NULL;
  

  double pos0 = 0;
  double pos1 = 0;
  double pos2 = 0;

  part = getChildNode(root_element,"pos0",part);
  if (part!=NULL) {
    pos0 = atof(getValue(part).c_str());
  }
  part = getChildNode(root_element,"pos1",part);
  if (part!=NULL) {
    pos1 = atof(getValue(part).c_str());
  }
  part = getChildNode(root_element,"pos2",part);
  if (part!=NULL) {
    pos2 = atof(getValue(part).c_str());
  }
 
  xmlarm_pos[0] = pos0;
  xmlarm_pos[1] = pos1;
  xmlarm_pos[2] = pos2;
  printf("Starting position is %g %g %g\n", pos0, pos1, pos2);

  xmlFreeDoc(doc);

  xmlCleanupParser();
}

void xmlarm_stop() {
}

void xmlarm_set(double pos0, double pos1, double pos2) {
  xmlarm_pos[0] = pos0;
  xmlarm_pos[1] = pos1;
  xmlarm_pos[2] = pos2;  
}

double xmlarm_get(int index) {
  return xmlarm_pos[index];
}
Personal tools
Namespaces

Variants
Actions
Navigation
Print/export
Toolbox