Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Integrating a new type of EtherCAT device with ROS.

This tutorial shows the steps needed to integrate an new type of EtherCAT device into and ethercat_hardware and pr2_etherCAT. This tutorial integrates an imaginary EtherCAT device with one 16bit analog input and one 16bit analog output. The imaginary EtherCAT device is assumed to have a product ID of 1234567.

Getting ROS

The code for this tutorial was writen for the ROS boxturtle release. The PR2 version of the release has the required packages for this tutorial. There are a couples methods for obtaining ROS. The following method was used when this tutorial was created:

SVN Checkout

These instructions were taken from ROS/Installation/Ubuntu/SVN.

wget --no-check-certificate http://ros.org/rosinstall -O ~/rosinstall
chmod 755 ~/rosinstall
~/rosinstall ~/ros http://ros.org/rosinstalls/boxturtle_pr2.rosinstall

Run pr2_etherCAT

To verify the new EtherCAT device is connected properly, run pr2_etherCAT before writing any code. Go through this tutorial for instruction on building and running pr2_etherCAT.

On startup, pr2_etherCAT will read the product ID, serial number, and revision of all EtherCAT devices it detects. If pr2_etherCAT can not read these values or does not see any devices, there may be something wrong with the new device or cabling connecting it to the computer.

When pr2_etherCAT finds an EtherCAT device it does not recognize, it will print an error message and quit. Below is the error message caused by using the new EtherCAT device.

[FATAL] [1272302813.320857903]: Unable to configure slave #0, product code: 1234567 (0x12D687), serial: 1004 (0x3EC), revision: 33685781 (0x2020115)
[FATAL] [1272302813.321324003]: Perhaps you should power-cycle the MCBs
[FATAL] [1272302814.321531811]: BREAKPOINT HIT
        file = /u/dking/wg_latest/stacks/pr2_ethercat_drivers/ethercat_hardware/src/ethercat_hardware.cpp
        line=190

The error message shows the unidentified device has a product code of 1234567

Create ROS package

All the code to support the new device will be put in a new package. Some steps must be taken for ROS to find a new package.

Create directory to hold new packages

First, create a directory to store new ROS packages in. This directory could be put anywhere, but for this tutorial create the directory in your home folder.

cd $HOME
mkdir my_pkgs

Add directory to ROS package search path

Modify ROS setup script to look for packages inside of new directory. my_pkgs. The setup script should be located at $HOME/ros/setup.sh.

Prepend the new directory path ($HOME/my_pkgs) to the the ROS_PACKAGE_PATH variable in the ROS setup script.

This is what the line in setup.sh should look like after the changes:

export ROS_PACKAGE_PATH=$HOME/my_pkgs:$HOME/ros/stacks

Any previously open terminals will not have the new value for ROS_PACKAGE_PATH. Re-source the setup script to get the new value.

source ~/ros/setup.sh

Create a new package

Create a new ROS package called my_device_pkg for the new device. Create the package inside the my_pkgs directory. The new package will need to be given have a couple of dependencies:

• roscpp
• ethercat_hardware

Run:

cd $HOME/my_pkgs
roscreate-pkg my_device_pkg roscpp ethercat_hardware

Verify new package

Verify ROS can find the new package by running:

rospack find my_device_pkg

The command should provide path to the new package. The path will probably be something like:

$HOME/my_pkgs/my_device_pkg

Create header file for new device

A header file for the new device should be created inside the include directory of the new package.

roscd my_device_pkg/include/my_device_pkg/

Create a header file called my_device.h with the following code.

   1 #ifndef MY_DEVICE_H
   2 #define MY_DEVICE_H
   3 
   4 #include <ethercat_hardware/ethercat_device.h>
   5 
   6 class MyDevice : public EthercatDevice
   7 {
   8 public:
   9   void construct(EtherCAT_SlaveHandler *sh, int &start_address);
  10   int initialize(pr2_hardware_interface::HardwareInterface *, bool);
  11   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
  12   void packCommand(unsigned char *buffer, bool halt, bool reset);
  13   bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);
  14  
  15   MyDevice();
  16   ~MyDevice();
  17 
  18 protected:
  19   int counter_;
  20 };
  21 
  22 #endif /* MY_DEVICE_H */
  23 

