pr2_base_controller2.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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 /*
35  * Author: Sachin Chitta and Matthew Piccoli
36  */
37 
40 
42 
43 namespace controller {
44 
45  const static double EPS = 1e-5;
46 
47 Pr2BaseController2::Pr2BaseController2()
48 {
49  //init variables
50  cmd_vel_.linear.x = 0;
51  cmd_vel_.linear.y = 0;
52  cmd_vel_.angular.z = 0;
53 
54  desired_vel_.linear.x = 0;
55  desired_vel_.linear.y = 0;
56  desired_vel_.angular.z = 0;
57 
58  cmd_vel_t_.linear.x = 0;
59  cmd_vel_t_.linear.y = 0;
60  cmd_vel_t_.angular.z = 0;
61 
62  new_cmd_available_ = false;
63  last_publish_time_ = ros::Time(0.0);
64 
65  pthread_mutex_init(&pr2_base_controller_lock_, NULL);
66 }
67 
68 Pr2BaseController2::~Pr2BaseController2()
69 {
70  cmd_sub_.shutdown();
71  cmd_sub_deprecated_.shutdown();
72 }
73 
74 bool Pr2BaseController2::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
75 {
76  if(!base_kinematics_.init(robot,n))
77  return false;
78  node_ = n;
79  state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseControllerState2>(n, base_kinematics_.name_ + "/state", 1));
80 
81  int num_joints = base_kinematics_.num_wheels_ + base_kinematics_.num_casters_;
82  state_publisher_->msg_.joint_names.resize(num_joints);
83  state_publisher_->msg_.joint_velocity_commanded.resize(num_joints);
84  state_publisher_->msg_.joint_velocity_measured.resize(num_joints);
85  state_publisher_->msg_.joint_effort_measured.resize(num_joints);
86  state_publisher_->msg_.joint_command.resize(num_joints);
87  state_publisher_->msg_.joint_effort_commanded.resize(num_joints);
88  state_publisher_->msg_.joint_error.resize(num_joints);
89  state_publisher_->msg_.joint_effort_error.resize(num_joints);
90 
91  //Get params from param server
92  node_.param<double> ("max_translational_velocity", max_translational_velocity_,0.5);
93  node_.param<double> ("max_rotational_velocity", max_rotational_velocity_, 10.0); //0.5
94  node_.param<double> ("max_translational_acceleration/x", max_accel_.linear.x, .2);
95  node_.param<double> ("max_translational_acceleration/y", max_accel_.linear.y, .2);
96  node_.param<double> ("max_rotational_acceleration", max_accel_.angular.z, 10.0); //0.2
97 
98  node_.param<double> ("kp_caster_steer", kp_caster_steer_, 80.0);
99  node_.param<double> ("timeout", timeout_, 1.0);
100  node_.param<double> ("state_publish_rate", state_publish_rate_,2.0);
101  if(state_publish_rate_ <= 0.0)
102  {
103  publish_state_ = false;
104  state_publish_time_ = 0.0;
105  }
106  else
107  {
108  publish_state_ = true;
109  state_publish_time_ = 1.0/state_publish_rate_;
110  }
111 
112  // cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController2::commandCallback, this);
113  cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController2::commandCallback, this);
114 
115  //casters
116  caster_controller_.resize(base_kinematics_.num_casters_);
117  caster_position_pid_.resize(base_kinematics_.num_casters_);
118  for(int i = 0; i < base_kinematics_.num_casters_; i++)
119  {
120  control_toolbox::Pid p_i_d;
121  state_publisher_->msg_.joint_names[i] = base_kinematics_.caster_[i].joint_name_;
122  if(!p_i_d.init(ros::NodeHandle(node_, base_kinematics_.caster_[i].joint_name_+"/velocity_controller")))
123  {
124  ROS_ERROR("Could not initialize pid for %s",base_kinematics_.caster_[i].joint_name_.c_str());
125  return false;
126  }
127 
128  if(!caster_position_pid_[i].init(ros::NodeHandle(node_, base_kinematics_.caster_[i].joint_name_+"/position_controller")))
129  {
130  ROS_ERROR("Could not initialize position pid controller for %s",base_kinematics_.caster_[i].joint_name_.c_str());
131  return false;
132  }
133  caster_controller_[i].reset(new JointVelocityController());
134  if(!caster_controller_[i]->init(base_kinematics_.robot_state_, base_kinematics_.caster_[i].joint_name_, p_i_d))
135  {
136  ROS_ERROR("Could not initialize pid for %s",base_kinematics_.caster_[i].joint_name_.c_str());
137  return false;
138  }
139  if (!caster_controller_[i]->joint_state_->calibrated_)
140  {
141  ROS_ERROR("Caster joint \"%s\" not calibrated (namespace: %s)",
142  base_kinematics_.caster_[i].joint_name_.c_str(), node_.getNamespace().c_str());
143  return false;
144  }
145  }
146  //wheels
147  wheel_pid_controllers_.resize(base_kinematics_.num_wheels_);
148  // wheel_controller_.resize(base_kinematics_.num_wheels_);
149  for(int j = 0; j < base_kinematics_.num_wheels_; j++)
150  {
151  control_toolbox::Pid p_i_d;
152  state_publisher_->msg_.joint_names[j + base_kinematics_.num_casters_] = base_kinematics_.wheel_[j].joint_name_;
153  if(!wheel_pid_controllers_[j].init(ros::NodeHandle(node_,base_kinematics_.wheel_[j].joint_name_)))
154  {
155  ROS_ERROR("Could not initialize pid for %s",base_kinematics_.wheel_[j].joint_name_.c_str());
156  return false;
157  }
158  /* wheel_controller_[j].reset(new JointVelocityController());
159  if(!wheel_controller_[j]->init(base_kinematics_.robot_state_, base_kinematics_.wheel_[j].joint_name_, p_i_d))
160  {
161  ROS_ERROR("Could not initialize joint velocity controller for %s",base_kinematics_.wheel_[j].joint_name_.c_str());
162  return false;
163  }*/
164  }
165  for(int i = 0; i < base_kinematics_.num_casters_; ++i)
166  {
167  if(!base_kinematics_.caster_[i].joint_->calibrated_)
168  {
169  ROS_ERROR("The Base controller could not start because the casters were not calibrated. Relaunch the base controller after you see the caster calibration finish.");
170  return false; // Casters are not calibrated
171  }
172  }
173 
174  if (!((filters::MultiChannelFilterBase<double>&)caster_vel_filter_).configure(base_kinematics_.num_casters_, std::string("caster_velocity_filter"), node_)){
175  ROS_ERROR("BaseController: could not configure velocity filters for casters");
176  return false;
177  }
178  if (!((filters::MultiChannelFilterBase<double>&)wheel_vel_filter_).configure(base_kinematics_.num_wheels_, std::string("wheel_velocity_filter"), node_)){
179  ROS_ERROR("BaseController: could not configure velocity filters for wheels");
180  return false;
181  }
182  filtered_velocity_.resize(base_kinematics_.num_casters_);
183  filtered_wheel_velocity_.resize(base_kinematics_.num_wheels_);
184  return true;
185 }
186 
187 // Set the base velocity command
188 void Pr2BaseController2::setCommand(const geometry_msgs::Twist &cmd_vel)
189 {
190  double vel_mag = sqrt(cmd_vel.linear.x * cmd_vel.linear.x + cmd_vel.linear.y * cmd_vel.linear.y);
191  double clamped_vel_mag = filters::clamp(vel_mag,-max_translational_velocity_, max_translational_velocity_);
192  if(vel_mag > EPS)
193  {
194  cmd_vel_t_.linear.x = cmd_vel.linear.x * clamped_vel_mag / vel_mag;
195  cmd_vel_t_.linear.y = cmd_vel.linear.y * clamped_vel_mag / vel_mag;
196  }
197  else
198  {
199  cmd_vel_t_.linear.x = 0.0;
200  cmd_vel_t_.linear.y = 0.0;
201  }
202  cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_rotational_velocity_, max_rotational_velocity_);
203  cmd_received_timestamp_ = base_kinematics_.robot_state_->getTime();
204 
205  ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
206  ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
207  ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag);
208  ROS_DEBUG("BaseController:: vel: %f", vel_mag);
209 
210  for(int i=0; i < (int) base_kinematics_.num_wheels_; i++)
211  {
212  ROS_DEBUG("BaseController:: wheel speed cmd:: %d %f",i,(base_kinematics_.wheel_[i].direction_multiplier_*base_kinematics_.wheel_[i].wheel_speed_cmd_));
213  }
214  for(int i=0; i < (int) base_kinematics_.num_casters_; i++)
215  {
216  ROS_DEBUG("BaseController:: caster speed cmd:: %d %f",i,(base_kinematics_.caster_[i].steer_velocity_desired_));
217  }
218  new_cmd_available_ = true;
219 }
220 
221 geometry_msgs::Twist Pr2BaseController2::interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT)
222 {
223  geometry_msgs::Twist result;
224  geometry_msgs::Twist alpha;
225  double delta(0), max_delta(0);
226 
227  delta = end.linear.x - start.linear.x;
228  max_delta = max_rate.linear.x * dT;
229  if(fabs(delta) <= max_delta || max_delta < EPS)
230  alpha.linear.x = 1;
231  else
232  alpha.linear.x = max_delta / fabs(delta);
233 
234  delta = end.linear.y - start.linear.y;
235  max_delta = max_rate.linear.y * dT;
236  if(fabs(delta) <= max_delta || max_delta < EPS)
237  alpha.linear.y = 1;
238  else
239  alpha.linear.y = max_delta / fabs(delta);
240 
241  delta = end.angular.z - start.angular.z;
242  max_delta = max_rate.angular.z * dT;
243  if(fabs(delta) <= max_delta || max_delta < EPS)
244  alpha.angular.z = 1;
245  else
246  alpha.angular.z = max_delta / fabs(delta);
247 
248  double alpha_min = alpha.linear.x;
249  if(alpha.linear.y < alpha_min)
250  alpha_min = alpha.linear.y;
251  if(alpha.angular.z < alpha_min)
252  alpha_min = alpha.angular.z;
253 
254  result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x);
255  result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y);
256  result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z);
257  return result;
258 }
259 
260 geometry_msgs::Twist Pr2BaseController2::getCommand()// Return the current velocity command
261 {
262  geometry_msgs::Twist cmd_vel;
263  pthread_mutex_lock(&pr2_base_controller_lock_);
264  cmd_vel.linear.x = cmd_vel_.linear.x;
265  cmd_vel.linear.y = cmd_vel_.linear.y;
266  cmd_vel.angular.z = cmd_vel_.angular.z;
267  pthread_mutex_unlock(&pr2_base_controller_lock_);
268  return cmd_vel;
269 }
270 
271 void Pr2BaseController2::starting()
272 {
273  last_time_ = base_kinematics_.robot_state_->getTime();
274  cmd_received_timestamp_ = base_kinematics_.robot_state_->getTime();
275  for(int i = 0; i < base_kinematics_.num_casters_; i++)
276  {
277  caster_controller_[i]->starting();
278  }
279  for(int j = 0; j < base_kinematics_.num_wheels_; j++)
280  {
281  // wheel_controller_[j]->starting();
282  }
283 }
284 
285 void Pr2BaseController2::update()
286 {
287  ros::Time current_time = base_kinematics_.robot_state_->getTime();
288  double dT = std::min<double>((current_time - last_time_).toSec(), base_kinematics_.MAX_DT_);
289 
290  if(new_cmd_available_)
291  {
292  if(pthread_mutex_trylock(&pr2_base_controller_lock_) == 0)
293  {
294  desired_vel_.linear.x = cmd_vel_t_.linear.x;
295  desired_vel_.linear.y = cmd_vel_t_.linear.y;
296  desired_vel_.angular.z = cmd_vel_t_.angular.z;
297  new_cmd_available_ = false;
298  pthread_mutex_unlock(&pr2_base_controller_lock_);
299  }
300  }
301 
302  if((current_time - cmd_received_timestamp_).toSec() > timeout_)
303  {
304  cmd_vel_.linear.x = 0;
305  cmd_vel_.linear.y = 0;
306  cmd_vel_.angular.z = 0;
307  }
308  else
309  cmd_vel_ = interpolateCommand(cmd_vel_, desired_vel_, max_accel_, dT);
310 
311  computeJointCommands(dT);
312 
313  setJointCommands();
314 
315  updateJointControllers();
316 
317  if(publish_state_)
318  publishState(current_time);
319 
320  last_time_ = current_time;
321 
322 }
323 
324 void Pr2BaseController2::publishState(const ros::Time &time)
325 {
326  if((time - last_publish_time_).toSec() < state_publish_time_)
327  {
328  return;
329  }
330 
331  if(state_publisher_->trylock())
332  {
333  state_publisher_->msg_.command.linear.x = cmd_vel_.linear.x;
334  state_publisher_->msg_.command.linear.y = cmd_vel_.linear.y;
335  state_publisher_->msg_.command.angular.z = cmd_vel_.angular.z;
336 
337  for(int i = 0; i < base_kinematics_.num_casters_; i++)
338  {
339  state_publisher_->msg_.joint_names[i] = base_kinematics_.caster_[i].joint_name_;
340  state_publisher_->msg_.joint_velocity_measured[i] = base_kinematics_.caster_[i].joint_->velocity_;
341  state_publisher_->msg_.joint_command[i]= base_kinematics_.caster_[i].steer_angle_desired_;
342  state_publisher_->msg_.joint_error[i] = base_kinematics_.caster_[i].joint_->position_ - base_kinematics_.caster_[i].steer_angle_desired_;
343 
344  state_publisher_->msg_.joint_effort_measured[i] = base_kinematics_.caster_[i].joint_->measured_effort_;
345  state_publisher_->msg_.joint_effort_commanded[i] = base_kinematics_.caster_[i].joint_->commanded_effort_;
346  state_publisher_->msg_.joint_effort_error[i] = base_kinematics_.caster_[i].joint_->measured_effort_ - base_kinematics_.caster_[i].joint_->commanded_effort_;
347  }
348  for(int i = 0; i < base_kinematics_.num_wheels_; i++)
349  {
350  state_publisher_->msg_.joint_names[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_name_;
351  state_publisher_->msg_.joint_velocity_commanded[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].wheel_speed_cmd_;
352  state_publisher_->msg_.joint_velocity_measured[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->velocity_;
353  state_publisher_->msg_.joint_command[i+base_kinematics_.num_casters_]= base_kinematics_.wheel_[i].joint_->velocity_-base_kinematics_.wheel_[i].wheel_speed_cmd_;
354  state_publisher_->msg_.joint_error[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].wheel_speed_cmd_;
355 
356  state_publisher_->msg_.joint_effort_measured[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->measured_effort_;
357  state_publisher_->msg_.joint_effort_commanded[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->commanded_effort_;
358  state_publisher_->msg_.joint_effort_error[i+base_kinematics_.num_casters_] = base_kinematics_.wheel_[i].joint_->measured_effort_ - base_kinematics_.wheel_[i].joint_->commanded_effort_;
359  }
360  state_publisher_->unlockAndPublish();
361  last_publish_time_ = time;
362  }
363 }
364 
365 void Pr2BaseController2::computeJointCommands(const double &dT)
366 {
367  base_kinematics_.computeWheelPositions();
368 
369  computeDesiredCasterSteer(dT);
370 
371  computeDesiredWheelSpeeds(dT);
372 }
373 
374 void Pr2BaseController2::setJointCommands()
375 {
376  setDesiredCasterSteer();
377 
378  setDesiredWheelSpeeds();
379 }
380 
381 void Pr2BaseController2::computeDesiredCasterSteer(const double &dT)
382 {
383  geometry_msgs::Twist result;
384 
385  double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
386  double error_steer(0.0), error_steer_m_pi(0.0);
387  double trans_vel = sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x + cmd_vel_.linear.y * cmd_vel_.linear.y);
388 
389  for(int i = 0; i < base_kinematics_.num_casters_; i++)
390  {
391  filtered_velocity_[i] = 0.0 - base_kinematics_.caster_[i].joint_->velocity_;
392  }
393  caster_vel_filter_.update(filtered_velocity_,filtered_velocity_);
394 
395  for(int i = 0; i < base_kinematics_.num_casters_; i++)
396  {
397  result = base_kinematics_.pointVel2D(base_kinematics_.caster_[i].offset_, cmd_vel_);
398  if(trans_vel < EPS && fabs(cmd_vel_.angular.z) < EPS)
399  {
400  steer_angle_desired = base_kinematics_.caster_[i].steer_angle_stored_;
401  }
402  else
403  {
404  steer_angle_desired = atan2(result.linear.y, result.linear.x);
405  base_kinematics_.caster_[i].steer_angle_stored_ = steer_angle_desired;
406  }
407  steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired + M_PI);
408  error_steer = angles::shortest_angular_distance(
409  base_kinematics_.caster_[i].joint_->position_,
410  steer_angle_desired);
411  error_steer_m_pi = angles::shortest_angular_distance(
412  base_kinematics_.caster_[i].joint_->position_,
413  steer_angle_desired_m_pi);
414 
415  if(fabs(error_steer_m_pi) < fabs(error_steer))
416  {
417  error_steer = error_steer_m_pi;
418  steer_angle_desired = steer_angle_desired_m_pi;
419  }
420  base_kinematics_.caster_[i].steer_angle_desired_ = steer_angle_desired;
421  double command = caster_position_pid_[i].computeCommand(error_steer,
422  filtered_velocity_[i], ros::Duration(dT));
423  base_kinematics_.caster_[i].joint_->commanded_effort_ = command;
424 
425  base_kinematics_.caster_[i].caster_position_error_ = error_steer;
426  }
427 }
428 
429 void Pr2BaseController2::setDesiredCasterSteer()
430 {
431  for(int i = 0; i < base_kinematics_.num_casters_; i++)
432  {
433  // caster_controller_[i]->setCommand(base_kinematics_.caster_[i].steer_velocity_desired_);
434  }
435 }
436 
437 void Pr2BaseController2::computeDesiredWheelSpeeds(const double &dT)
438 {
439  geometry_msgs::Twist wheel_point_velocity;
440  geometry_msgs::Twist wheel_point_velocity_projected;
441  geometry_msgs::Twist wheel_caster_steer_component;
442  geometry_msgs::Twist caster_2d_velocity;
443 
444  caster_2d_velocity.linear.x = 0;
445  caster_2d_velocity.linear.y = 0;
446  caster_2d_velocity.angular.z = 0;
447 
448  for(int i = 0; i < base_kinematics_.num_wheels_; i++)
449  {
450  filtered_wheel_velocity_[i] = base_kinematics_.wheel_[i].joint_->velocity_;
451  }
452  wheel_vel_filter_.update(filtered_wheel_velocity_,filtered_wheel_velocity_);
453 
454  double steer_angle_actual = 0;
455  for(int i = 0; i < (int) base_kinematics_.num_wheels_; i++)
456  {
457  base_kinematics_.wheel_[i].updatePosition();
458  // caster_2d_velocity.angular.z = base_kinematics_.wheel_[i].parent_->steer_velocity_desired_;
459  caster_2d_velocity.angular.z = 0.0 - base_kinematics_.wheel_[i].parent_->caster_position_error_;
460  steer_angle_actual = base_kinematics_.wheel_[i].parent_->joint_->position_;
461  wheel_point_velocity = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].position_, cmd_vel_);
462  wheel_caster_steer_component = base_kinematics_.pointVel2D(base_kinematics_.wheel_[i].offset_, caster_2d_velocity);
463 
464  double costh = cos(-steer_angle_actual);
465  double sinth = sin(-steer_angle_actual);
466 
467  wheel_point_velocity_projected.linear.x = costh * wheel_point_velocity.linear.x - sinth * wheel_point_velocity.linear.y;
468  wheel_point_velocity_projected.linear.y = sinth * wheel_point_velocity.linear.x + costh * wheel_point_velocity.linear.y;
469  base_kinematics_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.linear.x) / (base_kinematics_.wheel_[i].wheel_radius_);
470  double command = wheel_pid_controllers_[i].computeCommand(
471  - wheel_caster_steer_component.linear.x/base_kinematics_.wheel_[i].wheel_radius_,
472  base_kinematics_.wheel_[i].wheel_speed_cmd_ - filtered_wheel_velocity_[i],
473  ros::Duration(dT));
474  base_kinematics_.wheel_[i].joint_->commanded_effort_ = command;
475  }
476 }
477 
478 void Pr2BaseController2::setDesiredWheelSpeeds()
479 {
480  /* for(int i = 0; i < (int) base_kinematics_.num_wheels_; i++)
481  {
482  wheel_controller_[i]->setCommand(base_kinematics_.wheel_[i].direction_multiplier_ * base_kinematics_.wheel_[i].wheel_speed_cmd_);
483  }*/
484 }
485 
486 void Pr2BaseController2::updateJointControllers()
487 {
488  /* for(int i = 0; i < base_kinematics_.num_wheels_; i++)
489  wheel_controller_[i]->update();
490  for(int i = 0; i < base_kinematics_.num_casters_; i++)
491  caster_controller_[i]->update();*/
492 }
493 
494 void Pr2BaseController2::commandCallback(const geometry_msgs::TwistConstPtr& msg)
495 {
496  pthread_mutex_lock(&pr2_base_controller_lock_);
497  base_vel_msg_ = *msg;
498  this->setCommand(base_vel_msg_);
499  pthread_mutex_unlock(&pr2_base_controller_lock_);
500 }
501 } // namespace
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
static const double EPS
bool init(const ros::NodeHandle &n, const bool quiet=false)
ROSLIB_DECL std::string command(const std::string &cmd)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static const T & clamp(const T &a, const T &b, const T &c)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
def normalize_angle(angle)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
#define ROS_ERROR(...)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
def shortest_angular_distance(from_angle, to_angle)
#define ROS_DEBUG(...)


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Wed Jun 5 2019 19:34:08