gripper_calibration_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
36 #include "ros/time.h"
38 
39 using namespace std;
40 using namespace controller;
41 
43 
44 namespace controller
45 {
46 
47 GripperCalibrationController::GripperCalibrationController()
48  : last_publish_time_(0), joint_(NULL)
49 {
50 }
51 
53 {
54 }
55 
57  ros::NodeHandle &n)
58 {
59  assert(robot);
60  robot_ = robot;
61  node_ = n;
62 
63  node_.param("stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.0001);
64 
65  XmlRpc::XmlRpcValue other_joint_names;
66  if (node_.getParam("other_joints", other_joint_names))
67  {
68  if (other_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
69  {
70  ROS_ERROR("\"other_joints\" was not an array (namespace: %s)", node_.getNamespace().c_str());
71  return false;
72  }
73  else
74  {
75  for (int i = 0; i < other_joint_names.size(); ++i)
76  {
78  std::string name = (std::string)other_joint_names[i];
79  if ((j = robot->getJointState(name))){
80  other_joints_.push_back(j);
81  }
82  else {
83  ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
84  name.c_str(), node_.getNamespace().c_str());
85  return false;
86  }
87  }
88  }
89  }
90 
91  if (!node_.getParam("velocity", search_velocity_))
92  {
93  ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
94  return false;
95  }
96 
97  std::string joint_name;
98  if (!node_.getParam("joint", joint_name))
99  {
100  ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
101  return false;
102  }
103  if (!(joint_ = robot->getJointState(joint_name)))
104  {
105  ROS_ERROR("Could not find joint \"%s\" (namespace: %s)",
106  joint_name.c_str(), node_.getNamespace().c_str());
107  return false;
108  }
109 
110  std::string actuator_name;
111  if (!node_.getParam("actuator", actuator_name))
112  {
113  ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
114  return false;
115  }
116  if (!(actuator_ = robot->model_->getActuator(actuator_name)))
117  {
118  ROS_ERROR("Could not find actuator \"%s\" (namespace: %s)",
119  actuator_name.c_str(), node_.getNamespace().c_str());
120  return false;
121  }
122 
123  bool force_calibration = false;
124  node_.getParam("force_calibration", force_calibration);
125 
127  joint_->calibrated_ = false;
128  if (actuator_->state_.zero_offset_ != 0){
129  if (force_calibration)
130  {
131  ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f",
132  joint_name.c_str(), actuator_->state_.zero_offset_);
133  }
134  else
135  {
136  ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
137  joint_->calibrated_ = true;
138  for (size_t i = 0; i < other_joints_.size(); ++i)
139  other_joints_[i]->calibrated_ = true;
140  state_ = CALIBRATED;
141  }
142  }
143  else{
144  ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
145  }
146 
147  if (!vc_.init(robot, node_))
148  return false;
149 
150  // advertise service to check calibration
152 
153  // "Calibrated" topic
155 
156  return true;
157 }
158 
159 
161 {
164  joint_->calibrated_ = false;
165 }
166 
167 
168 bool GripperCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
169  pr2_controllers_msgs::QueryCalibrationState::Response& resp)
170 {
171  resp.is_calibrated = (state_ == CALIBRATED);
172  return true;
173 }
174 
175 
177 {
178  assert(joint_);
179  assert(actuator_);
180 
181  switch (state_)
182  {
183  case INITIALIZED:
184  state_ = BEGINNING;
185  return;
186  case BEGINNING:
187  count_ = 0;
188  stop_count_ = 0;
189  joint_->calibrated_ = false;
191 
193 
194  state_ = STARTING;
195  break;
196  case STARTING:
197  // Makes sure we start moving for a bit before checking if we've stopped.
198  if (++count_ > 100)
199  {
200  count_ = 0;
201  stop_count_ = 0;
202  state_ = CLOSING;
203  }
204  break;
205  case CLOSING:
206  // Makes sure the gripper is stopped for a while before cal
207  if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
208  stop_count_++;
209  else
210  stop_count_ = 0;
211 
212  if (stop_count_ > 100)
213  {
214  state_ = BACK_OFF;
215  stop_count_ = 0;
217  }
218  break;
219  case BACK_OFF: // Back off so we can reset from a known good position
220  if (++stop_count_ > 1000)
221  {
223  count_ = 0;
224  stop_count_ = 0;
226  }
227 
228  break;
229  case CLOSING_SLOWLY: // Close slowly to avoid windup
230  // Makes sure the gripper is stopped for a while before cal
231  if (fabs(joint_->velocity_) < this->stopped_velocity_tolerance_)
232  stop_count_++;
233  else
234  stop_count_ = 0;
235 
236  if (stop_count_ > 500)
237  {
238  state_ = CALIBRATED;
240  joint_->calibrated_ = true;
241  for (size_t i = 0; i < other_joints_.size(); ++i)
242  other_joints_[i]->calibrated_ = true;
243  vc_.setCommand(0);
244  }
245 
246  break;
247  case CALIBRATED:
248  if (pub_calibrated_) {
250  if (pub_calibrated_->trylock()) {
252  pub_calibrated_->unlockAndPublish();
253  }
254  }
255  }
256  break;
257  }
258  if (state_ != CALIBRATED)
259  vc_.update();
260 }
261 }
262 
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
int size() const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Type const & getType() const
virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Empty > > pub_calibrated_
const std::string & getNamespace() const
bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request &req, pr2_controllers_msgs::QueryCalibrationState::Response &resp)
JointState * getJointState(const std::string &name)
bool getParam(const std::string &key, std::string &s) const
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
#define ROS_ERROR(...)
std::vector< pr2_mechanism_model::JointState * > other_joints_
bool init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name, const control_toolbox::Pid &pid)


pr2_calibration_controllers
Author(s): Stuart Glaser
autogenerated on Mon Jun 10 2019 14:26:37