Explanation of header file

All device types should be derived from EthercatDevice class.

   6 class MyDevice : public EthercatDevice

EthercatDevice defines a few virtual functions that MyDevice must implement.

   9   void construct(EtherCAT_SlaveHandler *sh, int &start_address);
  10   int initialize(pr2_hardware_interface::HardwareInterface *, bool);
  11   void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *);
  12   void packCommand(unsigned char *buffer, bool halt, bool reset);
  13   bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer);

Create source for new device

Create a source file in the source directory of the ethercat_hardware package.

roscd my_device_pkg/src

Copy the following code into a file called my_device.cpp

   1 #include <my_device_pkg/my_device.h>
   2 #include <ros/console.h>
   3 #include <sstream>
   4 #include <iomanip>
   5 using namespace std;
   6 
   7 PLUGINLIB_REGISTER_CLASS(1234567, MyDevice, EthercatDevice);
   8 
   9 MyDevice::MyDevice()
  10 {
  11   counter_ = 0;  
  12 }
  13 
  14 void MyDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address)
  15 {
  16   EthercatDevice::construct(sh, start_address);
  17   sh->set_fmmu_config( new EtherCAT_FMMU_Config(0) );
  18   sh->set_pd_config( new EtherCAT_PD_Config(0) );
  19 }
  20 
  21 MyDevice::~MyDevice()
  22 {
  23   delete sh_->get_fmmu_config();
  24   delete sh_->get_pd_config();
  25 }
  26 
  27 int MyDevice::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
  28 {
  29   ROS_WARN("Device #%02d: MyDevice (%d)", 
  30             sh_->get_ring_position(), sh_->get_product_code());
  31   return 0;
  32 }
  33 
  34 void MyDevice::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
  35 {
  36   stringstream name;
  37   name << "EtherCAT Device #" << setw(2) << setfill('0') 
  38        << sh_->get_ring_position() << " (MyDevice)";
  39   d.name = name.str();
  40   d.summary(d.OK, "OK");
  41   stringstream hwid;
  42   hwid << sh_->get_product_code() << "-" << sh_->get_serial();
  43   d.hardware_id = hwid.str();
  44 
  45   d.clear();
  46   d.addf("Position", "%02d", sh_->get_ring_position());
  47   d.addf("Product Code", "%d", sh_->get_product_code());
  48   d.addf("Serial Number", "%d", sh_->get_serial());
  49   d.addf("Revision", "%d", sh_->get_revision());
  50   d.addf("Counter", "%d", ++counter_);
  51 
  52   EthercatDevice::ethercatDiagnostics(d, 2);
  53 }
  54 
  55 void MyDevice::packCommand(unsigned char *buffer, bool halt, bool reset)
  56 {
  57 }
  58 
  59 bool MyDevice::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
  60 {
  61   return true;
  62 }

Explanation of source file

Classes for different type of devices are used as plugins. The plugins class will register themselves and the devices they support when the class is loaded. PLUGINLIB_REGISTER_CLASS is a macro that registers the MyDevice class as child class plugin for EthercatDevice. For each device, a setup function will try to match the device to a plugin using its product code. The code snippet registers MyDevice as a plugin for any device with the product code 1234567.

   7 PLUGINLIB_REGISTER_CLASS(1234567, MyDevice, EthercatDevice);

There is more information on pluginlib available here.

construct() is supposed to configure the EtherCAT SyncManagers and Fieldbus Memory Mannegment Units (FMMU) for a given device. This configuration is needed for proper communication with a device. Setting up communication with an EtherCAT device is covered in the next tutorial. For now, no SyncManagers and no FMMUs are configured.

  14 void MyDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address)
  15 {
  16   EthercatDevice::construct(sh, start_address);
  17   sh->set_fmmu_config( new EtherCAT_FMMU_Config(0) );
  18   sh->set_pd_config( new EtherCAT_PD_Config(0) );
  19 }

initialize() is called once communication with a device available. The purpose of this function is to setup ROS communication channels, create hardware interface for controllers and possibly collect more information about the device. More code will be implemented in next part of tutorial. For now, the function just print out some information about the device.

  27 int MyDevice::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
  28 {
  29   ROS_WARN("Device #%02d: MyDevice (%d)", 
  30             sh_->get_ring_position(), sh_->get_product_code());
  31   return 0;
  32 }

Every EtherCAT device must provide diagnostics information. A diagnostics message has a few required fields, but most of the information is provided in the form of key-value pairs. The DiagnosticStatusWrapper provides an easy way to create and append data to a diagnostics message.

  34 void MyDevice::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)

The name is one piece of required information for a diagnostics message. The name that is provided for a particular device should be unique and should not change while the program is running. In the code snippet, the device position is used to make sure the device name is unique even when there are multiple devices of the same type.

  37   name << "EtherCAT Device #" << setw(2) << setfill('0') 
  38        << sh_->get_ring_position() << " (MyDevice)";
  39   d.name = name.str();

The device summary contains two pieces of information. The level provides an easy way to identify devices that have problems. The possible values for level are : ERROR, WARN, and OK. The summary is a short description of the any problem that is occurring on a device. When there are no issues with a device, the summary should be OK.

  40   d.summary(d.OK, "OK");

The hardware ID should be unique identifier for a given piece of hardware. A combination of product code and serial number works well for this purpose.

  42   hwid << sh_->get_product_code() << "-" << sh_->get_serial();
  43   d.hardware_id = hwid.str();

In next chunk of code a few key-value pairs are added to the diagnostics message. Both key and values are formatted as strings. Notice the counter variable is increment every time the diagnostics function is called.

  45   d.clear();
  46   d.addf("Position", "%02d", sh_->get_ring_position());
  47   d.addf("Product Code", "%d", sh_->get_product_code());
  48   d.addf("Serial Number", "%d", sh_->get_serial());
  49   d.addf("Revision", "%d", sh_->get_revision());
  50   d.addf("Counter", "%d", ++counter_);

The EthercatDevice base class collects information from the EtherCAT device that is available on most EtherCAT ASICs. The call to ethercatDiagnostics allows this information to be added to the diagnostics message. The number 2 is the number of Ethernet/EBUS ports the device has.

  52   EthercatDevice::ethercatDiagnostics(d, 2);

The last two functions handle communication to and from the device. They will be covered in the next tutorial. Leave them empty for now.

Build new package

Open my_device_pkg/CMakeLists.txt and add the following line to the end of the file:

rosbuild_add_library(my_device_plugin src/my_device.cpp)

Now to build the new package:

rosmake my_device_pkg

Running above command will build a shared library called libmy_device_plugin.so. This shared library should be located inside the lib directory or my_device_pkg.

Register plugin

my_device_plugin.so needs to register as a ethercat_device plugin. Create ethercat_device_plugin.xml in the top level directory of my_device_pkg and add the following:

<library path="lib/libmy_device_plugin" >
  <class name="1234567" type="MyDevice" base_class_type="EthercatDevice">
    <description>
      MyDevice - Fake analog I/O device.
    </description>
  </class>
</library>

Also open manifest.xml and added the following between the <package> and </package> tags.

  <export>
    <ethercat_hardware plugin="${prefix}/ethercat_device_plugin.xml" />
  </export>

Now verify that ROS knows that my_device_pkg contains a ethercat_hardware plugin.

rospack plugins --attrib=plugin ethercat_hardware

The command should find two packages that contain ethercat_device plugins.

my_device_pkg $HOME/my_pkgs/my_device_pkg/ethercat_device_plugin.xml
ethercat_hardware $HOME/ros/stacks/pr2_ethercat_drivers/ethercat_hardware/ethercat_device_plugin.xml

For more information on plugins, see pluginlib

Run pr2_etherCAT

Run pr2_etherCAT. It should no longer quit right away. Instead, you should see the output from the ROS_WARN statement in the initialize() function:

[ WARN] [1272314533.520699463]: Device #00: MyDevice (1234567)

Look at diagnostics with runtime_monitor. There should be diagnostics for the new device. Look at the Counter field, its value should be incrementing once a second. Also notice the extra diagnostic information that is provided by EthercatDevice::ethercatDiagnostics()