dock_drive.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Yujin Robot.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Yujin Robot nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00034 /*****************************************************************************
00035 ** Includes
00036 *****************************************************************************/
00037 
00038 #include "kobuki_auto_docking/dock_drive.hpp"
00039 
00040 /*****************************************************************************
00041 ** Defines
00042 *****************************************************************************/
00043 
00044 #define stringfy(x) #x
00045 #define setState(x) {state=x;state_str=stringfy(x);}
00046 #define setVel(v,w) {vx=v;wz=w;}
00047 #define setStateVel(x,v,w) {setState(x);setVel(v,w);}
00048 
00049 /*****************************************************************************
00050 ** Namespaces
00051 *****************************************************************************/
00052 
00053 namespace kobuki {
00054 
00055 /*****************************************************************************
00056 ** Implementation
00057 *****************************************************************************/
00058 DockDrive::DockDrive() :
00059   is_enabled(false), can_run(false) 
00060   , state(IDLE), state_str("IDLE")
00061   , vx(0.0), wz(0.0)
00062   , bump_remainder(0)
00063   , dock_stabilizer(0)
00064   , dock_detector(0)
00065   , rotated(0.0)
00066 {
00067   pose.setIdentity();
00068 }
00069 
00070 DockDrive::~DockDrive(){;}
00071 
00072 
00073 void DockDrive::modeShift(const std::string& mode)
00074 {
00075   if (mode == "enable")  { is_enabled = true;  can_run = true; }
00076   if (mode == "disable") { is_enabled = false; can_run = false; }
00077   if (mode == "run")  can_run = true;
00078   if (mode == "stop") can_run = false;
00079 }
00080 
00081 void DockDrive::update(const std::vector<unsigned char> &signal
00082                 , const unsigned char &bumper
00083                 , const unsigned char &charger
00084                 , const ecl::Pose2D<double> &pose) {
00085   static ecl::Pose2D<double> pose_priv;
00086   ecl::Pose2D<double> pose_update;
00087 
00088   double dx = pose.x() - pose_priv.x();
00089   double dy = pose.y() - pose_priv.y();
00090   pose_update.x( std::sqrt( dx*dx + dy*dy ) );
00091   pose_update.heading( pose.heading() - pose_priv.heading() );
00092   //std::cout << pose_diff << "=" << pose << "-" << pose_priv << " | " << pose_update << std::endl;
00093   pose_priv = pose;
00094 
00095   ecl::linear_algebra::Vector3d pose_update_rates; //dummy
00096   update( signal, bumper, charger, pose_update, pose_update_rates );
00097 }
00098 
00099 
00111 void DockDrive::update(const std::vector<unsigned char> &signal
00112                 , const unsigned char &bumper
00113                 , const unsigned char &charger
00114                 , const ecl::Pose2D<double> &pose_update
00115                 , const ecl::linear_algebra::Vector3d &pose_update_rates) {
00116   /*************************
00117    * pre processing
00118    *************************/
00119   //pose update
00120   pose *= pose_update;
00121 
00122   //dock_ir signals filtering
00123   past_signals.push_back(signal);
00124   unsigned int window = 20;
00125   while (past_signals.size() > window)
00126     past_signals.erase( past_signals.begin(), past_signals.begin() + past_signals.size() - window );
00127 
00128   std::vector<unsigned char> signal_filt(signal.size(), 0);
00129   for ( unsigned int i=0; i<past_signals.size(); i++) {
00130     if (signal_filt.size() != past_signals[i].size()) continue;
00131     for (unsigned int j=0; j<signal_filt.size(); j++)
00132       signal_filt[j] |= past_signals[i][j];
00133   }
00134 
00135 
00136   /*************************
00137    * debug prints
00138    *************************/
00139   std::ostringstream debug_stream;
00140   // pose
00141   debug_stream << pose;
00142   // pose_update and pose_update_rates for debugging
00143   debug_stream << std::fixed << std::setprecision(4)
00144     << "[x: "    << std::setw(7) << pose_update.x()
00145     << ", y: "  << std::setw(7) << pose_update.y()
00146     << ", th: " << std::setw(7) << pose_update.heading()
00147     << "]";
00148 
00149   //dock_ir signal
00150   /*
00151   debug_stream 
00152     << "[l: "  << binary(signal_filt[2])
00153     << ", c: " << binary(signal_filt[1])
00154     << ", r: " << binary(signal_filt[0])
00155     << "]";
00156   */
00157   std::string far_signal  = "[F: "; //far field
00158   std::string near_signal = "[N: "; //near field
00159   for (unsigned int i=0; i<3; i++) {
00160     if (signal_filt[2-i]&FAR_LEFT   ) far_signal  += "L"; else far_signal  += "-";
00161     if (signal_filt[2-i]&FAR_CENTER ) far_signal  += "C"; else far_signal  += "-";
00162     if (signal_filt[2-i]&FAR_RIGHT  ) far_signal  += "R"; else far_signal  += "-";
00163     if (signal_filt[2-i]&NEAR_LEFT  ) near_signal += "L"; else near_signal += "-";
00164     if (signal_filt[2-i]&NEAR_CENTER) near_signal += "C"; else near_signal += "-";
00165     if (signal_filt[2-i]&NEAR_RIGHT ) near_signal += "R"; else near_signal += "-";
00166     far_signal  += " ";
00167     near_signal += " ";
00168   }
00169   far_signal  += "]";
00170   near_signal += "]";
00171   debug_stream << far_signal << near_signal;
00172 
00173   //bumper
00174   {
00175   std::string out = "[B: ";
00176   if (bumper&4) out += "L"; else out += "-";
00177   if (bumper&2) out += "C"; else out += "-";
00178   if (bumper&1) out += "R"; else out += "-";
00179   out += "]";
00180   debug_stream << out;
00181   }
00182 
00183   //charger
00184   {
00185   std::ostringstream oss;
00186   oss << "[C:" << std::setw(2) << (unsigned int)charger;
00187   oss << "(";
00188   if (charger) oss << "ON"; else oss << "  ";
00189   oss << ")]";
00190   debug_stream << oss.str();
00191   }
00192 
00193 
00194   /*************************
00195    * processing. algorithms; transforma to velocity command
00196    *************************/
00197   //std::string debug_str = "";
00198   debug_str = "";
00199   do {  // a kind of hack
00200     if ( state==DONE ) setState(IDLE); // when this function is called after final state 'DONE'.
00201     if ( state==DOCKED_IN ) {
00202       if ( dock_stabilizer++ > 20 ) {
00203         is_enabled = false;
00204         can_run = false;
00205         setStateVel(DONE, 0.0, 0.0); break;
00206       }
00207       setStateVel(DOCKED_IN, 0.0, 0.0); break;
00208     }
00209     if ( bump_remainder > 0 ) {
00210       bump_remainder--;
00211       if ( charger ) { setStateVel(DOCKED_IN, 0.0, 0.0); break; } // when bumper signal is received early than charger(dock) signal.
00212       else {           setStateVel(BUMPED_DOCK, -0.01, 0.0); break; }
00213     } else if (state == BUMPED) {
00214       setState(IDLE); //should I remember and recall previous state?
00215       debug_str="how dare!!";
00216     }
00217     if ( bumper || charger ) {
00218       if( bumper && charger ) {
00219         bump_remainder = 0;
00220         setStateVel(BUMPED_DOCK, -0.01, 0.0); break;
00221       }
00222       if ( bumper ) {
00223         bump_remainder = 50;
00224         setStateVel(BUMPED, -0.05, 0.0); break;
00225       }
00226       if ( charger ) { // already docked in
00227         dock_stabilizer = 0;
00228         setStateVel(DOCKED_IN, 0.0, 0.0); break;
00229       }
00230     } else {
00231       if ( state==IDLE ) {
00232         dock_detector = 0;
00233         rotated = 0.0;
00234         setStateVel(SCAN, 0.00, 0.66); break;
00235       }
00236       if ( state==SCAN ) {
00237         rotated += pose_update.heading()/(2.0*M_PI);
00238         std::ostringstream oss;
00239         oss << "rotated: " << std::fixed << std::setprecision(2) << std::setw(4) << rotated;
00240         debug_str = oss.str();
00241         if( std::abs(rotated) > 1.6 ) {
00242           setStateVel(FIND_STREAM, 0.0, 0.0); break;
00243         }
00244         if (  signal_filt[1]&(FAR_LEFT  + NEAR_LEFT )) dock_detector--;
00245         if (  signal_filt[1]&(FAR_RIGHT + NEAR_RIGHT)) dock_detector++;
00246         if ( (signal_filt[1]&FAR_CENTER) || (signal_filt[1]&NEAR_CENTER) ) {
00247           setStateVel(ALIGNED, 0.05, 0.00); break;
00248         } else if ( signal_filt[1] ) {
00249           setStateVel(SCAN, 0.00, 0.10); break;
00250         } else {
00251           setStateVel(SCAN, 0.00, 0.66); break;
00252         }
00253 
00254       } else if (state==ALIGNED || state==ALIGNED_FAR || state==ALIGNED_NEAR) {
00255         if ( signal_filt[1] ) {
00256           if ( signal_filt[1]&NEAR )
00257           {
00258             if ( ((signal_filt[1]&NEAR) == NEAR_CENTER) || ((signal_filt[1]&NEAR) == NEAR) ) { setStateVel(ALIGNED_NEAR, 0.05,  0.0); debug_str = "AlignedNearCenter"; break; }
00259             if (   signal_filt[1]&NEAR_LEFT  ) {                                               setStateVel(ALIGNED_NEAR, 0.05,  0.1); debug_str = "AlignedNearLeft"  ; break; }
00260             if (   signal_filt[1]&NEAR_RIGHT ) {                                               setStateVel(ALIGNED_NEAR, 0.05, -0.1); debug_str = "AlignedNearRight" ; break; }
00261           }
00262           if ( signal_filt[1]&FAR )
00263           {
00264             if ( ((signal_filt[1]&FAR) == FAR_CENTER) || ((signal_filt[1]&FAR) == FAR) ) { setStateVel(ALIGNED_FAR, 0.1,  0.0); debug_str = "AlignedFarCenter"; break; }
00265             if (   signal_filt[1]&FAR_LEFT  ) {                                            setStateVel(ALIGNED_FAR, 0.1,  0.3); debug_str = "AlignedFarLeft"  ; break; }
00266             if (   signal_filt[1]&FAR_RIGHT ) {                                            setStateVel(ALIGNED_FAR, 0.1, -0.3); debug_str = "AlignedFarRight" ; break; }
00267           }
00268           dock_detector = 0;
00269           rotated = 0.0;
00270           setStateVel(SCAN, 0.00, 0.66); break;
00271         } else {
00272           debug_str = "lost signals";
00273           setStateVel(LOST, 0.00, 0.00); break;
00274         }
00275       } else if (state==FIND_STREAM) {
00276         if (dock_detector > 0 ) { // robot is placed in right side of docking station
00277           //turn  right , negative direction til get right signal from left sensor
00278           if (signal_filt[2]&(FAR_RIGHT+NEAR_RIGHT)) {
00279             setStateVel(GET_STREAM, 0.05, 0.0); break;
00280           } else {
00281             setStateVel(FIND_STREAM, 0.0, -0.33); break;
00282           }
00283         } else if (dock_detector < 0 ) { // robot is placed in left side of docking station
00284           //turn left, positive direction till get left signal from right sensor
00285           if (signal_filt[0]&(FAR_LEFT+NEAR_LEFT)) {
00286             setStateVel(GET_STREAM, 0.05, 0.0); break;
00287           } else {
00288             setStateVel(FIND_STREAM, 0.0, 0.33); break;
00289           }
00290         }
00291       } else if (state==GET_STREAM) {
00292         if (dock_detector > 0) { //robot is placed in right side of docking station
00293           if (signal_filt[2]&(FAR_LEFT+NEAR_LEFT)) {
00294             dock_detector = 0;
00295             rotated = 0.0;
00296             setStateVel(SCAN, 0.0, 0.10); break;
00297           } else {
00298             setStateVel(GET_STREAM, 0.05, 0.0); break;
00299           }
00300         } else if (dock_detector < 0) { // robot is placed in left side of docking station
00301           if (signal_filt[0]&(FAR_RIGHT+NEAR_RIGHT)) {
00302             dock_detector = 0;
00303             rotated = 0.0;
00304             setStateVel(SCAN, 0.0, 0.10); break;
00305           } else {
00306             setStateVel(GET_STREAM, 0.05, 0.0); break;
00307           }
00308         }
00309       } else {
00310         dock_detector = 0;
00311         rotated = 0.0;
00312         setStateVel(SCAN, 0.00, 0.66); break;
00313       }
00314     }
00315     setStateVel(UNKNOWN, 0.00, 0.00); break;
00316   } while(0);
00317 
00318   //debug_stream << std::fixed << std::setprecision(4)
00319   debug_stream << "[vx: " << std::setw(7) << vx << ", wz: " << std::setw(7) << wz << "]";
00320   debug_stream << "[S: " << state_str << "]";
00321   debug_stream << "[" << debug_str << "]";
00322   //debug_stream << std::endl;
00323   debug_output = debug_stream.str();
00324 
00325   //std::cout << debug_output << std::endl;;
00326 
00327   velocityCommands(vx, wz);
00328 //  this->vx = vx; this->wz = wz;
00329   return;
00330 }
00331 
00332 void DockDrive::velocityCommands(const double &vx_, const double &wz_) {
00333   // vx: in m/s
00334   // wz: in rad/s
00335   vx = vx_;
00336   wz = wz_;
00337 }
00338 
00339 std::string DockDrive::binary(unsigned char number) const {
00340   std::string ret;
00341   for( unsigned int i=0;i<6; i++){
00342     if (number&1) ret = "1" + ret;
00343     else          ret = "0" + ret;
00344     number = number >> 1;
00345   }
00346   return ret;
00347 }
00348 
00349 
00350 } // namespace kobuki


kobuki_auto_docking
Author(s): Younghun Ju
autogenerated on Mon Oct 6 2014 01:30:37