pr2_base_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 /*
00035  * Author: Sachin Chitta and Matthew Piccoli
00036  */
00037 
00038 #include "pr2_mechanism_controllers/pr2_base_controller.h"
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_EXPORT_CLASS( controller::Pr2BaseController, pr2_controller_interface::Controller)
00042 
00043 namespace controller {
00044 
00045   const static double EPS = 1e-5;
00046 
00047 Pr2BaseController::Pr2BaseController()
00048 {
00049   //init variables
00050   cmd_vel_.linear.x = 0;
00051   cmd_vel_.linear.y = 0;
00052   cmd_vel_.angular.z = 0;
00053 
00054   desired_vel_.linear.x = 0;
00055   desired_vel_.linear.y = 0;
00056   desired_vel_.angular.z = 0;
00057 
00058   cmd_vel_t_.linear.x = 0;
00059   cmd_vel_t_.linear.y = 0;
00060   cmd_vel_t_.angular.z = 0;
00061 
00062   new_cmd_available_ = false;
00063   last_publish_time_ = ros::Time(0.0);
00064 
00065   pthread_mutex_init(&pr2_base_controller_lock_, NULL);
00066 }
00067 
00068 Pr2BaseController::~Pr2BaseController()
00069 {
00070   cmd_sub_.shutdown();
00071   cmd_sub_deprecated_.shutdown();
00072 }
00073 
00074 bool Pr2BaseController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00075 {
00076   if(!base_kin_.init(robot,n))
00077     return false;
00078   node_ = n;
00079   state_publisher_.reset(new realtime_tools::RealtimePublisher<pr2_mechanism_controllers::BaseControllerState>(n, base_kin_.name_ + "/state", 1));
00080 
00081   int num_joints = base_kin_.num_wheels_ + base_kin_.num_casters_;
00082   state_publisher_->msg_.joint_names.resize(num_joints);
00083   state_publisher_->msg_.joint_velocity_measured.resize(num_joints);
00084   state_publisher_->msg_.joint_effort_measured.resize(num_joints);
00085   state_publisher_->msg_.joint_velocity_commanded.resize(num_joints);
00086   state_publisher_->msg_.joint_effort_commanded.resize(num_joints);
00087   state_publisher_->msg_.joint_velocity_error.resize(num_joints);
00088   state_publisher_->msg_.joint_effort_error.resize(num_joints);
00089 
00090   //Get params from param server
00091   node_.param<double> ("max_translational_velocity", max_translational_velocity_,0.5);
00092   node_.param<double> ("max_rotational_velocity", max_rotational_velocity_, 10.0); //0.5
00093   node_.param<double> ("max_translational_acceleration/x", max_accel_.linear.x, .2);
00094   node_.param<double> ("max_translational_acceleration/y", max_accel_.linear.y, .2);
00095   node_.param<double> ("max_rotational_acceleration", max_accel_.angular.z, 10.0); //0.2
00096 
00097   node_.param<double> ("kp_caster_steer", kp_caster_steer_, 80.0);
00098   node_.param<double> ("timeout", timeout_, 1.0);
00099   node_.param<double> ("state_publish_rate", state_publish_rate_,2.0);
00100   if(state_publish_rate_ <= 0.0)
00101   {
00102     publish_state_ = false;
00103     state_publish_time_ = 0.0;
00104   }
00105   else
00106   {
00107     publish_state_ = true;
00108     state_publish_time_ = 1.0/state_publish_rate_;
00109   }
00110 
00111   //  cmd_sub_deprecated_ = root_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &Pr2BaseController::commandCallback, this);
00112   cmd_sub_ = node_.subscribe<geometry_msgs::Twist>("command", 1, &Pr2BaseController::commandCallback, this);
00113 
00114   //casters
00115   caster_controller_.resize(base_kin_.num_casters_);
00116   caster_position_pid_.resize(base_kin_.num_casters_);
00117   for(int i = 0; i < base_kin_.num_casters_; i++)
00118   {
00119     control_toolbox::Pid p_i_d;
00120     state_publisher_->msg_.joint_names[i] = base_kin_.caster_[i].joint_name_;
00121     if(!p_i_d.init(ros::NodeHandle(node_, base_kin_.caster_[i].joint_name_+"/velocity_controller")))
00122     {
00123       ROS_ERROR("Could not initialize pid for %s",base_kin_.caster_[i].joint_name_.c_str());
00124       return false;
00125     }
00126 
00127     if(!caster_position_pid_[i].init(ros::NodeHandle(node_, base_kin_.caster_[i].joint_name_+"/position_controller")))
00128     {
00129       ROS_ERROR("Could not initialize position pid controller for %s",base_kin_.caster_[i].joint_name_.c_str());
00130       return false;
00131     }
00132     caster_controller_[i].reset(new JointVelocityController());
00133     if(!caster_controller_[i]->init(base_kin_.robot_state_, base_kin_.caster_[i].joint_name_, p_i_d))
00134     {
00135       ROS_ERROR("Could not initialize pid for %s",base_kin_.caster_[i].joint_name_.c_str());
00136       return false;
00137     }
00138     if (!caster_controller_[i]->joint_state_->calibrated_)
00139     {
00140       ROS_ERROR("Caster joint \"%s\" not calibrated (namespace: %s)",
00141                 base_kin_.caster_[i].joint_name_.c_str(), node_.getNamespace().c_str());
00142       return false;
00143     }
00144   }
00145   //wheels
00146   wheel_controller_.resize(base_kin_.num_wheels_);
00147   for(int j = 0; j < base_kin_.num_wheels_; j++)
00148   {
00149     control_toolbox::Pid p_i_d;
00150     state_publisher_->msg_.joint_names[j + base_kin_.num_casters_] = base_kin_.wheel_[j].joint_name_;
00151     if(!p_i_d.init(ros::NodeHandle(node_,base_kin_.wheel_[j].joint_name_)))
00152     {
00153       ROS_ERROR("Could not initialize pid for %s",base_kin_.wheel_[j].joint_name_.c_str());
00154       return false;
00155     }
00156     wheel_controller_[j].reset(new JointVelocityController());
00157    if(!wheel_controller_[j]->init(base_kin_.robot_state_, base_kin_.wheel_[j].joint_name_, p_i_d))
00158    {
00159       ROS_ERROR("Could not initialize joint velocity controller for %s",base_kin_.wheel_[j].joint_name_.c_str());
00160       return false;
00161    }
00162   }
00163   for(int i = 0; i < base_kin_.num_casters_; ++i)
00164   {
00165     if(!base_kin_.caster_[i].joint_->calibrated_)
00166     {
00167       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.");
00168       return false; // Casters are not calibrated
00169     }
00170   }
00171 
00172   if (!((filters::MultiChannelFilterBase<double>&)caster_vel_filter_).configure(base_kin_.num_casters_, std::string("caster_velocity_filter"), node_)){
00173      ROS_ERROR("BaseController: could not configure velocity filters for casters");
00174      return false;
00175   }
00176   filtered_velocity_.resize(base_kin_.num_casters_);
00177   return true;
00178 }
00179 
00180 // Set the base velocity command
00181 void Pr2BaseController::setCommand(const geometry_msgs::Twist &cmd_vel)
00182 {
00183   double vel_mag = sqrt(cmd_vel.linear.x * cmd_vel.linear.x + cmd_vel.linear.y * cmd_vel.linear.y);
00184   double clamped_vel_mag = filters::clamp(vel_mag,-max_translational_velocity_, max_translational_velocity_);
00185   if(vel_mag > EPS)
00186   {
00187     cmd_vel_t_.linear.x = cmd_vel.linear.x * clamped_vel_mag / vel_mag;
00188     cmd_vel_t_.linear.y = cmd_vel.linear.y * clamped_vel_mag / vel_mag;
00189   }
00190   else
00191   {
00192     cmd_vel_t_.linear.x = 0.0;
00193     cmd_vel_t_.linear.y = 0.0;
00194   }
00195   cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_rotational_velocity_, max_rotational_velocity_);
00196   cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
00197 
00198   ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
00199   ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
00200   ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag);
00201   ROS_DEBUG("BaseController:: vel: %f", vel_mag);
00202 
00203   for(int i=0; i < (int) base_kin_.num_wheels_; i++)
00204   {
00205     ROS_DEBUG("BaseController:: wheel speed cmd:: %d %f",i,(base_kin_.wheel_[i].direction_multiplier_*base_kin_.wheel_[i].wheel_speed_cmd_));
00206   }
00207   for(int i=0; i < (int) base_kin_.num_casters_; i++)
00208   {
00209     ROS_DEBUG("BaseController:: caster speed cmd:: %d %f",i,(base_kin_.caster_[i].steer_velocity_desired_));
00210   }
00211   new_cmd_available_ = true;
00212 }
00213 
00214 geometry_msgs::Twist Pr2BaseController::interpolateCommand(const geometry_msgs::Twist &start, const geometry_msgs::Twist &end, const geometry_msgs::Twist &max_rate, const double &dT)
00215 {
00216   geometry_msgs::Twist result;
00217   geometry_msgs::Twist alpha;
00218   double delta(0), max_delta(0);
00219 
00220   delta = end.linear.x - start.linear.x;
00221   max_delta = max_rate.linear.x * dT;
00222   if(fabs(delta) <= max_delta || max_delta < EPS)
00223     alpha.linear.x = 1;
00224   else
00225     alpha.linear.x = max_delta / fabs(delta);
00226 
00227   delta = end.linear.y - start.linear.y;
00228   max_delta = max_rate.linear.y * dT;
00229   if(fabs(delta) <= max_delta || max_delta < EPS)
00230     alpha.linear.y = 1;
00231   else
00232     alpha.linear.y = max_delta / fabs(delta);
00233 
00234   delta = end.angular.z - start.angular.z;
00235   max_delta = max_rate.angular.z * dT;
00236   if(fabs(delta) <= max_delta || max_delta < EPS)
00237     alpha.angular.z = 1;
00238   else
00239     alpha.angular.z = max_delta / fabs(delta);
00240 
00241   double alpha_min = alpha.linear.x;
00242   if(alpha.linear.y < alpha_min)
00243     alpha_min = alpha.linear.y;
00244   if(alpha.angular.z < alpha_min)
00245     alpha_min = alpha.angular.z;
00246 
00247   result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x);
00248   result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y);
00249   result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z);
00250   return result;
00251 }
00252 
00253 geometry_msgs::Twist Pr2BaseController::getCommand()// Return the current velocity command
00254 {
00255   geometry_msgs::Twist cmd_vel;
00256   pthread_mutex_lock(&pr2_base_controller_lock_);
00257   cmd_vel.linear.x = cmd_vel_.linear.x;
00258   cmd_vel.linear.y = cmd_vel_.linear.y;
00259   cmd_vel.angular.z = cmd_vel_.angular.z;
00260   pthread_mutex_unlock(&pr2_base_controller_lock_);
00261   return cmd_vel;
00262 }
00263 
00264 void Pr2BaseController::starting()
00265 {
00266   last_time_ = base_kin_.robot_state_->getTime();
00267   cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
00268   for(int i = 0; i < base_kin_.num_casters_; i++)
00269   {
00270     caster_controller_[i]->starting();
00271   }
00272   for(int j = 0; j < base_kin_.num_wheels_; j++)
00273   {
00274     wheel_controller_[j]->starting();
00275   }
00276 }
00277 
00278 void Pr2BaseController::update()
00279 {
00280   ros::Time current_time = base_kin_.robot_state_->getTime();
00281   double dT = std::min<double>((current_time - last_time_).toSec(), base_kin_.MAX_DT_);
00282 
00283   if(new_cmd_available_)
00284   {
00285     if(pthread_mutex_trylock(&pr2_base_controller_lock_) == 0)
00286     {
00287       desired_vel_.linear.x = cmd_vel_t_.linear.x;
00288       desired_vel_.linear.y = cmd_vel_t_.linear.y;
00289       desired_vel_.angular.z = cmd_vel_t_.angular.z;
00290       new_cmd_available_ = false;
00291       pthread_mutex_unlock(&pr2_base_controller_lock_);
00292     }
00293   }
00294 
00295   if((current_time - cmd_received_timestamp_).toSec() > timeout_)
00296   {
00297     cmd_vel_.linear.x = 0;
00298     cmd_vel_.linear.y = 0;
00299     cmd_vel_.angular.z = 0;
00300   }
00301   else
00302     cmd_vel_ = interpolateCommand(cmd_vel_, desired_vel_, max_accel_, dT);
00303 
00304   computeJointCommands(dT);
00305 
00306   setJointCommands();
00307 
00308   updateJointControllers();
00309 
00310   if(publish_state_)
00311     publishState(current_time);
00312 
00313   last_time_ = current_time;
00314 
00315 }
00316 
00317 void Pr2BaseController::publishState(const ros::Time &time)
00318 {
00319   if((time - last_publish_time_).toSec()  < state_publish_time_)
00320   {
00321     return;
00322   }
00323 
00324   if(state_publisher_->trylock())
00325   {
00326     state_publisher_->msg_.command.linear.x  = cmd_vel_.linear.x;
00327     state_publisher_->msg_.command.linear.y  = cmd_vel_.linear.y;
00328     state_publisher_->msg_.command.angular.z = cmd_vel_.angular.z;
00329 
00330     for(int i = 0; i < base_kin_.num_casters_; i++)
00331     {
00332       state_publisher_->msg_.joint_names[i] = base_kin_.caster_[i].joint_name_;
00333       state_publisher_->msg_.joint_velocity_measured[i] = base_kin_.caster_[i].joint_->velocity_;
00334       state_publisher_->msg_.joint_velocity_commanded[i]= base_kin_.caster_[i].steer_velocity_desired_;
00335       state_publisher_->msg_.joint_velocity_error[i]    = base_kin_.caster_[i].joint_->velocity_ - base_kin_.caster_[i].steer_velocity_desired_;
00336 
00337       state_publisher_->msg_.joint_effort_measured[i]  = base_kin_.caster_[i].joint_->measured_effort_;
00338       state_publisher_->msg_.joint_effort_commanded[i] = base_kin_.caster_[i].joint_->commanded_effort_;
00339       state_publisher_->msg_.joint_effort_error[i]     = base_kin_.caster_[i].joint_->measured_effort_ - base_kin_.caster_[i].joint_->commanded_effort_;
00340     }
00341     for(int i = 0; i < base_kin_.num_wheels_; i++)
00342     {
00343       state_publisher_->msg_.joint_names[i+base_kin_.num_casters_] = base_kin_.wheel_[i].joint_name_;
00344       state_publisher_->msg_.joint_velocity_measured[i+base_kin_.num_casters_] = base_kin_.wheel_[i].wheel_speed_actual_;
00345       state_publisher_->msg_.joint_velocity_commanded[i+base_kin_.num_casters_]= base_kin_.wheel_[i].wheel_speed_error_;
00346       state_publisher_->msg_.joint_velocity_error[i+base_kin_.num_casters_]    = base_kin_.wheel_[i].wheel_speed_cmd_;
00347 
00348       state_publisher_->msg_.joint_effort_measured[i+base_kin_.num_casters_]  = base_kin_.wheel_[i].joint_->measured_effort_;
00349       state_publisher_->msg_.joint_effort_commanded[i+base_kin_.num_casters_] = base_kin_.wheel_[i].joint_->commanded_effort_;
00350       state_publisher_->msg_.joint_effort_error[i+base_kin_.num_casters_]     = base_kin_.wheel_[i].joint_->measured_effort_ - base_kin_.wheel_[i].joint_->commanded_effort_;
00351     }
00352     state_publisher_->unlockAndPublish();
00353     last_publish_time_ = time;
00354   }
00355 }
00356 
00357 void Pr2BaseController::computeJointCommands(const double &dT)
00358 {
00359   base_kin_.computeWheelPositions();
00360 
00361   computeDesiredCasterSteer(dT);
00362 
00363   computeDesiredWheelSpeeds();
00364 }
00365 
00366 void Pr2BaseController::setJointCommands()
00367 {
00368   setDesiredCasterSteer();
00369 
00370   setDesiredWheelSpeeds();
00371 }
00372 
00373 void Pr2BaseController::computeDesiredCasterSteer(const double &dT)
00374 {
00375   geometry_msgs::Twist result;
00376 
00377   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
00378   double error_steer(0.0), error_steer_m_pi(0.0);
00379   double trans_vel = sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x + cmd_vel_.linear.y * cmd_vel_.linear.y);
00380 
00381   for(int i = 0; i < base_kin_.num_casters_; i++)
00382   {
00383     filtered_velocity_[i] = 0.0 - base_kin_.caster_[i].joint_->velocity_;
00384   }
00385   caster_vel_filter_.update(filtered_velocity_,filtered_velocity_);
00386 
00387   for(int i = 0; i < base_kin_.num_casters_; i++)
00388   {
00389     result = base_kin_.pointVel2D(base_kin_.caster_[i].offset_, cmd_vel_);
00390     if(trans_vel < EPS && fabs(cmd_vel_.angular.z) < EPS)
00391     {
00392       steer_angle_desired = base_kin_.caster_[i].steer_angle_stored_;
00393     }
00394     else
00395     {
00396       steer_angle_desired = atan2(result.linear.y, result.linear.x);
00397       base_kin_.caster_[i].steer_angle_stored_ = steer_angle_desired;
00398     }
00399     steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired + M_PI);
00400     error_steer = angles::shortest_angular_distance(
00401           base_kin_.caster_[i].joint_->position_,
00402           steer_angle_desired);
00403     error_steer_m_pi = angles::shortest_angular_distance(
00404           base_kin_.caster_[i].joint_->position_,
00405           steer_angle_desired_m_pi);
00406 
00407     if(fabs(error_steer_m_pi) < fabs(error_steer))
00408     {
00409       error_steer = error_steer_m_pi;
00410       steer_angle_desired = steer_angle_desired_m_pi;
00411     }
00412     //    base_kin_.caster_[i].steer_velocity_desired_ = -kp_caster_steer_ * error_steer;
00413     base_kin_.caster_[i].steer_velocity_desired_ = caster_position_pid_[i].computeCommand(
00414           error_steer,
00415           filtered_velocity_[i],
00416           ros::Duration(dT));
00417     base_kin_.caster_[i].caster_position_error_ = error_steer;
00418   }
00419 }
00420 
00421 void Pr2BaseController::setDesiredCasterSteer()
00422 {
00423   for(int i = 0; i < base_kin_.num_casters_; i++)
00424   {
00425     caster_controller_[i]->setCommand(base_kin_.caster_[i].steer_velocity_desired_);
00426   }
00427 }
00428 
00429 void Pr2BaseController::computeDesiredWheelSpeeds()
00430 {
00431   geometry_msgs::Twist wheel_point_velocity;
00432   geometry_msgs::Twist wheel_point_velocity_projected;
00433   geometry_msgs::Twist wheel_caster_steer_component;
00434   geometry_msgs::Twist caster_2d_velocity;
00435 
00436   caster_2d_velocity.linear.x = 0;
00437   caster_2d_velocity.linear.y = 0;
00438   caster_2d_velocity.angular.z = 0;
00439 
00440   double steer_angle_actual = 0;
00441   for(int i = 0; i < (int) base_kin_.num_wheels_; i++)
00442   {
00443     base_kin_.wheel_[i].updatePosition();
00444     caster_2d_velocity.angular.z = base_kin_.wheel_[i].parent_->steer_velocity_desired_;
00445     steer_angle_actual = base_kin_.wheel_[i].parent_->joint_->position_;
00446     wheel_point_velocity = base_kin_.pointVel2D(base_kin_.wheel_[i].position_, cmd_vel_);
00447     wheel_caster_steer_component = base_kin_.pointVel2D(base_kin_.wheel_[i].offset_, caster_2d_velocity);
00448 
00449     double costh = cos(-steer_angle_actual);
00450     double sinth = sin(-steer_angle_actual);
00451 
00452     wheel_point_velocity_projected.linear.x = costh * wheel_point_velocity.linear.x - sinth * wheel_point_velocity.linear.y;
00453     wheel_point_velocity_projected.linear.y = sinth * wheel_point_velocity.linear.x + costh * wheel_point_velocity.linear.y;
00454     base_kin_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.linear.x + wheel_caster_steer_component.linear.x) / (base_kin_.wheel_[i].wheel_radius_);
00455   }
00456 }
00457 
00458 void Pr2BaseController::setDesiredWheelSpeeds()
00459 {
00460   for(int i = 0; i < (int) base_kin_.num_wheels_; i++)
00461   {
00462     wheel_controller_[i]->setCommand(base_kin_.wheel_[i].direction_multiplier_ * base_kin_.wheel_[i].wheel_speed_cmd_);
00463   }
00464 }
00465 
00466 void Pr2BaseController::updateJointControllers()
00467 {
00468   for(int i = 0; i < base_kin_.num_wheels_; i++)
00469     wheel_controller_[i]->update();
00470   for(int i = 0; i < base_kin_.num_casters_; i++)
00471     caster_controller_[i]->update();
00472 }
00473 
00474 void Pr2BaseController::commandCallback(const geometry_msgs::TwistConstPtr& msg)
00475 {
00476   pthread_mutex_lock(&pr2_base_controller_lock_);
00477   base_vel_msg_ = *msg;
00478   this->setCommand(base_vel_msg_);
00479   pthread_mutex_unlock(&pr2_base_controller_lock_);
00480 }
00481 } // namespace


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Sat Jun 8 2019 20:49:33