00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "pr2_calibration_controllers/gripper_calibration_controller.h"
00036 #include "ros/time.h"
00037 #include "pluginlib/class_list_macros.h"
00038
00039 using namespace std;
00040 using namespace controller;
00041
00042 PLUGINLIB_DECLARE_CLASS(pr2_calibration_controllers, GripperCalibrationController,
00043 controller::GripperCalibrationController, pr2_controller_interface::Controller)
00044
00045 namespace controller
00046 {
00047
00048 GripperCalibrationController::GripperCalibrationController()
00049 : last_publish_time_(0), joint_(NULL)
00050 {
00051 }
00052
00053 GripperCalibrationController::~GripperCalibrationController()
00054 {
00055 }
00056
00057 bool GripperCalibrationController::init(pr2_mechanism_model::RobotState *robot,
00058 ros::NodeHandle &n)
00059 {
00060 assert(robot);
00061 robot_ = robot;
00062 node_ = n;
00063
00064 node_.param("stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.0001);
00065
00066 XmlRpc::XmlRpcValue other_joint_names;
00067 if (node_.getParam("other_joints", other_joint_names))
00068 {
00069 if (other_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00070 {
00071 ROS_ERROR("\"other_joints\" was not an array (namespace: %s)", node_.getNamespace().c_str());
00072 return false;
00073 }
00074 else
00075 {
00076 for (int i = 0; i < other_joint_names.size(); ++i)
00077 {
00078 pr2_mechanism_model::JointState *j;
00079 std::string name = (std::string)other_joint_names[i];
00080 if ((j = robot->getJointState(name))){
00081 other_joints_.push_back(j);
00082 }
00083 else {
00084 ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00085 name.c_str(), node_.getNamespace().c_str());
00086 return false;
00087 }
00088 }
00089 }
00090 }
00091
00092 if (!node_.getParam("velocity", search_velocity_))
00093 {
00094 ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
00095 return false;
00096 }
00097
00098 std::string joint_name;
00099 if (!node_.getParam("joint", joint_name))
00100 {
00101 ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
00102 return false;
00103 }
00104 if (!(joint_ = robot->getJointState(joint_name)))
00105 {
00106 ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
00107 joint_name.c_str(), node_.getNamespace().c_str());
00108 return false;
00109 }
00110
00111 std::string actuator_name;
00112 if (!node_.getParam("actuator", actuator_name))
00113 {
00114 ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
00115 return false;
00116 }
00117 if (!(actuator_ = robot->model_->getActuator(actuator_name)))
00118 {
00119 ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
00120 actuator_name.c_str(), node_.getNamespace().c_str());
00121 return false;
00122 }
00123 if (actuator_->state_.zero_offset_ != 0){
00124 ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
00125 joint_->calibrated_ = true;
00126 for (size_t i = 0; i < other_joints_.size(); ++i)
00127 other_joints_[i]->calibrated_ = true;
00128 state_ = CALIBRATED;
00129 }
00130 else{
00131 ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
00132 state_ = INITIALIZED;
00133 joint_->calibrated_ = false;
00134 }
00135
00136
00137
00138 if (!vc_.init(robot, node_))
00139 return false;
00140
00141
00142 is_calibrated_srv_ = node_.advertiseService("is_calibrated", &GripperCalibrationController::isCalibrated, this);
00143
00144
00145 pub_calibrated_.reset(new realtime_tools::RealtimePublisher<std_msgs::Empty>(node_, "calibrated", 1));
00146
00147 return true;
00148 }
00149
00150
00151 void GripperCalibrationController::starting()
00152 {
00153 state_ = INITIALIZED;
00154 actuator_->state_.zero_offset_ = 0.0;
00155 joint_->calibrated_ = false;
00156 }
00157
00158
00159 bool GripperCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
00160 pr2_controllers_msgs::QueryCalibrationState::Response& resp)
00161 {
00162 resp.is_calibrated = (state_ == CALIBRATED);
00163 return true;
00164 }
00165
00166
00167 void GripperCalibrationController::update()
00168 {
00169 assert(joint_);
00170 assert(actuator_);
00171
00172 switch (state_)
00173 {
00174 case INITIALIZED:
00175 state_ = BEGINNING;
00176 return;
00177 case BEGINNING:
00178 count_ = 0;
00179 stop_count_ = 0;
00180 joint_->calibrated_ = false;
00181 actuator_->state_.zero_offset_ = 0.0;
00182
00183 vc_.setCommand(search_velocity_);
00184
00185 state_ = STARTING;
00186 break;
00187 case STARTING:
00188
00189 if (++count_ > 100)
00190 {
00191 count_ = 0;
00192 stop_count_ = 0;
00193 state_ = CLOSING;
00194 }
00195 break;
00196 case CLOSING:
00197
00198 if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
00199 stop_count_++;
00200 else
00201 stop_count_ = 0;
00202
00203 if (stop_count_ > 100)
00204 {
00205 state_ = BACK_OFF;
00206 stop_count_ = 0;
00207 vc_.setCommand(-1 * search_velocity_);
00208 }
00209 break;
00210 case BACK_OFF:
00211 if (++stop_count_ > 1000)
00212 {
00213 state_ = CLOSING_SLOWLY;
00214 count_ = 0;
00215 stop_count_ = 0;
00216 vc_.setCommand(1.0 * search_velocity_);
00217 }
00218
00219 break;
00220 case CLOSING_SLOWLY:
00221
00222 if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
00223 stop_count_++;
00224 else
00225 stop_count_ = 0;
00226
00227 if (stop_count_ > 500)
00228 {
00229 state_ = CALIBRATED;
00230 actuator_->state_.zero_offset_ = actuator_->state_.position_;
00231 joint_->calibrated_ = true;
00232 for (size_t i = 0; i < other_joints_.size(); ++i)
00233 other_joints_[i]->calibrated_ = true;
00234 vc_.setCommand(0);
00235 }
00236
00237 break;
00238 case CALIBRATED:
00239 if (pub_calibrated_) {
00240 if (last_publish_time_ + ros::Duration(0.5) < robot_->getTime()) {
00241 if (pub_calibrated_->trylock()) {
00242 last_publish_time_ = robot_->getTime();
00243 pub_calibrated_->unlockAndPublish();
00244 }
00245 }
00246 }
00247 break;
00248 }
00249 if (state_ != CALIBRATED)
00250 vc_.update();
00251 }
00252 }
00253
