|
Size: 7858
Comment:
|
Size: 7858
Comment:
|
| Deletions are marked like this. | Additions are marked like this. |
| Line 73: | Line 73: |
| bool starting(); | void starting(); |
| 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 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.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 controller{
7
8 class MyController: 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 bool init(pr2_mechanism_model::RobotState *robot,
21 ros::NodeHandle &n);
22 void starting();
23 void update();
24 void stopping();
25 };
26 }
The changes in the header file:
4 #include <my_controller_pkg/SetAmplitude.h>
5
Here we include the autogenerated header file that defines the new service call we just created.
11 bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
12 my_controller_pkg::SetAmplitude::Response& resp);
Here we declare the method that will get called when the service is called.
16 double amplitude_;
17 ros::ServiceServer srv_;
Here we declare the service provider and a local variable to store the amplitude of the arm motion.
src/my_controller.cpp
1 #include "my_controller_pkg/my_controller.h"
2 #include <pluginlib/class_list_macros.h>
3
4 using namespace controller;
5
6
7 /// Controller initialization in non-realtime
8 bool MyController::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 &MyController::setAmplitude, this);
29
30 amplitude_ = 0.5;
31 return true;
32 }
33
34
35 /// Controller startup in realtime
36 bool MyController::starting()
37 {
38 init_pos_ = joint_state_->position_;
39 return true;
40 }
41
42
43 /// Controller update loop in realtime
44 void MyController::update()
45 {
46 double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
47 double current_pos = joint_state_->position_;
48 joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
49 }
50
51
52 /// Controller stopping in realtime
53 void MyController::stopping()
54 {}
55
56
57 /// Service call to set amplitude of sin
58 bool MyController::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
59 my_controller_pkg::SetAmplitude::Response& resp)
60 {
61 if (fabs(req.amplitude) < 2.0){
62 amplitude_ = req.amplitude;
63 ROS_INFO("Mycontroller: set amplitude to %f", req.amplitude);
64 }
65 else
66 ROS_WARN("MyController: requested amplitude %f to large", req.amplitude);
67
68 resp.amplitude = amplitude_;
69 return true;
70 }
71
72
73 /// Register controller to pluginlib
74 PLUGINLIB_REGISTER_CLASS(MyController,
75 controller::MyController,
76 pr2_controller_interface::Controller)
The changes in the cpp file are:
27 srv_ = n.advertiseService("set_amplitude",
28 &MyControllerClass::setAmplitude, this);
29
30 amplitude_ = 0.5;
Here we create the service call, give it a name, and connect it with the setAmplitude method.
46 double current_pos = joint_state_->position_;
In the update loop we use the new local variable that specifies the amplitude of the arm motion.
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
Here we implement the service call itself. Note how the requested amplitude is not accepted if it is larger than 1.0.
Testing the communication
So now we're ready to test this extension to the controller. First build the controller:
$ make
In order for this new code to get loaded into the controller manager, you either need to restart gazebo, OR call the pr2_mechanism_msgs/ReloadControllerLibraries service.
So, if you don't have gazebo running already, just type:
$ roslaunch gazebo 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_tutorial_controller/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 1.0
$ rosservice call /my_tutorial_controller/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.
