| 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
Contents
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.
and we will change the template argument of our driver to our new Example2JointProperties class.
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.
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>