Note: This tutorial assumes that you have completed the previous tutorials: Actuator Array Example1.
(!) 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.

Actuator Array Example2

Description: This example expands on Example1. In this tutorial we will modify the Example1 driver to read additional properties from the YAML configuration file. To do this we will make use of a custom JointProperties structure to hold the additional properties and the init_actuator_() function hook provided by the base class to fill them in.

Tutorial Level: BEGINNER

Next Tutorial: Actuator Array Example3

Creating the driver class

This tutorial assumes you have completed Example1. For the first step, we will copy the final version of example1_driver.cpp to example2_driver.cpp, as shown below.

   1 #include <actuator_array_driver/actuator_array_driver.h>
   2 #include <example_driver/dummy_actuator.h>
   3 
   4 using namespace actuator_array_driver;
   5 using namespace actuator_array_example;
   6 
   7 class Example2Driver : public ActuatorArrayDriver<JointProperties>
   8 {
   9 private:
  10   std::map<int, DummyActuator> actuators_;
  11   ros::Time previous_time_;
  12 public:
  13   Example2Driver()
  14   {
  15     ActuatorArrayDriver::init();
  16 
  17     for(unsigned int i = 0; i < command_msg_.name.size(); ++i)
  18     {
  19       actuators_[i] = DummyActuator();
  20     }
  21 
  22   };
  23   virtual ~Example2Driver() {};
  24 
  25   bool read_(ros::Time ts = ros::Time::now())
  26   {
  27     // Calculate the time from the last update
  28     double dt = (ts - previous_time_).toSec();
  29 
  30     // Loop through each joint and request the current status
  31     for (unsigned int i = 0; i < joint_state_msg_.name.size(); ++i)
  32     {
  33       // Update the simulated state of each actuator by dt seconds
  34       actuators_[i].update(dt);
  35 
  36       // Query the current state of each actuator
  37       joint_state_msg_.position[i] = actuators_[i].getPosition();
  38       joint_state_msg_.velocity[i] = actuators_[i].getVelocity();
  39       joint_state_msg_.effort[i]   = actuators_[i].getMaxTorque();
  40     }
  41 
  42     joint_state_msg_.header.stamp = ts;
  43     previous_time_ = ts;
  44 
  45     return true;
  46   }
  47 
  48   bool command_()
  49   {
  50     // Loop through each joint in the command message and send the
  51     // corresponding servo the desired behavior
  52     for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
  53     {
  54       actuators_[i].setVelocity(command_msg_.velocity[i]);
  55       actuators_[i].setPosition(command_msg_.position[i]);
  56     }
  57 
  58     return true;
  59   }
  60 
  61   bool stop_()
  62   {
  63     // Loop through each joint and send the stop command
  64     for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
  65     {
  66       // Update the simulated state of each actuator by dt seconds
  67       actuators_[i].stop();
  68     }
  69 
  70     return true;
  71   }
  72 
  73   bool home_()
  74   {
  75     // Loop through each joint and send the home command
  76     for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
  77     {
  78       // Update the simulated state of each actuator by dt seconds
  79       actuators_[i].home();
  80     }
  81 
  82     return true;
  83   }
  84 
  85 };
  86 
  87 int main(int argc, char** argv)
  88 {
  89   ros::init(argc, argv, "example2_driver");
  90   Example2Driver driver;
  91   driver.spin();
  92 
  93   return 0;
  94 }

Next, we add the new driver node to the CMakeLists.txt file so it will be compiled.

...
rosbuild_add_executable(example2_driver src/example2_driver.cpp src/dummy_actuator.cpp)
...

Adding custom YAML properties

The actuator_array_driver base class support two different YAML configuration formats. In Example1 we used a simple list of joint names. In this example we will include a channel and a home position along with the name of each joint. Create the YAML configuration file example2.yaml in the package cfg directory as shown below. Note that name is a required field when using this format.

joints:

  - name: joint1
    channel: 0      
    home: 0.0

  - name: joint2
    channel: 3      
    home: -1.57

  - name: joint3
    channel: 1      
    home: 1.57

  - name: joint4
    channel: 2      
    home: 0.785

Creating a custom `JointProperties` class

We will need a place to store these additional properties for each joint. There are many ways to accomplish this, but the base class provides one convenient method. The base class defines a JointProperties structure to hold various properties. The driver class is then templated on this structure type. However, we are free to template our driver on a different structure type, as long as it has the same fields as the JointProperties structure. This allows us to add additional fields and have the base class maintain them for us.

To start, we derive a custom structure from the JointProperties class. We will add fields for a channel and a home position.

   1 ...
   2 struct Example2JointProperties : public JointProperties
   3 {
   4   int channel;
   5   double home;
   6 };
   7 ...

