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 }

   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)