test_controller.cpp
Go to the documentation of this file.
00001 #include "test_controller.h"
00002 #include <pluginlib/class_list_macros.h>
00003 #include <boost/thread/condition.hpp>
00004 
00005 using namespace my_controller_ns;
00006 
00007 
00009 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
00010                              ros::NodeHandle &n)
00011 {
00012   // copy robot pointer so we can access time
00013   robot_ = robot;
00014 
00015 
00016   // get joint
00017   // ----------
00018   std::string joint_name;
00019   if (!n.getParam("joint_name", joint_name))
00020   {
00021     ROS_ERROR("No joint name given in namespace: '%s')", n.getNamespace().c_str());
00022     return false;
00023   }
00024   joint_state_ = robot->getJointState(joint_name);
00025   if (!joint_state_)
00026   {
00027     ROS_ERROR("MyController could not find joint named '%s'",
00028               joint_name.c_str());
00029     return false;
00030   }
00031   if (!joint_state_->joint_->limits)
00032   {
00033     ROS_ERROR("MyController could not find limits of joint '%s'",
00034               joint_name.c_str());
00035     return false;
00036   }
00037   max_effort_ = joint_state_->joint_->limits->effort;
00038 
00039 
00040 
00041   // get chain
00042   // ----------
00043   std::string root_name, tip_name;
00044   if (!n.getParam("root_name", root_name))
00045   {
00046     ROS_ERROR("No root name given in namespace: '%s')", n.getNamespace().c_str());
00047     return false;
00048   }
00049   if (!n.getParam("tip_name", tip_name))
00050   {
00051     ROS_ERROR("No tip name given in namespace: '%s')", n.getNamespace().c_str());
00052     return false;
00053   }
00054   if (!chain_.init(robot, root_name, tip_name))
00055     return false;
00056   chain_.toKDL(kdl_chain_);
00057 
00058   // advertise topic
00059   // ----------
00060   std::string topic_name;
00061   if (!n.getParam("topic_name", topic_name))
00062   {
00063     ROS_ERROR("No topic name given in namespace: '%s')", n.getNamespace().c_str());
00064     return false;
00065   }
00066   pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(n, topic_name, 100));
00067   pub_->msg_.name.resize(1);
00068   pub_->msg_.name[0] = "";
00069   pub_->msg_.effort.resize(1);
00070   pub_->msg_.effort[0] = 0.0;
00071 
00072 
00073   // advertise service 
00074   // ----------
00075   std::string service_name;
00076   if (!n.getParam("service_name", service_name))
00077   {
00078     ROS_ERROR("No service name given in namespace: '%s')", n.getNamespace().c_str());
00079     return false;
00080   }
00081   srv_ = n.advertiseService(service_name, &MyControllerClass::serviceCallback, this);
00082   return true;
00083 }
00084 
00086 void MyControllerClass::starting()
00087 {
00088   counter_ = 0;
00089   time_of_last_cycle_ = robot_->getTime();
00090 }
00091 
00092 
00094 void MyControllerClass::update()
00095 {
00096   counter_++;
00097   if (counter_ > 10 && pub_->trylock()){
00098     counter_ = 0;
00099     pub_->msg_.effort[0] = fabs(joint_state_->commanded_effort_) - max_effort_; // this should never be greater than zero
00100     pub_->unlockAndPublish();
00101   }
00102 
00103   ros::Time time_of_last_cycle_ = robot_->getTime();
00104 
00105   double tmp;
00106   tmp = joint_state_->position_;
00107   tmp = joint_state_->velocity_;
00108   tmp = joint_state_->measured_effort_;
00109   if (joint_state_->commanded_effort_ > 0)
00110     joint_state_->commanded_effort_ = -10000.0;  // above max effort
00111   else
00112     joint_state_->commanded_effort_ = 10000.0;  // above max effort
00113 }
00114 
00115 
00117 void MyControllerClass::stopping()
00118 {}
00119 
00120 
00122 bool MyControllerClass::serviceCallback(pr2_mechanism_msgs::LoadController::Request& req,
00123                                         pr2_mechanism_msgs::LoadController::Response& resp)
00124 {
00125   pub_->msg_.name[0] = req.name;
00126   return true;
00127 }
00128 
00130 PLUGINLIB_DECLARE_CLASS(pr2_controller_manager,
00131                          TestController,
00132                          my_controller_ns::MyControllerClass,
00133                          pr2_controller_interface::Controller)


pr2_controller_manager
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Thu Dec 12 2013 12:04:08