$search
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)