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 bool MyController::init(pr2_mechanism_model::RobotState *robot,
   7                         const ros::NodeHandle &n)
   8 {
   9   std::string joint_name;
  10   if (!n.getParam("joint_name", joint_name))
  11   {
  12     ROS_ERROR("No joint given in namespace: %s)",
  13               n.getNamespace().c_str());
  14     return false;
  15   }
  16 
  17   joint_state_ = robot->getJointState(joint_name);
  18   if (!joint_state_)
  19   {
  20     ROS_ERROR("MyController could not find joint named \"%s\"",
  21               joint_name.c_str());
  22     return false;
  23   }
  24 
  25   ros::NodeHandle n2(n);
  26   srv_ = n2.advertiseService("set_amplitude", 
  27                              &MyController::setAmplitude, this);
  28   amplitude_ = 0.5;
  29   return true;
  30 }
  31 
  32 bool MyController::starting()
  33 {
  34   init_pos_ = joint_state_->position_;
  35   return true;
  36 }
  37 
  38 void MyController::update()
  39 {
  40   double desired_pos = init_pos_ + amplitude_ * sin(ros::Time::now().toSec());
  41   double current_pos = joint_state_->position_;
  42   joint_state_->commanded_effort_ = 10 * (desired_pos - current_pos);
  43 }
  44 
  45 void MyController::stopping()
  46 {}
  47 
  48 bool MyController::setAmplitude(my_controller_pkg::SetAmplitude::Request& req,
  49                                 my_controller_pkg::SetAmplitude::Response& resp)
  50 {
  51   if (fabs(req.amplitude) < 2.0){
  52     amplitude_ = req.amplitude;
  53     ROS_INFO("Mycontroller: set amplitude to %f", req.amplitude);
  54   }
  55   else
  56     ROS_WARN("MyController: requested amplitude %f to large", req.amplitude);
  57 
  58   resp.amplitude = amplitude_;
  59   return true;
  60 }
  61 
  62 
  63 PLUGINLIB_REGISTER_CLASS(MyController,
  64                          controller::MyController,
  65                          pr2_controller_interface::Controller)