| Note: This tutorial assumes that you have completed the previous tutorials: Running 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. |
Communicating with a realtime joint controller
Description: This tutorial teaches you how to communicate with a controller over ROS.Tutorial Level: INTERMEDIATE
Next Tutorial: Adding a PID to a realtime joint controller
Introduction
This tutorial builds on the controller we created in the previous tutorial. Here we will add a ROS service call to the controller, to change the amplitude of the arm motion on the fly. If you don't yet know what a ROS service is, check out the ROS tutorials first.
Adding the service
First we specify the service call we'll use to communicate with the controller. Since the only thing we want to do is set the amplitude, the service request (above the "---") only has one value. The service response (below the "---") contains the actual amplitude that was set. The request and response may be different if the requested amplitude is invalid. So, first create a directory srv, a file called srv/SetAmplitude.srv, and copy-paste the following service description:
float64 amplitude --- float64 amplitude
Then, we need to tell cmake to generate the C++ headers from the above service description. Therefore simply add the following line to your CMakeLists.txt:
rosbuild_gensrv()
Changing the code
Now we need to add the service call to our controller code. The resulting code (shown below) is very similar to the original controller code.
include/my_controller_pkg/my_controller_file.h
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
6 namespace my_controller_ns{
7
8 class MyControllerClass: public pr2_controller_interface::Controller
9 {
10 private:
11 bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
12 my_controller_pkg::SetAmplitude::Response& resp);
13
14 pr2_mechanism_model::JointState* joint_state_;
15 double init_pos_;
16 double amplitude_;
17 ros::ServiceServer srv_;
18
19 public:
20 virtual bool init(pr2_mechanism_model::RobotState *robot,
21 ros::NodeHandle &n);
22 virtual void starting();
23 virtual void update();
24 virtual void stopping();
25 };
26 }
The changes in the header file:
Here we include the auto-generated header file that defines the new service call we just created, as well as the ros header file to define the service.
Here we declare the method that will get called when the service is called.
Here we declare the service provider and a local variable to store the amplitude of the arm motion.
src/my_controller_file.cpp
1 #include "my_controller_pkg/my_controller_file.h"
2 #include <pluginlib/class_list_macros.h>
3
4 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 return true;
32 }
33
34
35 /// Controller startup in realtime
36 void MyControllerClass::starting()
37 {
38 init_pos_ = joint_state_->position_;
39 }
40
41
42 /// Controller update loop in realtime
43 void MyControllerClass::update()
44 {
45 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
46 double current_pos = joint_state_->position_;
47 joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
48 }
49
50
51 /// Controller stopping in realtime
52 void MyControllerClass::stopping()
53 {}
54
55
56 /// Service call to set amplitude of sin
57 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
58 my_controller_pkg::SetAmplitude::Response& resp)
59 {
60 if (fabs(req.amplitude) < 2.0){
61 amplitude_ = req.amplitude;
62 ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
63 }
64 else
65 ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
66
67 resp.amplitude = amplitude_;
68 return true;
69 }
70 } // namespace
71
72 /// Register controller to pluginlib
73 PLUGINLIB_REGISTER_CLASS(MyControllerPlugin,
74 my_controller_ns::MyControllerClass,
75 pr2_controller_interface::Controller)
The changes in the cpp file are:
Here we create the service call, give it a name, and connect it with the setAmplitude method.
45 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
In the update loop we use the new local variable that specifies the amplitude of the arm motion.
56 /// Service call to set amplitude of sin
57 bool MyControllerClass::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
58 my_controller_pkg::SetAmplitude::Response& resp)
59 {
60 if (fabs(req.amplitude) < 2.0){
61 amplitude_ = req.amplitude;
62 ROS_INFO("Mycontroller: set amplitude too %f", req.amplitude);
63 }
64 else
65 ROS_WARN("MyController: requested amplitude %f too large", req.amplitude);
66
67 resp.amplitude = amplitude_;
68 return true;
69 }
Here we implement the service call itself. Note how the requested amplitude is only accepted if it is less than 2.0.
Testing the communication
So now we're ready to test this extension to the controller. First build the controller, which compiles the code into the library:
$ make
In order for this new code to get loaded into the realtime process, you either need to restart gazebo and hence the entire process, OR use the pr2_controller_manager to request a reload of the libraries.
So, if you don't have gazebo running already, just type:
$ roslaunch gazebo_worlds empty_world.launch $ roslaunch pr2_gazebo pr2.launch
But if gazebo was still running from the previous tutorial, run:
$ rosrun pr2_controller_manager pr2_controller_manager reload-libraries
And now, launch our new controller:
$ roslaunch my_controller.launch
Now, let's call this brand new service:
$ rosservice call /my_controller_name/set_amplitude 0.1
You should see the response is 0.1, and the arm should start moving with a smaller amplitude.
If you try to set the amplitude to a value greater than 2.0
$ rosservice call /my_controller_name/set_amplitude 2.9
You'll see that the response did not change from the last time you set the amplitude. Large amplitudes are rightfully rejected.
You're now ready to start the next tutorial and learn how to add a PID to your controller.