and we will change the template argument of our driver to our new Example2JointProperties class.

   1 ...
   2 class Example2Driver : public ActuatorArrayDriver<Example2JointProperties>
   3 {
   4 ...

Reading the custom properties

So far, we have added custom properties to the YAML configuration file and added fields to the JointProperties class. Now we need to read those custom YAML fields and update the JointProperties with those values. The actuator_array_driver base class provides a function hook, init_actuator_(), that is called while parsing the YAML configuration. This function is provided with the current JointProperties structure and access to YAML fields for that joint.

We implement a version of this function that checks the YAML data for the new fields and updates the JointProperties object. This is also a good place to create and initialize the DummyActuator objects, as all custom joint information is available.

   1 ...
   2 bool init_actuator_(const std::string& joint_name, 
   3                     Example2JointProperties& joint_properties, 
   4                     XmlRpc::XmlRpcValue& joint_data)
   5 {
   6   // Read custom data from the XMLRPC struct
   7   if (joint_data.hasMember("channel"))
   8   {
   9     joint_properties.channel = (int) joint_data["channel"];
  10   }
  11   if (joint_data.hasMember("home"))
  12   {
  13     joint_properties.home = (double) joint_data["home"];
  14   }
  15 
  16   // Create a dummy actuator object and store in container
  17   actuators_[joint_properties.channel] = 
  18       DummyActuator(-1.57, 1.57, 10.0, joint_properties.home);
  19 
  20   return true;
  21 }
  22 ...

Now that the actuators_ container is being populated by the init_actuator_() function, that functionality should be removed from the constructor.

   1 ...
   2 public:
   3   Example2Driver()
   4   {
   5     ActuatorArrayDriver::init();
   6   };
   7 ...

Access using `channel` number

In this example, we have switch to accessing the actuators by a channel number instead of the array index of the messages. This was done to mimic how a real servo communication system might be used. The comamnd_(), read_(), stop_(), and home_() functions must be modified to first look up the channel number of a specific actuator using the joints_ container defined in the base class, then manipulate the actuator using the channel number.

`read_()`

   1 ...
   2 bool read_(ros::Time ts = ros::Time::now())
   3 {
   4   // Calculate the time from the last update
   5   double dt = (ts - previous_time_).toSec();
   6 
   7   // Loop through each joint and request the current status
   8   for (unsigned int i = 0; i < joint_state_msg_.name.size(); ++i)
   9   {
  10     // Look up the channel number from the JointProperties using the name
  11     int channel = joints_[command_msg_.name[i]].channel;
  12 
  13     // Update the simulated state of each actuator by dt seconds
  14     actuators_[channel].update(dt);
  15 
  16     // Query the current state of each actuator
  17     joint_state_msg_.position[i] = actuators_[channel].getPosition();
  18     joint_state_msg_.velocity[i] = actuators_[channel].getVelocity();
  19     joint_state_msg_.effort[i]   = actuators_[channel].getMaxTorque();
  20   }
  21 
  22   joint_state_msg_.header.stamp = ts;
  23   previous_time_ = ts;
  24 
  25   return true;
  26 }
  27 ...

`command_()`

   1 ...
   2 bool command_()
   3 {
   4   // Loop through each joint in the command message and send the
   5   // corresponding servo the desired behavior
   6   for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
   7   {
   8     // Look up the channel number from the JointProperties using the name
   9     int channel = joints_[command_msg_.name[i]].channel;
  10 
  11     // Send the actuator the desired position and velocity
  12     actuators_[channel].setVelocity(command_msg_.velocity[i]);
  13     actuators_[channel].setPosition(command_msg_.position[i]);
  14   }
  15 
  16   return true;
  17 }
  18 ...

`stop_()`

   1 ...
   2 bool stop_()
   3 {
   4   // Loop through each joint and send the stop command
   5   for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
   6   {
   7     // Look up the channel number from the JointProperties using the name
   8     int channel = joints_[command_msg_.name[i]].channel;
   9 
  10     // Tell the actuator to stop
  11     actuators_[channel].stop();
  12   }
  13 
  14   return true;
  15 }
  16 ...

`home_()`

   1 ...
   2 bool home_()
   3 {
   4   // Loop through each joint and send the stop command
   5   for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
   6   {
   7     // Look up the channel number from the JointProperties using the name
   8     int channel = joints_[command_msg_.name[i]].channel;
   9 
  10     // Tell the actuator to go to the home position
  11     actuators_[channel].home();
  12   }
  13 
  14   return true;
  15 ...

Trying it out

To try out this version, we need to create a new ROS launch file. This is identical to the launch file used in Example1, except the node names and configuration file names have been updated.

<launch>
  <node pkg="example_driver" type="example2_driver" name="robot2_driver" >
    <rosparam command="load" file="$(find example_driver)/cfg/example2.yaml" />
    <param name="robot_description_parameter" type="string" value="" />
  </node>
                
  <node pkg="actuator_array_gui" type="actuator_array_gui.py" name="robot2_gui">
    <rosparam command="load" file="$(find example_driver)/cfg/example2.yaml" />
    <param name="robot_description_parameter" type="string" value="" />
  </node>
</launch>

Wiki: actuator_array_driver/Tutorials/ActuatorArrayExample2 (last edited 2012-02-26 23:24:56 by StephenWilliams)