caster_calibration_controller.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Stuart Glaser
32  */
33 
36 
38 
39 namespace controller {
40 
41 CasterCalibrationController::CasterCalibrationController()
42 : robot_(NULL),
43  joint_(NULL), wheel_l_joint_(NULL), wheel_r_joint_(NULL), last_publish_time_(0)
44 {
45 }
46 
48 {
49  for (size_t i = 0; i < fake_as.size(); ++i)
50  delete fake_as[i];
51  for (size_t i = 0; i < fake_js.size(); ++i)
52  delete fake_js[i];
53 }
54 
56 {
57  node_ = n;
58  robot_ = robot;
59 
60  // Joints
61 
62  std::string joint_name;
63  if (!node_.getParam("joints/caster", joint_name))
64  {
65  ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
66  return false;
67  }
68  if (!(joint_ = robot->getJointState(joint_name)))
69  {
70  ROS_ERROR("Could not find joint %s (namespace: %s)",
71  joint_name.c_str(), node_.getNamespace().c_str());
72  return false;
73  }
74  if (!joint_->joint_->calibration)
75  {
76  ROS_ERROR("No calibration reference position specified for joint %s (namespace: %s)",
77  joint_name.c_str(), node_.getNamespace().c_str());
78  return false;
79  }
80  if (!node_.getParam("velocity", search_velocity_))
81  {
82  ROS_ERROR("No velocity given (namespace: %s)", node_.getNamespace().c_str());
83  return false;
84  }
85 
86  // check if calibration fields are supported by this controller
87  if (!joint_->joint_->calibration->falling && !joint_->joint_->calibration->rising){
88  ROS_ERROR("No rising or falling edge is specified for calibration of joint %s. Note that the reference_position is not used any more", joint_name.c_str());
89  return false;
90  }
91  if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising && joint_->joint_->type != urdf::Joint::CONTINUOUS){
92  ROS_ERROR("Both rising and falling edge are specified for non-continuous joint %s. This is not supported.", joint_name.c_str());
93  return false;
94  }
95  if (search_velocity_ < 0){
96  search_velocity_ *= -1;
97  ROS_WARN("Negative search velocity is not supported for joint %s. Making the search velocity positve.", joint_name.c_str());
98  }
99 
100  // finds search velocity based on rising or falling edge
101  if (joint_->joint_->calibration->falling && joint_->joint_->calibration->rising){
102  joint_->reference_position_ = *(joint_->joint_->calibration->rising);
103  ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
104  }
105  else if (joint_->joint_->calibration->falling){
106  joint_->reference_position_ = *(joint_->joint_->calibration->falling);
107  search_velocity_ *= -1.0;
108  ROS_DEBUG("Using negative search velocity for joint %s", joint_name.c_str());
109  }
110  else if (joint_->joint_->calibration->rising){
111  joint_->reference_position_ = *(joint_->joint_->calibration->rising);
112  ROS_DEBUG("Using positive search velocity for joint %s", joint_name.c_str());
113  }
114 
115  if (!node_.getParam("joints/wheel_l", joint_name))
116  {
117  ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
118  return false;
119  }
120  if (!(wheel_l_joint_ = robot->getJointState(joint_name)))
121  {
122  ROS_ERROR("Could not find joint %s (namespace: %s)",
123  joint_name.c_str(), node_.getNamespace().c_str());
124  return false;
125  }
126 
127  if (!node_.getParam("joints/wheel_r", joint_name))
128  {
129  ROS_ERROR("No joint given (namespace: %s)", node_.getNamespace().c_str());
130  return false;
131  }
132  if (!(wheel_r_joint_ = robot->getJointState(joint_name)))
133  {
134  ROS_ERROR("Could not find joint %s (namespace: %s)",
135  joint_name.c_str(), node_.getNamespace().c_str());
136  return false;
137  }
138 
139  // Actuator
140 
141  std::string actuator_name;
142  if (!node_.getParam("actuator", actuator_name))
143  {
144  ROS_ERROR("No actuator given (namespace: %s)", node_.getNamespace().c_str());
145  return false;
146  }
147  if (!(actuator_ = robot->model_->getActuator(actuator_name)))
148  {
149  ROS_ERROR("Could not find actuator %s (namespace: %s)",
150  actuator_name.c_str(), node_.getNamespace().c_str());
151  return false;
152  }
153 
154  bool force_calibration = false;
155  node_.getParam("force_calibration", force_calibration);
156 
158  joint_->calibrated_ = false;
159  wheel_l_joint_->calibrated_ = false;
160  wheel_r_joint_->calibrated_ = false;
161  if (actuator_->state_.zero_offset_ != 0){
162  if (force_calibration)
163  {
164  ROS_INFO("Joint %s will be recalibrated, but was already calibrated at offset %f",
165  joint_name.c_str(), actuator_->state_.zero_offset_);
166  }
167  else
168  {
169  ROS_INFO("Joint %s is already calibrated at offset %f", joint_name.c_str(), actuator_->state_.zero_offset_);
170  state_ = CALIBRATED;
171  joint_->calibrated_ = true;
172  wheel_l_joint_->calibrated_ = true;
173  wheel_r_joint_->calibrated_ = true;
174  }
175  }
176  else{
177  ROS_INFO("Joint %s is not yet calibrated", joint_name.c_str());
178  }
179 
180 
181  // Transmission
182  std::string transmission_name;
183  if (!node_.getParam("transmission", transmission_name))
184  {
185  ROS_ERROR("No transmission given (namespace: %s)", node_.getNamespace().c_str());
186  return false;
187  }
188  if (!(transmission_ = robot->model_->getTransmission(transmission_name)))
189  {
190  ROS_ERROR("Could not find transmission %s (namespace: %s)",
191  transmission_name.c_str(), node_.getNamespace().c_str());
192  return false;
193  }
194 
195  if (!node_.getParam("velocity", search_velocity_))
196  {
197  ROS_ERROR("Velocity value was not specified (namespace: %s)", node_.getNamespace().c_str());
198  return false;
199  }
200 
203 
204  if (!cc_.init(robot_, node_))
205  return false;
206 
207  // advertise service to check calibration
209 
210  // "Calibrated" topic
212 
213  return true;
214 }
215 
217 {
220  joint_->calibrated_ = false;
221  wheel_l_joint_->calibrated_ = false;
222  wheel_r_joint_->calibrated_ = false;
223 }
224 
225 
226 bool CasterCalibrationController::isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request& req,
227  pr2_controllers_msgs::QueryCalibrationState::Response& resp)
228 {
229  ROS_DEBUG("Is calibrated service %d", state_ == CALIBRATED);
230  resp.is_calibrated = (state_ == CALIBRATED);
231  return true;
232 }
233 
234 
235 
237 {
238  assert(joint_);
239  assert(actuator_);
240  ros::Time time = robot_->getTime();
241 
242  switch(state_)
243  {
244  case INITIALIZED:
245  cc_.steer_velocity_ = 0.0;
246  cc_.drive_velocity_ = 0.0;
247  state_ = BEGINNING;
248  break;
249  case BEGINNING:
250  beginning_ = time;
254  state_ = MOVING;
255  break;
256  case MOVING: {
257  bool switch_state_ = actuator_->state_.calibration_reading_ & 1;
258  if (switch_state_ != original_switch_state_)
259  {
260  // detect when we hit the wrong transition because someone pushed the caster during calibration
261  if ((cc_.steer_velocity_ > 0.0 && (joint_->position_ - original_position_) < 0) ||
263  {
264  state_ = BEGINNING;
265  ROS_ERROR("Caster hit the falling edge instead of the rising edge. Calibrating again...");
266  ros::Duration(1.0).sleep(); // give caster some time to move away from transition
267  break;
268  }
269 
270  // Where was the joint when the optical switch triggered?
271  if (switch_state_ == true)
273  else
275 
276  joint_->calibrated_ = true;
277  wheel_l_joint_->calibrated_ = true;
278  wheel_r_joint_->calibrated_ = true;
279 
280  state_ = CALIBRATED;
281  cc_.steer_velocity_ = 0.0;
282  }
283  else
284  {
285  // The caster is not strong enough to consistently move. The
286  // rest of this block contains the hacks to ensure that
287  // calibration always completes.
288  if (time > beginning_ + ros::Duration(6.0))
289  {
290  if ((unstick_iter_ / 1000) % 2 == 0)
292  else
293  cc_.steer_velocity_ = 0.0;
294  ++unstick_iter_;
295  }
296  else
297  unstick_iter_ = 0;
298  }
299  break;
300  }
301  case CALIBRATED:
302  cc_.steer_velocity_ = 0.0;
303  if (pub_calibrated_) {
305  if (pub_calibrated_->trylock()) {
307  pub_calibrated_->unlockAndPublish();
308  }
309  }
310  }
311  break;
312  }
313 
314  if (state_ != CALIBRATED)
315  cc_.update();
316 }
317 
318 } // namespace
pr2_mechanism_model::JointState * wheel_r_joint_
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
boost::shared_ptr< pr2_mechanism_model::Transmission > getTransmission(const std::string &name) const
pr2_mechanism_model::JointState * wheel_l_joint_
bool sleep() const
std::vector< pr2_mechanism_model::JointState * > fake_js
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< const urdf::Joint > joint_
#define ROS_WARN(...)
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Empty > > pub_calibrated_
std::vector< pr2_hardware_interface::Actuator * > fake_as
virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
boost::shared_ptr< pr2_mechanism_model::Transmission > transmission_
#define ROS_INFO(...)
bool isCalibrated(pr2_controllers_msgs::QueryCalibrationState::Request &req, pr2_controllers_msgs::QueryCalibrationState::Response &resp)
const std::string & getNamespace() const
JointState * getJointState(const std::string &name)
bool getParam(const std::string &key, std::string &s) const
bool init(pr2_mechanism_model::RobotState *robot_state, const std::string &caster_joint, const std::string &wheel_l_joint, const std::string &wheel_r_joint, const control_toolbox::Pid &caster_pid, const control_toolbox::Pid &wheel_pid)
pr2_hardware_interface::Actuator * getActuator(const std::string &name) const
#define ROS_ERROR(...)
pr2_hardware_interface::Actuator * actuator_
#define ROS_DEBUG(...)


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