IndexNextUpPreviousUrbi SDK 2.7.5

Chapter 6
Use Cases

 6.1 Writing a Servomotor Device
  6.1.1 Caching
  6.1.2 Using Timers
 6.2 Using Hubs to Group Objects
  6.2.1 Alternate Implementation
 6.3 Writing a Camera Device
  6.3.1 Optimization in Plugin Mode
 6.4 Writing a Speaker or Microphone Device
 6.5 Writing a Softdevice: Ball Detection

6.1 Writing a Servomotor Device

Let’s write a UObject for a servomotor device whose underlying API is:

First our header. Our servo device provides an attribute named val, the standard Urbi name, and two ways to set PID gain: a method, and three variables.

 
class servo : public urbi::UObject // must inherit UObject 

public
  // the class must have a single constructor taking a string 
  servo(const std::string&); 
 
  // Urbi  constructor 
  void init(int id); 
 
  // main attribute 
  urbi::UVar val; 
 
  // position variables: 
  //  P gain 
  urbi::UVar P; 
  //  I gain 
  urbi::UVar I; 
  //  D gain 
  urbi::UVar D; 
 
  // callback for val change 
  void valueChanged(UVar& v); 
  //callback for val access 
  void valueAccessed(UVar& v); 
  // callback for PID change 
  void pidChanged(UVar& v); 
 
  // method to change all values 
  void setPID(int p, int i, int d); 
 
  // motor ID 
  int id_; 
};  

The constructor only registers init, so that our default instance servo does nothing, and can only be used to create new instances.

 
servo::servo (const std::string& s) 
  : urbi::UObject (s) 

   // register init 
   UBindFunction (servo, init); 
}  

The init function, called in a new instance each time a new Urbi instance is created, registers the four variables (val, P, I and D), and sets up callback functions.

 
// Urbi constructor. 
void 
servo::init (int id) 

  id_ = id; 
 
  if (!initialize (id)) 
    return 1; 
 
  UBindVar (servo, val); 
 
  // val is both a sensor and an actuator. 
  Uowned (val); 
 
  // Set blend mode to mix. 
  val.blend = urbi::UMIX; 
 
  // Register variables. 
  UBindVar (servo, P); 
  UBindVar (servo, I); 
  UBindVar (servo, D); 
 
  // Register functions. 
  UBindFunction (servo, setPID); 
 
  // Register callbacks on functions. 
  UNotifyChange (val, &servo::valueChanged); 
  UNotifyAccess (val, &servo::valueAccessed); 
  UNotifyChange (P, &servo::pidChanged); 
  UNotifyChange (I, &servo::pidChanged); 
  UNotifyChange (D, &servo::pidChanged); 
}  

Then we define our callback methods. servo::valueChanged will be called each time the val variable is modified, just after the value is changed: we use this method to send our servo commands. servo::valueAccessed is called just before the value is going to be read. In this function we request the current value from the servo, and set val accordingly.

 
// Called each time val is written to. 
void 
servo::valueChanged (urbi::UVar& v) 

  // v is a reference to our class member val: you can use both 
  // indifferently. 
  setPosition (id, (double)val); 

 
// Called each time val is read. 
void 
servo::valueAccessed (urbi::UVar& v) 

  // v is a reference to val. 
  val = getPosition (id); 
}  

servo::pidChanged is called each time one of the PID variables is written to. The function servo::setPID can be called directly from Urbi.

 
void 
servo::pidChanged (urbi::UVar& v) 

  setPID(id, (int)P, (int)I, (int)D); 

 
void 
servo::setPID (int p, int i, int d) 

  setPID (id, p, i, d); 
  P = p; 
  I = i; 
  D = d; 

 
// Register servo class to the Urbi kernel. 
UStart (servo);  

That’s it, compile this module, and you can use it within urbiscript:

 
// Create a new instance.  Calls init (1). 
headPan = new servo (1); 
 
// Calls setPID (). 
headPan.setPID (8,2,1); 
 
// Calls valueChanged (). 
headPan.val = 13; 
 
// Calls valueAccessed (). 
headPan.val * 12; 
 
// Periodically calls valueChanged (). 
headPan.val = 0 sin:1s ampli:20, 
 
// Periodically calls valueAccessed (). 
at (headPan.val < 0) 
  echo ("left");  

The sample code above has one problem: valueAccessed and valueChanged are called each time the value is read or written from Urbi, which can happen quite often. This is a problem if sending the actual command (setPosition in our example) takes time to execute. There are two solutions to this issue.

6.1.1 Caching

