joint_qualification_controllers: joint_qualification_controllers: counterbalance_test

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: Kevin Watts */
00036 
00037 
00038 #include <joint_qualification_controllers/counterbalance_test_controller.h>
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_DECLARE_CLASS(joint_qualification_controllers, CounterbalanceTestController, 
00042                         joint_qualification_controllers::CounterbalanceTestController, 
00043                         pr2_controller_interface::Controller)
00044 
00045 using namespace std;
00046 using namespace joint_qualification_controllers;
00047 
00048 CounterbalanceTestController::CounterbalanceTestController() :
00049   lift_dither_(NULL),
00050   flex_dither_(NULL),
00051   lift_controller_(NULL),
00052   flex_controller_(NULL),
00053   robot_(NULL),
00054   initial_time_(0.0),
00055   start_time_(0.0),
00056   cb_data_pub_(NULL)
00057 {
00058   timeout_ = 180;
00059   lift_index_ = 0;
00060   flex_index_ = 0;
00061   data_sent_ = false;
00062 
00063   cb_test_data_.arg_name.resize(25);
00064   cb_test_data_.arg_value.resize(25);
00065   cb_test_data_.arg_name[0] = "Settle Time";
00066   cb_test_data_.arg_name[1] = "Dither Points";
00067   cb_test_data_.arg_name[2] = "Timeout";
00068   cb_test_data_.arg_name[3] = "Lift Min";
00069   cb_test_data_.arg_name[4] = "Lift Max";
00070   cb_test_data_.arg_name[5] = "Lift Delta";
00071   cb_test_data_.arg_name[6] = "Flex Min";
00072   cb_test_data_.arg_name[7] = "Flex Max";
00073   cb_test_data_.arg_name[8] = "Flex Delta";
00074 
00075   // P/F params
00076   cb_test_data_.arg_name[9] = "Lift MSE";
00077   cb_test_data_.arg_name[10] = "Lift Avg Abs";
00078   cb_test_data_.arg_name[11] = "Lift Avg Effort";
00079   cb_test_data_.arg_name[12] = "Flex MSE";
00080   cb_test_data_.arg_name[13] = "Flex Avg Abs";
00081   cb_test_data_.arg_name[14] = "Flex Avg Effort";
00082 
00083   // PID's
00084   cb_test_data_.arg_name[15] = "Lift P";
00085   cb_test_data_.arg_name[16] = "Lift I";
00086   cb_test_data_.arg_name[17] = "Lift D";
00087   cb_test_data_.arg_name[18] = "Lift I Clamp";
00088 
00089   cb_test_data_.arg_name[19] = "Flex P";
00090   cb_test_data_.arg_name[20] = "Flex I";
00091   cb_test_data_.arg_name[21] = "Flex D";
00092   cb_test_data_.arg_name[22] = "Flex I Clamp";
00093 
00094   cb_test_data_.arg_name[23] = "Screw Tolerance";
00095   cb_test_data_.arg_name[24] = "Bar Tolerance";
00096 
00097   cb_test_data_.timeout_hit = false;
00098   cb_test_data_.lift_joint = "None";
00099   cb_test_data_.lift_joint = "";
00100   cb_test_data_.lift_amplitude = 0;
00101   cb_test_data_.flex_amplitude = 0;
00102 
00104 
00105   state_ = STARTING;
00106 
00107 }
00108 
00109 CounterbalanceTestController::~CounterbalanceTestController()
00110 {
00111   if (lift_controller_) delete lift_controller_;
00112   if (flex_controller_) delete flex_controller_;
00113   if (flex_dither_) delete flex_dither_;
00114   if (lift_dither_) delete lift_dither_;
00115 }
00116 
00117 bool CounterbalanceTestController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00118 {
00119   ROS_ASSERT(robot);
00120   robot_ = robot;
00121 
00122   // Lift joint
00123   string lift_joint;
00124   if (!n.getParam("lift/joint", lift_joint)){
00125     ROS_ERROR("CounterbalanceTestController: No lift joint name found on parameter namespace: %s)",
00126               n.getNamespace().c_str());
00127     return false;
00128   }
00129   if (!(lift_state_ = robot->getJointState(lift_joint)))
00130   {
00131     ROS_ERROR("CounterbalanceTestController could not find lift joint named \"%s\"\n", lift_joint.c_str());
00132     return false;
00133   }
00134   cb_test_data_.lift_joint = lift_joint;
00135 
00136   lift_controller_ = new controller::JointPositionController();
00137   ros::NodeHandle n_lift(n, "lift");
00138   if (!lift_controller_->init(robot, n_lift)) return false;
00139 
00140   int num_flexs = 1;
00141   double flex_min = 0;
00142   double flex_max = 0;
00143   double flex_delta = 0;
00144   double flex_mse = 0;
00145   double flex_avg_abs = 0;
00146   double flex_avg_eff = 0;
00147 
00148   cb_test_data_.flex_test = false;
00149 
00150   // Flex joint
00151   string flex_joint;
00152   if (n.getParam("flex/joint", flex_joint))
00153   {
00154     cb_test_data_.flex_test = true;
00155 
00156     if (!(flex_state_ = robot->getJointState(flex_joint)))
00157     {
00158       ROS_ERROR("CounterbalanceTestController could not find flex joint named \"%s\"\n", flex_joint.c_str());
00159       return false;
00160     }
00161     cb_test_data_.flex_joint = flex_joint;
00162 
00163     // Flex range
00164     if (!n.getParam("flex/min", flex_min))
00165     {
00166       ROS_ERROR("CounterbalanceTestController: No min flex position found on parameter namespace: %s)",
00167                 n.getNamespace().c_str());
00168       return false;
00169     }
00170     if (!n.getParam("flex/max", flex_max))
00171     {
00172       ROS_ERROR("CounterbalanceTestController: No max flex position found on parameter namespace: %s)",
00173                 n.getNamespace().c_str());
00174       return false;
00175     }
00176     if (!n.getParam("flex/delta", flex_delta))
00177     {
00178     ROS_ERROR("CounterbalanceTestController: No flex delta found on parameter namespace: %s)",
00179               n.getNamespace().c_str());
00180     return false;
00181     }
00182 
00183     if (flex_max < flex_min)
00184     {
00185       ROS_ERROR("CounterbalanceTestController: Flex max position must be >= flex min position");
00186       return false;
00187     }
00188     if (flex_delta < 0)
00189     {
00190       ROS_ERROR("CounterbalanceTestController: \"flex_delta\" parameter must be >=0");
00191       return false;
00192     }
00193 
00194     num_flexs = (int)(((flex_max - flex_min) / flex_delta) + 1);
00195 
00196     // MSE, Avg. Abs. Effort
00197     if (!n.getParam("flex/mse", flex_mse))
00198     {
00199       ROS_ERROR("CounterbalanceTestController: No flex/mse (mean square effort) found on parameter namespace: %s)",
00200                 n.getNamespace().c_str());
00201       return false;
00202     }
00203     
00204     if (!n.getParam("flex/avg_abs", flex_avg_abs))
00205     {
00206       ROS_ERROR("CounterbalanceTestController: No flex/avg_abs (average absolute effort) found on parameter namespace: %s)",
00207                 n.getNamespace().c_str());
00208       return false;
00209     }
00210 
00211     if (!n.getParam("flex/avg_eff", flex_avg_eff))
00212     {
00213       ROS_ERROR("CounterbalanceTestController: No flex/avg_eff (average absolute effort) found on parameter namespace: %s)",
00214                 n.getNamespace().c_str());
00215       return false;
00216     }
00217 
00218     double flex_amplitude;
00219     if (!n.getParam("flex/dither", flex_amplitude))
00220     {
00221       ROS_ERROR("CounterbalanceTestController: No flex amplitude found on parameter namespace: %s)",
00222                 n.getNamespace().c_str());
00223       return false;
00224     }
00225     flex_dither_ = new control_toolbox::Dither; 
00226     if (!flex_dither_->init(flex_amplitude, 100)) return false;
00227 
00228     cb_test_data_.flex_amplitude = flex_amplitude;
00229 
00230     flex_controller_ = new controller::JointPositionController();
00231     ros::NodeHandle n_flex(n, "flex");
00232     if (!flex_controller_->init(robot, n_flex)) return false;
00233 
00234     ROS_INFO("Initializing CounterbalanceTestController as a lift and flex test. Namespace: %s", 
00235              n.getNamespace().c_str());
00236   }
00237   else
00238   {
00239     ROS_INFO("Initializing CounterbalanceTestController as a shoulder lift test only. Namespace: %s",
00240              n.getNamespace().c_str());
00241   }
00242 
00243   double lift_amplitude;
00244   if (!n.getParam("lift/dither", lift_amplitude))
00245   {
00246     ROS_ERROR("CounterbalanceTestController: No lift amplitude found on parameter namespace: %s)",
00247               n.getNamespace().c_str());
00248     return false;
00249   }
00250   lift_dither_ = new control_toolbox::Dither; 
00251   if (!lift_dither_->init(lift_amplitude, 100)) return false;
00252 
00253   cb_test_data_.lift_amplitude = lift_amplitude;
00254 
00255   // Lift range
00256   double lift_min, lift_max, lift_delta;
00257   if (!n.getParam("lift/min", lift_min))
00258   {
00259     ROS_ERROR("CounterbalanceTestController: No min lift position found on parameter namespace: %s)",
00260               n.getNamespace().c_str());
00261     return false;
00262   }
00263   if (!n.getParam("lift/max", lift_max))
00264   {
00265     ROS_ERROR("CounterbalanceTestController: No max lift position found on parameter namespace: %s)",
00266               n.getNamespace().c_str());
00267     return false;
00268   }
00269   if (lift_max < lift_min)
00270   {
00271     ROS_ERROR("CounterbalanceTestController was given a shoulder lift maximum with values less than its minimum. Max: %f, Min %f", lift_max, lift_min);
00272     return false;
00273   }
00274 
00275   if (!n.getParam("lift/delta", lift_delta))
00276   {
00277     ROS_ERROR("CounterbalanceTestController: No lift delta found on parameter namespace: %s)",
00278               n.getNamespace().c_str());
00279     return false;
00280   }
00281   if (lift_delta < 0)
00282   {
00283     ROS_ERROR("CounterbalanceTestController was given a shoulder lift position delta <0. Delta given: %f", lift_delta);
00284     return false;
00285   }
00286 
00287   // Setting controller defaults
00288   if (!n.getParam("settle_time", settle_time_))
00289   {
00290     ROS_ERROR("CounterbalanceTestController: No settle time found on parameter namespace: %s)",
00291               n.getNamespace().c_str());
00292     return false;
00293   }
00294 
00295   if (!n.getParam("dither_points", dither_points_))
00296   {
00297     ROS_ERROR("CounterbalanceTestController: No dither points param found on parameter namespace: %s)",
00298               n.getNamespace().c_str());
00299     return false;
00300   }
00301 
00302   if (!n.getParam("timeout", timeout_))
00303   {
00304     ROS_ERROR("CounterbalanceTestController: No timeout found on parameter namespace: %s)",
00305               n.getNamespace().c_str());
00306     return false;
00307   }
00308 
00309   double lift_mse, lift_avg_abs, lift_avg_eff;
00310   if (!n.getParam("lift/mse", lift_mse))
00311   {
00312     ROS_ERROR("CounterbalanceTestController: No lift/mse (mean square effort) found on parameter namespace: %s)",
00313               n.getNamespace().c_str());
00314     return false;
00315   }
00316 
00317   if (!n.getParam("lift/avg_abs", lift_avg_abs))
00318   {
00319     ROS_ERROR("CounterbalanceTestController: No lift/avg_abs (average absolute effort) found on parameter namespace: %s)",
00320               n.getNamespace().c_str());
00321     return false;
00322   }
00323 
00324   if (!n.getParam("lift/avg_eff", lift_avg_eff))
00325   {
00326     ROS_ERROR("CounterbalanceTestController: No lift/avg_eff (average absolute effort) found on parameter namespace: %s)",
00327               n.getNamespace().c_str());
00328     return false;
00329   }
00330 
00331   // Set up array of lift, flex commands
00332   int num_lifts = 1;
00333   if (lift_delta > 0)
00334     num_lifts = (int)((lift_max - lift_min)/lift_delta + 1);
00335   ROS_ASSERT(num_lifts > 0);
00336 
00337   cb_test_data_.lift_data.resize(num_lifts);
00338   for (int i = 0; i < num_lifts; ++i)
00339   {
00340     cb_test_data_.lift_data[i].lift_position = (double)lift_min + lift_delta * i;
00341     cb_test_data_.lift_data[i].flex_data.resize(num_flexs);
00342     for (int j = 0; j < num_flexs; ++j)
00343     {
00344       cb_test_data_.lift_data[i].flex_data[j].flex_position = (double)flex_min + flex_delta * ((double)j);
00345 
00346       cb_test_data_.lift_data[i].flex_data[j].lift_hold.time.resize(dither_points_);
00347       cb_test_data_.lift_data[i].flex_data[j].lift_hold.position.resize(dither_points_);
00348       cb_test_data_.lift_data[i].flex_data[j].lift_hold.velocity.resize(dither_points_);
00349       cb_test_data_.lift_data[i].flex_data[j].lift_hold.effort.resize(dither_points_);
00350 
00351       cb_test_data_.lift_data[i].flex_data[j].flex_hold.time.resize(dither_points_);
00352       cb_test_data_.lift_data[i].flex_data[j].flex_hold.position.resize(dither_points_);
00353       cb_test_data_.lift_data[i].flex_data[j].flex_hold.velocity.resize(dither_points_);
00354       cb_test_data_.lift_data[i].flex_data[j].flex_hold.effort.resize(dither_points_);
00355     }
00356   }
00357 
00358   cb_test_data_.arg_value[0] = settle_time_;
00359   cb_test_data_.arg_value[1] = dither_points_;
00360   cb_test_data_.arg_value[2] = timeout_;
00361   cb_test_data_.arg_value[3] = lift_min;
00362   cb_test_data_.arg_value[4] = lift_max;
00363   cb_test_data_.arg_value[5] = lift_delta;
00364   cb_test_data_.arg_value[6] = flex_min;
00365   cb_test_data_.arg_value[7] = flex_max;
00366   cb_test_data_.arg_value[8] = flex_delta;
00367 
00368   // Mean square efforts, avg abs efforts
00369   cb_test_data_.arg_value[9] = lift_mse;
00370   cb_test_data_.arg_value[10] = lift_avg_abs;
00371   cb_test_data_.arg_value[11] = lift_avg_eff;
00372 
00373   if (cb_test_data_.flex_test)
00374   {
00375     cb_test_data_.arg_value[12] = flex_mse;
00376     cb_test_data_.arg_value[13] = flex_avg_abs;
00377     cb_test_data_.arg_value[14] = flex_avg_eff;
00378   }
00379   else
00380   {
00381     cb_test_data_.arg_value[12] = 0;
00382     cb_test_data_.arg_value[13] = 0;
00383     cb_test_data_.arg_value[14] = 0;
00384   }
00385 
00386   double p, i, d, i_clamp, dummy;
00387   lift_controller_->getGains(p, i, d, i_clamp, dummy);
00388   cb_test_data_.arg_value[15] = p;
00389   cb_test_data_.arg_value[16] = i;
00390   cb_test_data_.arg_value[17] = d;
00391   cb_test_data_.arg_value[18] = i_clamp;
00392 
00393   if (cb_test_data_.flex_test)
00394   {
00395     flex_controller_->getGains(p, i, d, i_clamp, dummy);
00396     cb_test_data_.arg_value[19] = p;
00397     cb_test_data_.arg_value[20] = i;
00398     cb_test_data_.arg_value[21] = d;
00399     cb_test_data_.arg_value[22] = i_clamp;
00400   }
00401   else
00402   {
00403     cb_test_data_.arg_value[19] = 0;
00404     cb_test_data_.arg_value[20] = 0;
00405     cb_test_data_.arg_value[21] = 0;
00406     cb_test_data_.arg_value[22] = 0;
00407   }
00408 
00409   // Record screw, bar tolerances
00410   double screw_tol, bar_tol;
00411   if (!n.getParam("screw_tol", screw_tol))
00412   {
00413     ROS_INFO("CounterbalanceTestController was not given parameter 'screw_tol' on namespace %s. Using default 2.0",
00414               n.getNamespace().c_str());
00415     screw_tol = 2.0;
00416   }  
00417 
00418   if (!n.getParam("bar_tol", bar_tol))
00419   {
00420     ROS_INFO("CounterbalanceTestController was not given parameter 'bar_tol' on namespace %s. Using default 0.8",
00421               n.getNamespace().c_str());
00422     bar_tol = 0.8;
00423   }  
00424   cb_test_data_.arg_value[23] = screw_tol;
00425   cb_test_data_.arg_value[24] = bar_tol;
00426 
00427 
00428   cb_data_pub_.reset(new realtime_tools::RealtimePublisher<
00429                      joint_qualification_controllers::CounterbalanceTestData>(n, "/cb_test_data", 1, true));
00430 
00431   //ROS_INFO("Initialized CB controller successfully!");
00432 
00433   return true;
00434 }
00435 
00436 void CounterbalanceTestController::starting()
00437 {
00438   initial_time_ = robot_->getTime();
00439 }
00440 
00441 void CounterbalanceTestController::update()
00442 {
00443   // wait until the joints are calibrated to start
00444   if (!lift_state_->calibrated_)
00445     return;
00446   if (cb_test_data_.flex_test and !flex_state_->calibrated_)
00447     return;
00448 
00449   ros::Time time = robot_->getTime();
00450 
00451   if ((time - initial_time_).toSec() > timeout_ && state_ != DONE)
00452   {
00453     ROS_WARN("CounterbalanceTestController timed out during test. Timeout: %f.", timeout_);
00454     state_ = DONE;
00455     cb_test_data_.timeout_hit = true;
00456   }
00457 
00458   lift_controller_->update();
00459   if (cb_test_data_.flex_test) 
00460     flex_controller_->update();
00461 
00462   switch (state_)
00463   {
00464   case STARTING:
00465     {
00466       double lift_cmd = cb_test_data_.lift_data[lift_index_].lift_position;
00467       double flex_cmd = cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_position;
00468 
00469       // Set controllers
00470       lift_controller_->setCommand(lift_cmd);
00471       if (cb_test_data_.flex_test) flex_controller_->setCommand(flex_cmd);
00472 
00473       dither_count_ = 0;
00474       start_time_ = time;
00475 
00476       state_ = SETTLING;
00477 
00478       //ROS_INFO("Moving to position lift %f, flex %f", lift_cmd, flex_cmd);
00479       break;
00480     }
00481   case SETTLING:
00482     {
00483       if ((time - start_time_).toSec() > settle_time_)
00484       {
00485         state_ = DITHERING;
00486         start_time_ = time;
00487       }
00488 
00489       break;
00490     }
00491   case DITHERING:
00492     {
00493       // Add dither
00494       lift_state_->commanded_effort_ += lift_dither_->update();
00495       if (cb_test_data_.flex_test) flex_state_->commanded_effort_ += flex_dither_->update();
00496 
00497       // Lift
00498       cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.time[dither_count_] = (time - start_time_).toSec();
00499       cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.position[dither_count_] = lift_state_->position_;
00500       cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.velocity[dither_count_] = lift_state_->velocity_;
00501       cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].lift_hold.effort[dither_count_] = lift_state_->measured_effort_;
00502       
00503       // Flex
00504       cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.time[dither_count_] = (time - start_time_).toSec();
00505       if (cb_test_data_.flex_test)
00506       {
00507         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.position[dither_count_] = flex_state_->position_;
00508         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.velocity[dither_count_] = flex_state_->velocity_;
00509         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.effort[dither_count_] = flex_state_->measured_effort_;
00510       }
00511       else
00512       {
00513         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.position[dither_count_] = 0;
00514         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.velocity[dither_count_] = 0;
00515         cb_test_data_.lift_data[lift_index_].flex_data[flex_index_].flex_hold.effort[dither_count_] = 0;
00516       }
00517 
00518       ++dither_count_;
00519       
00520       if (dither_count_ >= dither_points_)
00521       {
00522         state_ = NEXT;
00523       }
00524       break;
00525     }
00526   case NEXT:
00527     {
00528       // Increment flex, lift indices
00529       ++flex_index_;
00530       if (flex_index_ >= cb_test_data_.lift_data[0].flex_data.size())
00531       {
00532         flex_index_ = 0;
00533         lift_index_++;
00534       }
00535       if (lift_index_ >= cb_test_data_.lift_data.size())
00536       {
00537         state_ = DONE;
00538         break;
00539       }
00540 
00541       state_ = STARTING;
00542       break;
00543     }
00544   case DONE:
00545     {
00546       if (!data_sent_)
00547         data_sent_ = sendData();
00548 
00549       break;
00550     }
00551   }
00552 }
00553 
00554 bool CounterbalanceTestController::sendData()
00555 {
00556   if (cb_data_pub_->trylock())
00557   {
00558     // Copy data and send
00559     joint_qualification_controllers::CounterbalanceTestData *out = &cb_data_pub_->msg_;
00560 
00561     out->lift_joint     = cb_test_data_.lift_joint;
00562     out->flex_joint     = cb_test_data_.flex_joint;
00563     out->lift_amplitude = cb_test_data_.lift_amplitude;
00564     out->flex_amplitude = cb_test_data_.flex_amplitude;
00565     out->timeout_hit    = cb_test_data_.timeout_hit;
00566     out->flex_test      = cb_test_data_.flex_test;
00567     out->arg_name       = cb_test_data_.arg_name;
00568     out->arg_value      = cb_test_data_.arg_value;
00569 
00570     out->lift_data      = cb_test_data_.lift_data;
00571     cb_data_pub_->unlockAndPublish();
00572     return true;
00573   }
00574   return false;
00575 }
00576
counterbalance_test_controller.cpp
