Additionally, we will explore the initialization process in detail. In the first two examples, the convenience function 'init()' was used. Here we will perform initialization by manually calling helper functions provided by the base class.
| Note: This tutorial assumes that you have completed the previous tutorials: Actuator Array Example2, URDF. |
 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 Example3
Description: In this example we will set parameters for each servo based on information stored in the urdf robot description. Because the base class was designed to use the robot description information, there are very few code changes. The only difference is the custom init_actuator_() function now makes use of information obtained from the robot description instead of using hard-coded defaults. The only extra work really involved is in the creation of the robot description file itself.Tutorial Level: ADVANCED
Next Tutorial: Simulating actuator arrays with Gazebo
Creating the driver class
This tutorial assumes you have completed Example2. For the first step, we will copy the final version of example2_driver.cpp to example3_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 // Create a custom JointProperties struct
8 struct Example3JointProperties : public JointProperties
9 {
10 int channel;
11 double home;
12 };
13
14 class Example3Driver : public ActuatorArrayDriver<Example3JointProperties>
15 {
16 private:
17 std::map<int, DummyActuator> actuators_;
18 ros::Time previous_time_;
19 public:
20 Example3Driver()
21 {
22 ActuatorArrayDriver::init();
23 };
24 virtual ~Example3Driver() {};
25
26 bool init_actuator_(const std::string& joint_name,
27 Example3JointProperties& joint_properties,
28 XmlRpc::XmlRpcValue& joint_data)
29 {
30 // Read custom data from the XMLRPC struct
31 if (joint_data.hasMember("channel"))
32 {
33 joint_properties.channel = (int) joint_data["channel"];
34 }
35
36 if (joint_data.hasMember("home"))
37 {
38 joint_properties.home = (double) joint_data["home"];
39 }
40
41 // Create a dummy actuator object and store in a container
42 actuators_[joint_properties.channel] =
43 DummyActuator(-1.57, 1.57, 10.0, joint_properties.home);
44
45 return true;
46 }
47
48 bool read_(ros::Time ts = ros::Time::now())
49 {
50 // Calculate the time from the last update
51 double dt = (ts - previous_time_).toSec();
52
53 // Loop through each joint and request the current status
54 for (unsigned int i = 0; i < joint_state_msg_.name.size(); ++i)
55 {
56 // Look up the channel number from the JointProperties using the name
57 int channel = joints_[command_msg_.name[i]].channel;
58
59 // Update the simulated state of each actuator by dt seconds
60 actuators_[channel].update(dt);
61
62 // Query the current state of each actuator
63 joint_state_msg_.position[i] = actuators_[channel].getPosition();
64 joint_state_msg_.velocity[i] = actuators_[channel].getVelocity();
65 joint_state_msg_.effort[i] = actuators_[channel].getMaxTorque();
66 }
67
68 joint_state_msg_.header.stamp = ts;
69 previous_time_ = ts;
70
71 return true;
72 }
73
74 bool command_()
75 {
76 // Loop through each joint in the command message and send the
77 // corresponding servo the desired behavior
78 for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
79 {
80 // Look up the channel number from the JointProperties using the name
81 int channel = joints_[command_msg_.name[i]].channel;
82
83 // Send the actuator the desired position and velocity
84 actuators_[channel].setVelocity(command_msg_.velocity[i]);
85 actuators_[channel].setPosition(command_msg_.position[i]);
86 }
87
88 return true;
89 }
90
91 bool stop_()
92 {
93 // Loop through each joint and send the stop command
94 for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
95 {
96 // Look up the channel number from the JointProperties using the name
97 int channel = joints_[command_msg_.name[i]].channel;
98
99 // Tell the actuator to stop
100 actuators_[channel].stop();
101 }
102
103 return true;
104 }
105
106 bool home_()
107 {
108 // Loop through each joint and send the stop command
109 for (unsigned int i = 0; i < command_msg_.name.size(); ++i)
110 {
111 // Look up the channel number from the JointProperties using the name
112 int channel = joints_[command_msg_.name[i]].channel;
113
114 // Tell the actuator to go to the home position
115 actuators_[channel].home();
116 }
117
118 return true;
119 }
120
121 };
122
123 int main(int argc, char** argv)
124 {
125 ros::init(argc, argv, "example3_driver");
126 Example3Driver driver;
127 driver.spin();
128
129 return 0;
130 }
Next, we add the new driver node to the CMakeLists.txt file so it will be compiled.
... rosbuild_add_executable(example3_driver src/example3_driver.cpp src/dummy_actuator.cpp) ...
Manually initializing the driver
For purely illustrative purposes, we will manually call all of the base class helper functions in the proper order. This is done instead of us using the all-in-one init() convenience call.
First, we need to create two ros node handles; one in the public namespace for advertising and subscribing to topics, and a second one in the private namespace for talking to the parameter server.
Next, we need to get the name of the parameter that holds the robot description. This is stored on the Parameter Server, and defaults to robot_description.
Now that we have the parameter name, we tell the base class to parse the robot description. The results are held in the urdf_model_ variable in the base class.
Now we will fill in the joints_ container with information from joints variable on the paramerter server and any available information inside the urdf_model_ variable. Only joints specified in the Parameter Server joints variable will be used. This is the function that calls init_actuator_() for each joint in the list. This also configures the command_msg_ and joint_state_msg_ to hold the proper number of joints and fills in the name portion of both messages.
Finally, we will advertise the joint_states topic, the stop and home services, and subscribe to the command topic.
Using the robot description
The base class automatically parses the robot description and stores that information inside the joints_ container. The main effort in using this functionality is creating the urdf itself. This example uses the following urdf describing a four-link arm.
<robot name="example_arm"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller">
<!-- Create a heavy base to which to attach the arm -->
<link name="base_link">
<inertial>
<origin xyz=" 0.000 0.000 0.025" rpy=" 0.000 0.000 0.000" />
<mass value="100.0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<collision>
<origin xyz=" 0.000 0.000 0.025" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.100 0.100 0.050" />
</geometry>
</collision>
<visual>
<origin xyz=" 0.000 0.000 0.025" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.100 0.100 0.050" />
</geometry>
<material name="base_link_color">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
</link>
<joint name="joint1" type="revolute">
<origin xyz=" 0.000 0.000 0.050" rpy=" 0.000 0.000 0.000" />
<parent link="base_link" />
<child link="link1" />
<axis xyz="0 0 1" />
<limit lower="-1.57" upper="+1.57" effort="10.0" velocity="15.71" />
<dynamics damping="0.0" friction="0.0" />
</joint>
<link name="link1">
<inertial>
<origin xyz=" 0.000 0.000 0.010" rpy=" 0.000 0.000 0.000" />
<mass value="0.50" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<collision>
<origin xyz=" 0.000 0.000 0.010" rpy=" 0.000 0.000 0.000" />
<geometry>
<cylinder radius="0.050" length="0.020" />
</geometry>
</collision>
<visual>
<origin xyz=" 0.000 0.000 0.010" rpy=" 0.000 0.000 0.000" />
<geometry>
<cylinder radius="0.050" length="0.020" />
</geometry>
<material name="link1_color">
<color rgba="0.5 0.5 0.5 1.0" />
</material>
</visual>
</link>
<joint name="joint2" type="revolute">
<origin xyz=" 0.000 0.000 0.030" rpy=" 0.000 0.000 0.000" />
<parent link="link1" />
<child link="link2" />
<axis xyz="0 1 0" />
<limit lower="-3.14" upper="0.0" effort="10.0" velocity="15.71" />
<dynamics damping="0.0" friction="0.0" />
</joint>
<link name="link2">
<inertial>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<mass value="0.10" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<collision>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.200 0.020 0.020" />
</geometry>
</collision>
<visual>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.200 0.020 0.020" />
</geometry>
<material name="link2_color">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
</link>
<joint name="joint3" type="revolute">
<origin xyz=" 0.200 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<parent link="link2" />
<child link="link3" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="10.0" velocity="15.71" />
<dynamics damping="0.0" friction="0.0" />
</joint>
<link name="link3">
<inertial>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<mass value="0.10" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<collision>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.200 0.020 0.020" />
</geometry>
</collision>
<visual>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.200 0.020 0.020" />
</geometry>
<material name="link3_color">
<color rgba="0.5 0.5 0.5 1.0" />
</material>
</visual>
</link>
<joint name="joint4" type="revolute">
<origin xyz=" 0.20000 0.00000 0.00000" rpy=" 0.00000 0.00000 0.00000" />
<parent link="link3" />
<child link="link4" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="10.0" velocity="15.71" />
<dynamics damping="0.0" friction="0.0" />
</joint>
<link name="link4">
<inertial>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<mass value="0.10" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
<collision>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.200 0.020 0.020" />
</geometry>
</collision>
<visual>
<origin xyz=" 0.100 0.000 0.000" rpy=" 0.000 0.000 0.000" />
<geometry>
<box size="0.20000 0.02000 0.02000" />
</geometry>
<material name="link4_color">
<color rgba="0.1 0.1 0.1 1.0" />
</material>
</visual>
</link>
</robot>The base class will parse the urdf and update the JointProperties object before calling init_actuator_(). This allows us to use the urdf information inside of the init_actuator_() function call to set joint limits, etc.
1 ...
2 bool Example3Driver::init_actuator_(const std::string& joint_name,
3 Example3JointProperties& 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
12 if (joint_data.hasMember("home"))
13 {
14 joint_properties.home = (double) joint_data["home"];
15 }
16
17 // Create a dummy actuator object and store in a container
18 actuators_[joint_properties.channel] =
19 DummyActuator(joint_properties.min_position,
20 joint_properties.max_position,
21 joint_properties.max_velocity,
22 joint_properties.home);
23 return true;
24 }
25 ...
Finally, we modify the launch file to load the urdf into the robot_description parameter. Additionally, we have started a robot_state_publisher node. This node subscribes to the joint_states topic and publishes to the tf system. This allows the use of tools such as rviz to monitor the current state of the robot.
<launch>
<!-- Load Robot Description onto the Parameter Server -->
<param name="robot_description" file="$(find example_driver)/robots/example_arm.urdf" />
<!-- Start the Example Driver -->
<node pkg="example_driver" type="example3_driver" name="robot3_driver">
<rosparam command="load" file="$(find example_driver)/cfg/example3.yaml" />
</node>
<!-- Start the manual control GUI -->
<node pkg="actuator_array_gui" type="actuator_array_gui.py" name="robot3_gui">
<rosparam command="load" file="$(find example_driver)/cfg/example3.yaml" />
</node>
<!-- Start the state publisher -->
<node pkg="robot_state_publisher" type="state_publisher" name="robot3_state" >
<param name="tf_prefix" value="robot3" />
</node>
</launch>Rviz monitoring the current state of the example arm. 