One solution is to remember the last time the value was read/written, and not apply the new command before a fixed time. Note that the kernel is doing this automatically for UOwned’d variables that are in a blend mode different than normal. So the easiest solution to the above problem is likely to set the variable to the mix blending mode. The unavoidable drawback is that commands are not applied immediately, but only after a small delay.

6.1.2 Using Timers

Instead of updating/fetching the value on demand, you can chose to do it periodically based on a timer. A small difference between the two API methods comes in handy for this case: the update() virtual method called periodically after being set up by USetUpdate(interval) is called just after one pass of Urbi code execution, whereas the timers set up by USetTimer are called just before one pass of Urbi code execution. So the ideal solution is to read your sensors in the second callback, and write to your actuators in the first. Our previous example (omitting PID handling for clarity) can be rewritten. The header becomes:

 
// Inherit from UObject. 
class servo : public urbi::UObject 

public
  // The class must have a single constructor taking a string. 
  servo (const std::string&) 
 
  // Urbi constructor. 
  void init (int id); 
 
  // Called periodically. 
  virtual int update (); 
  // Called periodically. 
  void getVal (); 
 
  // Our position variable. 
  urbi::UVar val; 
 
  // Motor ID. 
  int id_; 
};  

Constructor is unchanged, init becomes:

 
// Urbi constructor. 
void 
servo::init (int id) 

  id_ = id; 
 
  if (!initialize (id)) 
    return 0; 
 
  UBindVar (servo,val); 
  // Val is both a sensor and an actuator. 
  UOwned(val); 
 
  // Will call update () periodically. 
  USetUpdate(1); 
  // Idem for getVal (). 
  USetTimer (1, &servo::getVal); 
}  

valueChanged becomes update and valueAccessed becomes getVal. Instead of being called on demand, they are now called periodically. The period of the call cannot be lower than the value returned by Object.getPeriod; so you can set it to 0 to mean “as fast as is useful”.

6.2 Using Hubs to Group Objects

Now, suppose that, for our previous example, we can speed things up by sending all the servomotor commands at the same time, using the following method that takes two arrays of ids and positions.

 
void setPositions(int count, int* ids, double* positions);  

A hub is the perfect way to handle this task. The UObject header stays the same. We add a hub declaration:

 
class servohub : public urbi::UObjectHub 

public
  // The class must have a single constructor taking a string. 
  servohub (const std::string&); 
 
  // Called periodically. 
  virtual int update (); 
 
  // Called by servo. 
  void addValue (int id, double val); 
 
  int* ids; 
  double* vals; 
  int size; 
  int count; 
};  

servo::update becomes a call to the addValue method of the hub:

 
int 
servo::update() 

  ((servohub*)getUObjectHub ("servohub"))->addValue (id, (double)val); 
};  

The following line can be added to the servo init method, although it has no use in our specific example:

 
URegister(servohub);  

Finally, the implementation of our hub methods is:

 
servohub::servohub (const std::string& s) 
  : UObjectHub (s) 
  , ids   (0) 
  , vals  (0) 
  , size  (0) 
  , count (0) 

  // setup our timer 
  USetUpdate (1); 

 
int 
servohub::update () 

  // Called periodically. 
  setPositions (count, ids, vals); 
 
  // Reset position counter. 
  count = 0; 
 
  return 0; 

 
void 
servohub::addValue (int id, double val) 

  if (count + 1 < size) 
  { 
    // Allocate more memory. 
    ids = (int*) realloc (ids, (count + 1) * sizeof (int)); 
    vals = (double*) realloc (vals, (count + 1) * sizeof (double)); 
    size = count + 1; 
  } 
  ids[count] = id; 
  vals[count++] = val; 

 
UStartHub (servohub);  

Periodically, the update method is called on each servo instance, which adds commands to the hub arrays, then the update method of the hub is called, actually sending the command and resetting the array.

6.2.1 Alternate Implementation

Alternatively, to demonstrate the use of the members hub variable, we can entirely remove the update method in the servo class (and the USetUpdate() call in init), and rewrite the hub update method the following way:

 
int servohub::update() 

  //called periodically 
  for (UObjectList::iterator i = members.begin (); 
       i != members.end (); 
       ++i) 
    addValue (((servo*)*i)->id, (double)((servo*)*i)->val); 
  setPositions(count, ids, vals); 
  // reset position counter 
  count = 0; 
 
  return 0; 
}  

6.3 Writing a Camera Device

A camera device is an UObject whose val field is a binary object. The Urbi kernel itself doesn’t make any difference between all the possible binary formats and data type, but the API provides image-specific structures for convenience. You must be careful about memory management. The UBinary structure handles its own memory: copies are deep, and the destructor frees the associated buffer. The UImage and USound structures do not.

Let’s suppose we have an underlying camera API with the following functions:

Our device code can be written as follows:

 
// Inherit from UObject. 
class Camera : public urbi::UObject 

public
  // The class must have a single constructor taking a string. 
  Camera(const std::string&); 
 
  // Urbi constructor. Throw in case of error. 
  void init (int id); 
 
  // Our image variable and dimensions. 
  urbi::UVar val; 
  urbi::UVar width; 
  urbi::UVar height; 
 
  // Called on access. 
  void getVal (UVar&); 
 
  // Called periodically. 
  virtual int update (); 
 
  // Frame counter for caching. 
  int frame; 
  // Frame number of last access. 
  int accessFrame; 
  // Camera id. 
  int id_; 
  // Storage for last captured image. 
  UBinary bin; 
};  

The constructor only registers init:

 
Camera::Camera (const std::string& s) 
  : urbi::UObject (s) 
  , frame (0) 

  UBindFunction (Camera, init); 
}  

The init function binds the variable, a function called on access, and sets a timer up on update. It also initializes the UBinary structure.

 
void 
Camera::init (int id) 

  //urbi constructor 
  id_ = id; 
  frame = 0; 
  accessFrame = 0; 
 
  if (!initialize (id)) 
    throw std::runtime_error("Failed to initialize camera"); 
 
  UBindVar (Camera, val); 
  UBindVar (Camera, width); 
  UBindVar (Camera, height); 
  width = getWidth (id); 
  height = getHeight (id); 
 
  UNotifyAccess (val, &Camera::getVal); 
 
  bin.type = BINARY_IMAGE; 
  bin.image.width = width; 
  bin.image.height = height; 
  bin.image.imageFormat = IMAGE_RGB; 
  bin.image.size = width * height * 3; 
 
  // Call update () periodically. 
  USetUpdate (1); 
}  

The update function simply updates the frame counter:

 
int 
Camera::update () 

  ++frame; 
  return 0; 
}  

The getVal updates the camera value, only if it hasn’t already been called this frame, which provides a simple caching mechanism to avoid performing the potentially long operation of acquiring an image too often.

 
void 
Camera::getVal(urbi::UVar&) 

  if (frame == accessFrame) 
    return
 
  bin.image.data = getImage (id); 
  // Assign image to bin. 
  val = bin; 

 
UStart(Camera);  

The image data is copied inside the kernel when proceeding this way.

Be careful, suppose that we had created the UBinary structure inside the getVal method, our buffer would have been freed at the end of the function. To avoid this, set it to 0 after assigning the UBinary to the UVar.

6.3.1 Optimization in Plugin Mode

In plugin mode, it is possible to access the buffer used by the kernel by casting the UVar to a UImage. You can modify the content of the kernel buffer but no other argument.

6.4 Writing a Speaker or Microphone Device

Sound handling works similarly to image manipulation, the USound structure is provided for this purpose. The recommended way to implement a microphone is to fill the UObject val variable with the sound data corresponding to one kernel period. If you do so, the Urbi code loop tag:micro.val, will produce the expected result.

6.5 Writing a Softdevice: Ball Detection

Algorithms that require intense computation can be written in C++ but still be usable within Urbi: they acquire their data using UVar referencing other modules’ variables, and output their results to other UVar. Let’s consider the case of a ball detector device that takes an image as input, and outputs the coordinates of a ball if one is found.

The header is defined like:

 
class BallTracker : public urbi::UObject 

public
  BallTracker (const std::string&); 
  void init (const std::string& varname); 
 
  // Is the ball visible? 
  urbi::UVar visible; 
 
  // Ball coordinates. 
  urbi::UVar x; 
  urbi::UVar y; 
 };  

The constructor only registers init:

 
// The constructor registers init only. 
BallTracker::BallTracker (const::string& s) 
  : urbi::UObject (s) 

  UBindFunction (BallTracker, init); 
}  

The init function binds the variables and a callback on update of the image variable passed as a argument.

 
void 
BallTracker::init (const std::string& cameraval) 

  UBindVar (BallTracker, visible); 
  UBindVar (BallTracker, x); 
  UBindVar (BallTracker, y); 
  UNotifyChange (cameraval, &BallTracker::newImage); 
 
  visible = 0; 
}  

The newImage function runs the detection algorithm on the image in its argument, and updates the variables.

 
void 
BallTracker::newImage (urbi::UVar& v) 

  // Cast to UImage. 
  urbi::UImage i = v; 
  int px,py; 
  bool found = detectBall (i.data, i.width, i.height, &px, &py); 
 
  if (found) 
  { 
    visible = 1; 
    x = px / i.width; 
    y = py / i.height; 
  } 
  else 
    visible = 0; 
}