Go to the documentation of this file.00001
00027 #include "sr_mechanism_controllers/srh_fake_joint_calibration_controller.h"
00028 #include "ros/time.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include <boost/algorithm/string.hpp>
00031 #include <string>
00032 #include <std_srvs/Empty.h>
00033
00034 PLUGINLIB_EXPORT_CLASS( controller::SrhFakeJointCalibrationController, controller_interface::ControllerBase)
00035
00036 using namespace std;
00037
00038 namespace controller {
00039
00040 SrhFakeJointCalibrationController::SrhFakeJointCalibrationController()
00041 : robot_(NULL),
00042 last_publish_time_(0),
00043 calibration_state_(IS_INITIALIZED),
00044 actuator_(NULL),
00045 joint_(NULL)
00046 {
00047 }
00048
00049 bool SrhFakeJointCalibrationController::init(ros_ethercat_model::RobotState *robot, ros::NodeHandle &n)
00050 {
00051 ROS_ASSERT(robot);
00052 robot_ = robot;
00053 node_ = n;
00054
00055
00056 if (node_.getParam("robot_id", robot_id_)
00057 && (!robot_id_.empty()))
00058 {
00059 joint_prefix_ = robot_id_ + "_";
00060 ns_ = robot_id_ + "/";
00061 }
00062
00063
00064
00065 if (!node_.getParam("joint", joint_name_))
00066 {
00067 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00068 return false;
00069 }
00070 if (!(joint_ = robot->getJointState(joint_prefix_ + joint_name_)))
00071 {
00072 ROS_ERROR("Could not find joint %s (namespace: %s)",
00073 (joint_prefix_ + joint_name_).c_str(), node_.getNamespace().c_str());
00074 return false;
00075 }
00076
00077
00078 if (!node_.getParam("actuator", actuator_name_))
00079 {
00080 ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00081 return false;
00082 }
00083 if (!(actuator_ = robot->getActuator(joint_prefix_ + actuator_name_)))
00084 {
00085 ROS_ERROR("Could not find actuator %s (namespace: %s)",
00086 (joint_prefix_ + actuator_name_).c_str(), node_.getNamespace().c_str());
00087 return false;
00088 }
00089
00090
00091 string transmission_name;
00092 if (!node_.getParam("transmission", transmission_name))
00093 {
00094 ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
00095 return false;
00096 }
00097
00098
00099 pub_calibrated_.reset(
00100 new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00101
00102 return true;
00103 }
00104
00105
00106 void SrhFakeJointCalibrationController::update(const ros::Time& time, const ros::Duration& period)
00107 {
00108 ROS_ASSERT(joint_);
00109 ROS_ASSERT(actuator_);
00110
00111 switch(calibration_state_)
00112 {
00113 case IS_INITIALIZED:
00114 calibration_state_ = BEGINNING;
00115 break;
00116 case BEGINNING:
00117 initialize_pids();
00118 joint_->calibrated_ = true;
00119 calibration_state_ = CALIBRATED;
00120
00121 last_publish_time_ = robot_->getTime();
00122 break;
00123 case CALIBRATED:
00124 if (pub_calibrated_)
00125 {
00126 if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime())
00127 {
00128 ROS_ASSERT(pub_calibrated_);
00129 if (pub_calibrated_->trylock())
00130 {
00131 last_publish_time_ = robot_->getTime();
00132 pub_calibrated_->unlockAndPublish();
00133 }
00134 }
00135 }
00136 break;
00137 }
00138 }
00139
00140 void SrhFakeJointCalibrationController::initialize_pids()
00141 {
00143
00144 std::string lowlevel_actuator_name= actuator_name_.substr(actuator_name_.size()-4,4);
00145 string service_name = "realtime_loop/" + ns_ + "reset_motor_" + boost::to_upper_copy(lowlevel_actuator_name);
00146 if( ros::service::waitForService (service_name, ros::Duration(2.0)) )
00147 {
00148 std_srvs::Empty srv;
00149 if (ros::service::call(service_name, srv))
00150 {
00151 return;
00152 }
00153 else
00154 {
00155 ROS_WARN("Reset failed: %s", service_name.c_str());
00156 }
00157 }
00158 }
00159
00160 }
00161
00162
00163
00164
00165
00166