| Note: This tutorial assumes that you have completed the previous tutorials: Communicating with a realtime joint controller. |
 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. |
Capturing data from a controller
Description: This tutorial teaches you how to store data in a controller and extract it for offline viewing, debugging, and tuning.Tutorial Level: INTERMEDIATE
Next Tutorial: Plotting controller data in matlab
Contents
Introduction
Debugging and even tuning a controller can be very difficult if you can't observe signals. Running a debugger is impossible as it violates the realtime requirements. And missing signals for even one cycle can mean you are unable to reconstruct what happened.
So this tutorial adds the ability to store and extract signals, which can be viewed, analyzed, and manipulated offline to examine the system. In particular, we capture signals over a fixed time window (generally several seconds). We store the signals in a storage buffer filled from within the realtime update() function. When the buffer is full, we publish to a ROS topic from outside realtime. This indirection (store, then publish) is realtime safe and guarantees that every cycle will be captured.
Please note this tutorial follows the tutorial on Communicating with a realtime joint controller, as we will leverage this code for tuning in later tutorials. Also, if you are unfamiliar with a ROS topic, check out the ROS tutorials first.
Adding the ROS message
We define a ROS message both to give structure to our storage buffer and to allow easy extraction via a ROS topic.
In the package, create the directory msg, the file msg/MyStateMessage.msg, and copy-paste the following message definition:
float64 dt float64 position float64 desired_position float64 velocity float64 desired_velocity float64 commanded_effort float64 measured_effort
We will store these variables and be able to view them offline.
To auto-generate the necessary source and header files for this message, add the following line to your CmakeLists.txt:
rosbuild_genmsg()
Including the standard services
To trigger the entire behavior, capturing data and export via the ROS topic, we use a standard ROS service. In particular, we use the 'Empty' service, as we need no arguments.
This requires us to depend on the standard services package, which we declare in the manifest.xml file. You need to explicitly add the following depend statement to manifest.xml in the <package> <\package> scope:
<depend package="std_srvs"/>
Changing the header file
We need to update the header file include/my_controller_pkg/my_controller_file.h to define the storage buffer and publisher. This result is (changes are given afterwards):
1 #include <pr2_controller_interface/controller.h>
2 #include <pr2_mechanism_model/joint.h>
3 #include <ros/ros.h>
4 #include <my_controller_pkg/SetAmplitude.h>
5 #include <std_srvs/Empty.h>
6 #include "my_controller_pkg/MyStateMessage.h"
7
8 namespace my_controller_ns{
9
10 enum
11 {
12 StoreLen = 5000
13 };
14
15 class MyControllerClass: public pr2_controller_interface::Controller
16 {
17 private:
18 bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
19 my_controller_pkg::SetAmplitude::Response& resp);
20
21 pr2_mechanism_model::JointState* joint_state_;
22 double init_pos_;
23 double amplitude_;
24 ros::ServiceServer srv_;
25
26 bool capture(std_srvs::Empty::Request& req,
27 std_srvs::Empty::Response& resp);
28 ros::ServiceServer capture_srv_;
29 ros::Publisher mystate_pub_;
30 my_controller_pkg::MyStateMessage storage_[StoreLen];
31 volatile int storage_index_;
32
33 public:
34 bool init(pr2_mechanism_model::RobotState *robot,
35 ros::NodeHandle &n);
36 void starting();
37 void update();
38 void stopping();
39 };
40 }
The changes in the header file are:
Here we include the empty service header file (which we will use to trigger the entire capture behavior), as well as the auto-generated message header file that defines the new message we just created.
Here we declare the length of the storage buffer. 5000 samples corresponds to a 5 second time window.
Here we
- declare the capture function that will execute the entire behavior,
- declare the service provider,
- declare the message publisher that will publish the messages once ready,
- define a storage buffer, using the message to give it structure,
- an index into the storage buffer. Note the index is declared as a volatile int, as it will be updated inside the realtime update() loop but also monitored from outside realtime.
Changing the code
The updated source code file src/my_controller_file.cpp in full is (changes are again given afterwards):
1 #include "my_controller_pkg/my_controller_file.h"
2 #include <pluginlib/class_list_macros.h>
3
4 using namespace my_controller_ns;
5
6
7 /// Controller initialization in non-realtime
8 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
9 ros::NodeHandle &n)
10 {
11 std::string joint_name;
12 if (!n.getParam("joint_name", joint_name))
13 {
14 ROS_ERROR("No joint given in namespace: '%s')",
15 n.getNamespace().c_str());
16 return false;
17 }
18
19 joint_state_ = robot->getJointState(joint_name);
20 if (!joint_state_)
21 {
22 ROS_ERROR("MyController could not find joint named '%s'",
23 joint_name.c_str());
24 return false;
25 }
26
27 srv_ = n.advertiseService("set_amplitude",
28 &MyControllerClass::setAmplitude, this);
29
30 amplitude_ = 0.5;
31
32 capture_srv_ = n.advertiseService("capture", &MyControllerClass::capture, this);
33 mystate_pub_ = n.advertise<my_controller_pkg::MyStateMessage>("mystate_topic", StoreLen);
34 storage_index_ = StoreLen;
35
36 return true;
37 }
38
39
40 /// Controller startup in realtime
41 void MyControllerClass::starting()
42 {
43 init_pos_ = joint_state_->position_;
44 }
45
46
47 /// Controller update loop in realtime
48 void MyControllerClass::update()
49 {
50 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
51 double current_pos = joint_state_->position_;
52 joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
53
54 int index = storage_index_;
55 if ((index >= 0) && (index < StoreLen))
56 {
57 storage_[index].dt = 0.0;
58 storage_[index].position = joint_state_->position_;
59 storage_[index].desired_position = desired_pos;
60 storage_[index].velocity = joint_state_->velocity_;
61 storage_[index].desired_velocity = 0.0;
62 storage_[index].commanded_effort = joint_state_->commanded_effort_;
63 storage_[index].measured_effort = joint_state_->measured_effort_;
64
65 // Increment for the next cycle.
66 storage_index_ = index+1;
67 }
68 }
69
70
71 /// Controller stopping in realtime
72 void MyControllerClass::stopping()
73 {}
74
75
76 /// Service call to set amplitude of sin
77 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
78 my_controller_pkg::SetAmplitude::Response& resp)
79 {
80 if (fabs(req.amplitude) < 2.0){
81 amplitude_ = req.amplitude;
82 ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
83 }
84 else
85 ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
86
87 resp.amplitude = amplitude_;
88 return true;
89 }
90
91
92 /// Service call to capture and extract the data
93 bool MyControllerClass::capture(std_srvs::Empty::Request& req,
94 std_srvs::Empty::Response& resp)
95 {
96 /* Record the starting time. */
97 ros::Time started = ros::Time::now();
98
99 /* Mark the buffer as clear (which will start storing). */
100 storage_index_ = 0;
101
102 /* Now wait until the buffer is full. */
103 while (storage_index_ < StoreLen)
104 {
105 /* Sleep for 1ms as not to hog the CPU. */
106 ros::Duration(0.001).sleep();
107
108 /* Make sure we don't hang here forever. */
109 if (ros::Time::now() - started > ros::Duration(20.0))
110 {
111 ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
112 return false;
113 }
114 }
115
116 /* Then we can publish the buffer contents. */
117 int index;
118 for (index = 0 ; index < StoreLen ; index++)
119 mystate_pub_.publish(storage_[index]);
120
121 return true;
122 }
123
124
125 /// Register controller to pluginlib
126 PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
127 my_controller_ns::MyControllerClass,
128 pr2_controller_interface::Controller)
The changes in the cpp file are:
4 using namespace my_controller_ns;
A small change in namespace usage, compared to the previous "namespace my_controller_ns {...}"
Here we
- create the service call, give it a name, and connect it with the capture method,
- create the publisher, give it a name, and specify the queue length to equal the buffer length (this assures that no outgoing message will be lost),
- set the index to the full length, indicating not to do any storage.
54 int index = storage_index_;
55 if ((index >= 0) && (index < StoreLen))
56 {
57 storage_[index].dt = 0.0;
58 storage_[index].position = joint_state_->position_;
59 storage_[index].desired_position = desired_pos;
60 storage_[index].velocity = joint_state_->velocity_;
61 storage_[index].desired_velocity = 0.0;
62 storage_[index].commanded_effort = joint_state_->commanded_effort_;
63 storage_[index].measured_effort = joint_state_->measured_effort_;
64
65 // Increment for the next cycle.
66 storage_index_ = index+1;
67 }
At the end of the update loop, we fill in the current buffer entries. We check to verify the index is in range and then populate the data. Note for now, the time step and desired velocity are meaningless and set to zero. We could have removed them from the message definition, but are expecting to use them soon. Finally, we increment the index.
92 /// Service call to capture and extract the data
93 bool MyControllerClass::capture(std_srvs::Empty::Request& req,
94 std_srvs::Empty::Response& resp)
95 {
96 /* Record the starting time. */
97 ros::Time started = ros::Time::now();
98
99 /* Mark the buffer as clear (which will start storing). */
100 storage_index_ = 0;
101
102 /* Now wait until the buffer is full. */
103 while (storage_index_ < StoreLen)
104 {
105 /* Sleep for 1ms as not to hog the CPU. */
106 ros::Duration(0.001).sleep();
107
108 /* Make sure we don't hang here forever. */
109 if (ros::Time::now() - started > ros::Duration(20.0))
110 {
111 ROS_ERROR("Waiting for buffer to fill up took longer than 20 seconds!");
112 return false;
113 }
114 }
115
116 /* Then we can publish the buffer contents. */
117 int index;
118 for (index = 0 ; index < StoreLen ; index++)
119 mystate_pub_.publish(storage_[index]);
120
121 return true;
122 }
Here we implement the service call itself. Normally, the storage index sits at the full length and the update loop does not change it's value. This service resets the index to zero, which will initiate storage in the update loop. It then waits until the storage loop has completed the data collection. It sleeps to avoid hogging the CPU and times out at 20 seconds in case the controller is not running. Finally, it publishes the full buffer contents, one message at a time.
Testing the data capture
Now we're ready to test the data capture. First re-build the controller:
$ make
In order for this new code to get loaded into the controller manager, you either need to restart gazebo or the robot, OR re-load the controller libraries with
$ rosrun pr2_controller_manager pr2_controller_manager reload-libraries
And now, launch the udpated controller:
$ roslaunch my_controller.launch
To see whether the storage is successful, we need to listen to the new topic:
$ rostopic echo /my_controller_name/mystate_topic
This should initially report nothing. In a separate window trigger the data capture:
$ rosservice call /my_controller_name/capture
and the rostopic command should (after 5 seconds) display lots of data.
To analyze the data, you may want to save and plot the signals, as explained in the next tutorial, Plotting controller data in matlab.
