|
Size: 5863
Comment:
|
Size: 6401
Comment:
|
| Deletions are marked like this. | Additions are marked like this. |
| Line 78: | Line 78: |
| The changes in the header file are seen in lines | The changes in the header file are seen in lines 11-12 and 16-17: |
| Line 80: | Line 80: |
| <<CodeRef(controller_h, 2, 2)>> | <<CodeRef(controller_h, 11, 12)>> Here we add the method that will get called when the service is called. <<CodeRef(controller_h, 16, 17)>> Here we add the service provider and a local variable to store the amplitude of the arm motion. |
| Line 163: | Line 166: |
| The changes in the header file are seen in lines 11-12 and 16-17: <<CodeRef(controller_cpp, 11, 12)>> Here we add the method that will get called when the service is called. <<CodeRef(controller_cpp, 16, 17)>> Here we add the service provider and a local variable to store the amplitude of the arm motion. |
| 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 is shown below, and is obviously 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 const ros::NodeHandle &n);
22 bool starting();
23 void update();
24 void stopping();
25 };
26 }
The changes in the header file are seen in lines 11-12 and 16-17:
11 bool setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
12 my_controller_pkg::SetAmplitude::Response& resp);
Here we add the method that will get called when the service is called.
16 double amplitude_;
17 ros::ServiceServer srv_;
Here we add 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-relatime
8 bool MyController::init(pr2_mechanism_model::RobotState *robot,
9 const 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 ros::NodeHandle n2(n);
28 srv_ = n2.advertiseService("set_amplitude",
29 &MyController::setAmplitude, this);
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 * (desired_pos - current_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 header file are seen in lines 11-12 and 16-17:
11 std::string joint_name;
12 if (!n.getParam("joint_name", joint_name))
Here we add the method that will get called when the service is called.
16 return false;
17 }
Here we add the service provider and a local variable to store the amplitude of the arm motion.
Testing the communication
$ rosservice call /my_tutorial_controller/set_amplitude 0.1
$ rosservice call /my_tutorial_controller/set_amplitude 2.9
