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 Example1

Creating a new driver package

First, we need to create a new package for our example. For detailed information on how to create a new ROS package, see this tutorial. This package should be dependent on the actuator_array_driver package as well as roscpp.

$ roscreate-pkg example_driver roscpp actuator_array_driver

Creating the driver class

Next we need to create a new class for our driver implementation. This class needs to use the ActuatorArrayDriver as its base. Using your favorite text editor, create a source file in the standard location, example_driver/src/example1_driver.cpp. The ActuatorArrayDriver base class is templated on a JointProperties structure. In this first example, we will use the base implementation provided in actuator_array_driver.

   1 #include <actuator_array_driver/actuator_array_driver.h>
   2 
   3 using namespace actuator_array_driver;
   4 
   5 class Example1Driver : public ActuatorArrayDriver<JointProperties>
   6 {
   7 public:
   8   Example1Driver() {};
   9   virtual ~Example1Driver() {};
  10 };

Adding the `main` function

We now add a simple main function that creates an instance of our new class and puts it in an infinite read-publish loop using the spin() function call.

   1 ...
   2 int main(int argc, char** argv)
   3 {
   4   ros::init(argc, argv, "example1_driver");
   5   Example1Driver driver;
   6   driver.spin();
   7 
   8   return 0;
   9 }

Before we can compile our new node, we have to tell CMake about. Edit CmakeLists.txt and add the following line.

...
rosbuild_add_executable(example1_driver src/example1_driver.cpp)
...

This is the minimum possible implementation. You should be able to compile it using the following command, but it will not actually perform any useful functions.

$ rosmake example_driver

Reading a list of actuators

The base class provides several helper functions for reading in a list of actuators from the Parameter Server, parsing a urdf robot description, and setting up the standard ROS topics. All of these functions have been included in a single, convenience function call, init(). First, we add this call to the constructor:

   1 ...
   2 class Example1Driver : public ActuatorArrayDriver<JointProperties>
   3 {
   4 public:
   5   Example1Driver()
   6   { 
   7     ActuatorArrayDriver::init();
   8   };
   9 ...

Now we can create a YAML configuration file to specify the set of joint names this driver will control. There are two YAML array formats the base class understands. In this first example, we will use the simple one.

Using you favorite text editor, create example_driver/cfg/example1.yaml:

joints:
  - joint1
  - joint2
  - joint3
  - joint4

Launching our example

Next, we create a ROS launch file to load the YAML configuration file and start our new driver. At the same time we will also start the actuator_array_gui because our new driver already has the topics in place to communicate with the control GUI (though no code yet to actually execute the received commands). Also, because this example does not use a URDF robot description, we must set the robot_description_parameter parameter to an empty string.

Again, using you favorite text editor, create the launch file example_driver/launch/example1.launch:

<launch>
  <node pkg="example_driver" type="example1_driver" name="robot1_driver" >
    <rosparam command="load" file="$(find example_driver)/cfg/example1.yaml" />
    <param name="robot_description_parameter" type="string" value="" />
  </node>
                
  <node pkg="actuator_array_gui" type="actuator_array_gui.py" name="robot1_gui">
    <rosparam command="load" file="$(find example_driver)/cfg/example1.yaml" />
    <param name="robot_description_parameter" type="string" value="" />
  </node>
</launch>

This may be run using:

$ roslaunch example_driver example1.launch

You should now see the GUI appear with the joint names specified in the YAML file. However, the GUI will be unresponsive as we have not implemented any of the read or write functions yet.

Implementing `read_` and `command_`

In the rest of this tutorial we will make use of a software-simulated servo motor, called a DummyActuator. This class is provided in the actuator_array_example package to facilitate the exploration of the Actuator Array stack without requiring specific hardware.

Copy the Dummy Actuator implementation

First, copy dummy_actuator.h and dummay_actuator.cpp from the actuator_array_example package into this package.

$ cp `rospack find actuator_array_example`/include/actuator_array_example/dummy_actuator.h example_driver/include/example_driver/dummy_actuator.h

$ cp `rospack find actuator_array_example`/src/dummy_actuator.cpp example_driver/src/dummy_actuator.cpp

And change the include in the source file to point at the new location.

From:

...
#include <actuator_array_example/dummy_actuator.h>
...

To:

...
#include <example_driver/dummy_actuator.h>
...

Modify CMakeLists.txt to include the DummyActuator in the compile.

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

And include the Dummy Actuator header in our driver file.

...
#include <example_driver/dummy_actuator.h>
...

Now, we actually need to create some DummyActuator objects to use in our example driver. First, we create an STL container to hold our fake actuators.

   1 ...
   2 using namespace actuator_array_example;
   3 class Example1Driver : public ActuatorArrayDriver<JointProperties>
   4 {
   5 private:
   6   std::map<int, DummyActuator> actuators_;
   7 ...

Finally, we fill the container with actuator instances in the constructor.

   1 ...
   2   Example1Driver()
   3   { 
   4     ActuatorArrayDriver::init();
   5 
   6     for(unsigned int i = 0; i < command_msg_.name.size(); ++i)
   7     {
   8       actuators_[i] = DummyActuator();
   9     }
  10   };
  11 ...

Implement the 'command_' function

The base class provides hooks for custom implementations of the supported functionality. One of the first functions that should be implemented is the command_ function. This function is responsible for sending the commands contained within the command_msg_ message variable to the actual servos. Here we loop through each message entry and set the position and velocity of the corresponding actuator.

   1 ...
   2 ~Example1Driver() {};
   3 bool command_()
   4 {
   5   // Loop through each joint in the command message and send the
   6   // corresponding servo the desired behavior
   7   for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
   8   {
   9     actuators_[i].setVelocity(command_msg_.velocity[i]);
  10     actuators_[i].setPosition(command_msg_.position[i]);
  11   }
  12 
  13   return true;
  14 }
  15 ...

Implement the 'read_' function

The read function is slightly more complicated in this example. Since we are using a simulated servo, we must first update the simulation with the elapsed time, then query the current position etc. for each servo. The current status of each actuator is stored in the joint_state_msg_ variable. The base class will actually publish this message for us.

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

Recompiling and launching the example driver should now allow you to control the simulated servos using the GUI. However, the Stop and Home buttons will still not work.

Implementing `stop_` and `home_`

At this point, implementing the stop_ and home_ functions is trivial. In much the same way the command_ function works, we simply loop over all of the actuators and call the appropriate servo function.

Implement the 'stop_' function

   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       // Update the simulated state of each actuator by dt seconds
   8       actuators_[i].stop();
   9     }
  10 
  11     return true;
  12   }
  13 ...

Implement the 'home_' function

   1 ...
   2 bool home_()
   3 {
   4   // Loop through each joint and send the home command
   5   for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
   6   {
   7     // Update the simulated state of each actuator by dt seconds
   8     actuators_[i].home();
   9   }
  10 
  11   return true;
  12 }
  13 ...

Trying it out

Recompiling and launching the example driver should now allow you to control the simulated servos using the GUI, including the Stop and Home buttons. Try adjusting the requested velocity to 1.0 and sending a command. The position indicator bars should slowly move towards the goal. Click Stop should halt the progress of all servos.