## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note = 
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Actuator Array Example1
## multi-line description to be displayed in search 
## description = There are three example drivers inside the package [[actuator_array_example]]. The first example demonstrates the use of the [[actuator_array_driver]] base class. This example makes use of convenience classes provided in the base class to read a simple list of actuators from the parameter server and set up the standard Actuator Array Driver communication interface. A Dummy Actuator class is used to simulate the operation of a real R/C Servo motor. This removes the need to have specific hardware to test the basic Actuator Array Driver system. The code contained in this tutorial is available in the [[actuator_array_example]] package.
## the next tutorial description (optional)
## next = 
## links to next tutorial (optional)
## next.0.link=[[actuator_array_driver/Tutorials/ActuatorArrayExample2|Actuator Array Example2]]
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

=== 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 [[ROS/Tutorials/CreatingPackage|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]].

{{{
#!cplusplus
#include <actuator_array_driver/actuator_array_driver.h>

using namespace actuator_array_driver;

class Example1Driver : public ActuatorArrayDriver<JointProperties>
{
public:
  Example1Driver() {};
  virtual ~Example1Driver() {};
};

}}}

=== 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. 

{{{
#!cplusplus
...
int main(int argc, char** argv)
{
  ros::init(argc, argv, "example1_driver");
  Example1Driver driver;
  driver.spin();

  return 0;
}
}}}

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:

{{{
#!cplusplus
...
class Example1Driver : public ActuatorArrayDriver<JointProperties>
{
public:
  Example1Driver()
  { 
    ActuatorArrayDriver::init();
  };
...
}}}

Now we can create a [[YAML Overview|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 [[roslaunch|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.
{{{
#!cplusplus
...
using namespace actuator_array_example;
class Example1Driver : public ActuatorArrayDriver<JointProperties>
{
private:
  std::map<int, DummyActuator> actuators_;
...
}}}

Finally, we fill the container with actuator instances in the constructor.
{{{
#!cplusplus
...
  Example1Driver()
  { 
    ActuatorArrayDriver::init();

    for(unsigned int i = 0; i < command_msg_.name.size(); ++i)
    {
      actuators_[i] = DummyActuator();
    }
  };
...
}}}

==== 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.

{{{
#!cplusplus
...
~Example1Driver() {};
bool command_()
{
  // Loop through each joint in the command message and send the
  // corresponding servo the desired behavior
  for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
  {
    actuators_[i].setVelocity(command_msg_.velocity[i]);
    actuators_[i].setPosition(command_msg_.position[i]);
  }

  return true;
}
...
}}}


==== 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.

{{{
#!cplusplus
...
private:
  ros::Time previous_time_;
public:
  bool read_(ros::Time ts = ros::Time::now())
  {
    // Calculate the time from the last update
    double dt = (ts - previous_time_).toSec();

    for (unsigned int i = 0; i < joint_state_msg_.name.size(); ++i)
    {
      // Update the simulated state of each actuator by dt seconds
      actuators_[i].update(dt);

      // Query the current state of each actuator
      joint_state_msg_.position[i] = actuators_[i].getPosition();
      joint_state_msg_.velocity[i] = actuators_[i].getVelocity();
      joint_state_msg_.effort[i]   = actuators_[i].getMaxTorque();
    }

    joint_state_msg_.header.stamp = ts;
    previous_time_ = ts;

    return true;
  }
...
}}}

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 ====

{{{
#!cplusplus
...
  bool stop_()
  {
    // Loop through each joint and send the stop command
    for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
    {
      // Update the simulated state of each actuator by dt seconds
      actuators_[i].stop();
    }

    return true;
  }
...
}}}

==== Implement the 'home_' function ====

{{{
#!cplusplus
...
bool home_()
{
  // Loop through each joint and send the home command
  for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
  {
    // Update the simulated state of each actuator by dt seconds
    actuators_[i].home();
  }

  return true;
}
...
}}}


=== 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.



## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE