steering.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2008, 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: steering.cc 1161 2011-03-26 02:10:49Z jack.oquin $
00007  */
00008 
00009 #define USE_VOLTAGE_POLYNOMIAL 1
00010 
00011 #include <ros/ros.h>
00012 
00013 #include <art_msgs/ArtHertz.h>
00014 #include <art/polynomial.h>
00015 
00016 #include <art_msgs/SteeringCommand.h>
00017 #include <art_msgs/SteeringState.h>
00018 #include <art_msgs/IOadrState.h>
00019 
00020 #include "devsteer.h"                   // servo device interface
00021 #include "testwheel.h"                  // steering wheel self-test
00022 
00072 static float const epsilon = 0.01;      // "close enough" in degrees
00073 
00074 #define CLASS "ArtSteer"
00075 
00076 class ArtSteer
00077 {
00078 public:
00079 
00080   ArtSteer();
00081   ~ArtSteer();
00082   void  run();
00083 
00084 private:
00085 
00086   // internal methods
00087   int   calibrate_wheel_position(void);
00088   void  close();
00089   void  GetCmd(const art_msgs::SteeringCommand::ConstPtr &cmdIn);
00090   void  GetPos(const art_msgs::IOadrState::ConstPtr &ioIn);
00091   int   open();
00092   void  PublishStatus(void);
00093   void  read_wheel_angle(void);
00094 
00095 
00096   // .cfg variables:
00097   bool  diagnostic_;                    // enable diagnostic mode
00098   bool  simulate_;                      // simulate sensor input
00099   int   calibration_periods_;           // number of sensor calibration cycles
00100   double sensor_timeout_;               // sensor timeout (sec)
00101 
00102   // ROS topic interfaces
00103   ros::Subscriber ioadr_state_;         // ioadr/state (position sensor)
00104   ros::Subscriber steering_cmd_;        // steering/cmd
00105   ros::Publisher  steering_state_;      // steering/state
00106   ros::Publisher  steering_diag_;       // steering/diag
00107 
00108   float steering_angle_;                // current steering angle (degrees)
00109   float steering_sensor_;               // current steering sensor reading
00110   double cur_sensor_time_;              // current sensor data time (sec)
00111   double last_sensor_time_;             // previous sensor data time (sec)
00112   float set_point_;                     // requested steering setting
00113   float last_set_point_;                // previous requested steering setting
00114 
00115   // sensor calibration data
00116   int   calibration_cycle_;
00117   float mean_voltage_;
00118 
00119   // driver state (from art_msgs)
00120   typedef art_msgs::DriverState DriverState;
00121   DriverState::_state_type driver_state_;
00122 
00123   // driver state variables -- the four valid states occur in this order:
00124   //
00125   //    angle_known_    wheel_calibrated_       wheel_tested_
00126   //       false              false                 false
00127   //       true               false                 false
00128   //       true               true                  false
00129   //       true               true                  true
00130   //
00131   bool  angle_known_;                   // wheel angle known
00132   bool  wheel_calibrated_;              // wheel position calibrated
00133   bool  wheel_tested_;                  // wheel motion tested
00134 
00135   boost::shared_ptr<devsteer> dev_;     // servo device interface
00136   boost::shared_ptr<testwheel> tw_;     // wheel self-test class
00137 
00138   // polynomials for converting between sensor voltage and angle
00139   boost::shared_ptr<Polynomial> apoly_; // angle to voltage conversion
00140 #if defined(USE_VOLTAGE_POLYNOMIAL)
00141   boost::shared_ptr<Polynomial> vpoly_; // voltage to angle conversion
00142 #else
00143   std::vector<double> c_;               // volts2degrees() coefficients
00144 #endif
00145 
00146   // convert steering position sensor voltage to degrees
00147   float inline volts2degrees(float volts)
00148   {
00149 #if defined(USE_VOLTAGE_POLYNOMIAL)
00150     return vpoly_->value(volts);        // use polynomial
00151 #else
00152     // non-linear curve fit
00153     return c_[0] + c_[1]*volts + c_[2]*cos(c_[3]*volts+c_[4]);
00154 #endif
00155   }
00156 
00157   // convert steering position sensor degrees to voltage
00158   float inline degrees2volts(float degrees)
00159   {
00160     return apoly_->value(degrees);
00161   }
00162 };
00163 
00164 ArtSteer::ArtSteer():
00165   driver_state_(DriverState::CLOSED),
00166   angle_known_(false),
00167   wheel_calibrated_(false),
00168   wheel_tested_(false),
00169   dev_(new devsteer()),
00170   tw_(new testwheel(dev_))
00171 {
00172   // polynomial to convert steering angles to sensor voltages
00173   // (inverse of vpoly_, only used when simulating steering angles)
00174   apoly_.reset(new Polynomial("apoly"));
00175   apoly_->add_coef(2.544);              // default constant coefficient
00176   apoly_->add_coef(-0.05456918);        // default coefficient of a
00177   apoly_->add_coef(0.00036568);         // default coefficient of a**2
00178   //apoly_->configure(cf, section);     // fill in .cfg values
00179 
00180 #if defined(USE_VOLTAGE_POLYNOMIAL)
00181   // polynomial to convert sensor voltages to steering angles
00182   vpoly_.reset(new Polynomial("vpoly"));
00183   vpoly_->add_coef(62.5943141);         // default constant coefficient
00184   vpoly_->add_coef(-30.0634102);        // default coefficient of v
00185   vpoly_->add_coef(2.14785486);         // default coefficient of v**2
00186   //vpoly_->configure(cf, section);     // fill in .cfg values
00187 #else
00188   c_.clear();                           // fill in volts2degrees()
00189   c_.push_back(52.25490152);            //  coefficients
00190   c_.push_back(-18.15450073);
00191   c_.push_back(6.2447116);
00192   c_.push_back(0.90281232);
00193   c_.push_back(0.65112384);
00194 #endif
00195 
00196   // use private node handle to get parameters
00197   ros::NodeHandle mynh("~");
00198   
00199   diagnostic_ = false;
00200   mynh.getParam("diagnostic", diagnostic_);
00201   if (diagnostic_)
00202     ROS_WARN("diagnostic mode ignored");
00203 
00204   // simulate sensor input parameter
00205   mynh.param("simulate", simulate_, false);
00206   if (simulate_)
00207     ROS_INFO("simulating position sensor");
00208 
00209   calibration_periods_ = 19;
00210   mynh.getParam("calibration_periods", calibration_periods_);
00211   ROS_INFO("calibrate steering sensor for %d cycles.", calibration_periods_);
00212 
00214   sensor_timeout_ = 2.0;
00215   mynh.getParam("sensor_timeout", sensor_timeout_);
00216   ROS_INFO("steering sensor timeout: %.3f seconds.", sensor_timeout_);
00217 
00218   // allocate and initialize the steering device interface
00219   dev_->Configure();
00220 
00221   // allocate and configure the steering wheel self-test
00222   tw_->Configure();
00223 
00224   // subscribe to relevant ROS topics
00225   static int qDepth = 1;
00226   ros::NodeHandle node;
00227   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00228   steering_cmd_ =
00229     node.subscribe("steering/cmd", qDepth, &ArtSteer::GetCmd, this, noDelay);
00230   steering_state_ =
00231     node.advertise<art_msgs::SteeringState>("steering/state", qDepth);
00232   steering_diag_ =
00233     node.advertise<art_msgs::SteeringDiagnostics>("steering/diag", qDepth);
00234   ioadr_state_ =
00235     node.subscribe("ioadr/state", qDepth, &ArtSteer::GetPos, this, noDelay);
00236 }
00237 
00238 ArtSteer::~ArtSteer()
00239 {
00240   if (driver_state_ != DriverState::CLOSED)
00241     {
00242       close();
00243     }
00244 }
00245 
00251 int ArtSteer::open()
00252 {
00253   last_sensor_time_ = cur_sensor_time_ = 0.0;
00254   last_set_point_ = set_point_ = 180.0; // preposterous starting value
00255   steering_angle_ = 0.0;                // needed for simulation
00256   calibration_cycle_ = 0;
00257 
00258   // open the device
00259   int rc = dev_->Open();
00260   if (rc == 0)
00261     {
00262       driver_state_ = DriverState::OPENED;
00263     }
00264   return rc;
00265 }
00266 
00271 void ArtSteer::close()
00272 {
00273   dev_->Close();
00274   driver_state_ = DriverState::CLOSED;
00275 }
00276 
00277 
00278 void ArtSteer::GetCmd(const art_msgs::SteeringCommand::ConstPtr &cmdIn)
00279 {
00280   switch (cmdIn->request)
00281     {
00282     case art_msgs::SteeringCommand::Degrees:
00283       ROS_DEBUG(" %.3f degrees (absolute) steering request", cmdIn->angle);
00284       set_point_ = limit_travel(cmdIn->angle);
00285       break;
00286     case art_msgs::SteeringCommand::Relative:
00287       // Should this option be supported at all?  Initially it will
00288       // give bogus results.
00289       ROS_DEBUG(" %.3f degrees (relative) steering request", cmdIn->angle);
00290       set_point_ = limit_travel(set_point_ + cmdIn->angle);
00291       break;
00292     default:
00293       {
00294         ROS_WARN("invalid steering request %u (ignored)", cmdIn->request);
00295       }
00296     }
00297 }
00298 
00299 void ArtSteer::GetPos(const art_msgs::IOadrState::ConstPtr &ioIn)
00300 {
00301   if (simulate_)                        // not using real sensor?
00302     return;
00303 
00304   // save steering position sensor voltage, convert to degrees
00305   steering_sensor_ = ioIn->voltages[0];
00306   steering_angle_ = limit_travel(volts2degrees(steering_sensor_));
00307   cur_sensor_time_ = ioIn->header.stamp.toSec();
00308   angle_known_ = true;
00309 }
00310 
00311 /* Calibrate current steering wheel position.
00312  *
00313  * Assumes the wheel will not move while in this state.  Unless
00314  * someone forgot to turn on the steering breaker, that will be
00315  * true. Otherwise, the wheel self-test should detect the error.
00316  *
00317  * @return 0 if success, < 0 if still calibrating, > 0 errno if failure
00318  *
00319  * @note This implementation minimizes the effects of sensor noise by
00320  * accumulating a moving average of the first "calibration_periods_"
00321  * readings.
00322  */
00323 int ArtSteer::calibrate_wheel_position(void)
00324 {
00325   int retval = -1;                      // still working
00326 
00327   if (cur_sensor_time_ > last_sensor_time_) // new sensor data this cycle?
00328     {
00329       if (calibration_cycle_++ == 0)    // first time?
00330           mean_voltage_ = 0.0;
00331 
00332       // accumulate moving average
00333       mean_voltage_ += ((steering_sensor_ - mean_voltage_)
00334                        / calibration_cycle_);
00335       ROS_DEBUG("cumulative steering sensor average: %.3f volts",
00336                 mean_voltage_);
00337 
00338       if (calibration_cycle_ >= calibration_periods_)
00339         {
00340           // finished with calibration
00341           steering_angle_ = limit_travel(volts2degrees(mean_voltage_));
00342           ROS_INFO("initial steering angle: %.2f degrees", steering_angle_);
00343           retval = dev_->set_initial_angle(steering_angle_);
00344         }
00345     }
00346   return retval;
00347 }
00348 
00359 void ArtSteer::read_wheel_angle(void)
00360 {
00361   if (simulate_)                        // not using real sensor?
00362     {
00363       int rc = dev_->get_angle(steering_angle_);
00364       if (rc == 0)                      // got the angle?
00365         {
00366           steering_sensor_ = degrees2volts(steering_angle_);
00367           cur_sensor_time_ = ros::Time::now().toSec();
00368           angle_known_ = true;
00369         }
00370     }
00371 }
00372 
00374 void ArtSteer::PublishStatus(void)
00375 {
00376   art_msgs::SteeringState msg;         // steering state message
00377 
00378   msg.driver.state = driver_state_;
00379   msg.angle = steering_angle_;
00380   msg.sensor = steering_sensor_;
00381   msg.header.stamp = ros::Time::now();
00382   steering_state_.publish(msg);
00383 
00384   if (driver_state_ != DriverState::CLOSED)
00385     {
00386       // publish driver diagnostic info
00387       // (TODO: use ROS diagnostics package)
00388       dev_->publish_diag(steering_diag_);
00389     }
00390 }
00391 
00392 // Main function for device thread
00393 // TODO rationalize these states and bits
00394 void ArtSteer::run() 
00395 {
00396   ros::Rate cycle(art_msgs::ArtHertz::STEERING); // set driver cycle rate
00397 
00398   while(ros::ok())
00399     {
00400       ros::spinOnce();                  // handle incoming commands
00401 
00402       switch (driver_state_)
00403         {
00404         case DriverState::CLOSED:
00405           {
00406             // TODO: spin slower while closed
00407             open();                     // try to open the device
00408             // state: CLOSED ==> OPENED (if successful)
00409             break;
00410           }
00411 
00412         case DriverState::OPENED:
00413           {
00414             read_wheel_angle();         // (may be simulated)
00415             if (wheel_tested_)          // wheel previously tested?
00416               {
00417                 // state: OPENED ==> RUNNING
00418                 driver_state_ = DriverState::RUNNING;
00419               }
00420             else if (wheel_calibrated_) // initial position known?
00421               {
00422                 int rc = tw_->Run(steering_angle_);
00423                 if (rc < 0)             // test failed?
00424                   {
00425                     ROS_ERROR("steering self-test failure: closing driver");
00426                     close();            // (sets state to CLOSED)
00427                   }
00428                 else if (rc > 0)       // test completed successfully?
00429                   {
00430                     wheel_tested_ = true;
00431                   }
00432               }
00433             else if (angle_known_)      // sensor data received?
00434               {
00435                 int rc = calibrate_wheel_position();
00436                 if (rc == 0)            // calibration succeeded?
00437                   {
00438                     wheel_calibrated_ = true;
00439                   }
00440                 else if (rc > 0)        // calibration failed?
00441                   {
00442                     ROS_ERROR("steering calibration failure: closing driver");
00443                     close();            // (sets state to CLOSED)
00444                   }
00445               }
00446             break;
00447           }
00448 
00449         case DriverState::RUNNING:
00450           {
00451             read_wheel_angle();         // (may be simulated)
00452             int rc = dev_->check_status();
00453             if (rc != 0)                // status bad?
00454               {
00455                 ROS_ERROR("bad steering status: closing driver");
00456                 close();                // (sets state to CLOSED)
00457               }
00458             else if (fabs(set_point_ - last_set_point_) > epsilon)
00459               {
00460                 // command steering position to match desired set point.
00461                 rc = dev_->steering_absolute(set_point_);
00462                 if (rc == 0)
00463                   last_set_point_ = set_point_;
00464               }
00465             break;
00466           }
00467         } // end switch on driver state
00468 
00469       PublishStatus();                  // publish current status
00470       last_sensor_time_ = cur_sensor_time_;
00471       cycle.sleep();                    // sleep until next cycle
00472     }
00473 }
00474   
00475 
00476 int main(int argc, char** argv)
00477 {
00478   ros::init(argc, argv, "steering");
00479   ArtSteer dvr;
00480   dvr.run();
00481 
00482   return 0;
00483 }


art_servo
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:09:12