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 /// Controller startup in realtime
  35 bool MyController::starting()
  36 {
  37   init_pos_ = joint_state_->position_;
  38   return true;
  39 }
  40 
  41 /// Controller update loop in realtime
  42 void MyController::update()
  43 {
  44   double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
  45   double current_pos = joint_state_->position_;
  46   joint_state_->commanded_effort_ = 10 * (desired_pos - current_pos);
  47 }
  48 
  49 
  50 /// Controller stopping in realtime
  51 void MyController::stopping()
  52 {}
  53 
  54 
  55 /// Service call to set amplitude of sin
  56 bool MyController::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  57                                 my_controller_pkg::SetAmplitude::Response& resp)
  58 {
  59   if (fabs(req.amplitude) < 2.0){
  60     amplitude_ = req.amplitude;
  61     ROS_INFO("Mycontroller: set amplitude to %f", req.amplitude);
  62   }
  63   else
  64     ROS_WARN("MyController: requested amplitude %f to large", req.amplitude);
  65 
  66   resp.amplitude = amplitude_;
  67   return true;
  68 }
  69 
  70 /// Register controller to pluginlib
  71 PLUGINLIB_REGISTER_CLASS(MyController,
  72                          controller::MyController,
  73                          pr2_controller_interface::Controller)