dock_drive.cpp

Go to the documentation of this file.

/*
 * copyright (c) 2013, yujin robot.
 * all rights reserved.
 *
 * redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * neither the name of yujin robot nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * this software is provided by the copyright holders and contributors "as is"
 * and any express or implied warranties, including, but not limited to, the
 * implied warranties of merchantability and fitness for a particular purpose
 * are disclaimed. in no event shall the copyright owner or contributors be
 * liable for any direct, indirect, incidental, special, exemplary, or
 * consequential damages (including, but not limited to, procurement of
 * substitute goods or services; loss of use, data, or profits; or business
 * interruption) however caused and on any theory of liability, whether in
 * contract, strict liability, or tort (including negligence or otherwise)
 * arising in any way out of the use of this software, even if advised of the
 * possibility of such damage.
 */
/*****************************************************************************
** includes
*****************************************************************************/

#include "kobuki_dock_drive/dock_drive.hpp"

/*****************************************************************************
** defines
*****************************************************************************/

#define sign(x) (x>0?+1:x<0?-1:0)
#define stringfy(x) #x
#define setState(x) {state=x;}
#define setStateVel(x,v,w) {setState(x);setVel(v,w);}

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace kobuki {

/*****************************************************************************
** Implementation
*****************************************************************************/
DockDrive::DockDrive() :
  is_enabled(false), can_run(false)
  , state(RobotDockingState::IDLE), state_str("IDLE")
  , vx(0.0), wz(0.0)
  , bump_remainder(0)
  , dock_stabilizer(0)
  , dock_detector(0)
  , rotated(0.0)
  , min_abs_v(0.01)
  , min_abs_w(0.1)
  , signal_window(20)
  , ROBOT_STATE_STR(13)
{
  // Debug messages
  ROBOT_STATE_STR[0] = "IDLE";
  ROBOT_STATE_STR[1] = "DONE";
  ROBOT_STATE_STR[2] = "DOCKED_IN";
  ROBOT_STATE_STR[3] = "BUMPED_DOCK";
  ROBOT_STATE_STR[4] = "BUMPED";
  ROBOT_STATE_STR[5] = "SCAN";
  ROBOT_STATE_STR[6] = "FIND_STREAM";
  ROBOT_STATE_STR[7] = "GET_STREAM";
  ROBOT_STATE_STR[8] = "ALIGNED";
  ROBOT_STATE_STR[9] = "ALIGNED_FAR";
  ROBOT_STATE_STR[10] = "ALIGNED_NEAR";
  ROBOT_STATE_STR[11] = "UNKNOWN";
  ROBOT_STATE_STR[12] = "LOST";
}

DockDrive::~DockDrive(){;}

void DockDrive::setVel(double v, double w)
{
  vx = sign(v) * std::max(std::abs(v), min_abs_v);
  wz = sign(w) * std::max(std::abs(w), min_abs_w);
}

void DockDrive::modeShift(const std::string& mode)
{
  if (mode == "enable")  { is_enabled = true;  can_run = true; state = RobotDockingState::IDLE;}
  if (mode == "disable") { is_enabled = false; can_run = false; }
  if (mode == "run")  can_run = true;
  if (mode == "stop") can_run = false;
}


void DockDrive::update(const std::vector<unsigned char> &signal
                , const unsigned char &bumper
                , const unsigned char &charger
                , const ecl::LegacyPose2D<double>& pose) {

  ecl::LegacyPose2D<double> pose_update;
  std::vector<unsigned char> signal_filt(signal.size(), 0);
  std::string debug_str;

  // process bumper and charger event first
  // docking algorithm terminates here
  if(bumper || charger) {
    processBumpChargeEvent(bumper, charger);
  }
  else {
    computePoseUpdate(pose_update, pose);
    filterIRSensor(signal_filt, signal);
    updateVelocity(signal_filt, pose_update, debug_str);
  }

  velocityCommands(vx, wz);

  // for easy debugging
  generateDebugMessage(signal_filt, bumper, charger, pose_update, debug_str);

  return;
}

void DockDrive::computePoseUpdate(ecl::LegacyPose2D<double>& pose_update, const ecl::LegacyPose2D<double>& pose)
{
  double dx = pose.x() - pose_priv.x();
  double dy = pose.y() - pose_priv.y();
  pose_update.x( std::sqrt( dx*dx + dy*dy ) );
  pose_update.heading( pose.heading() - pose_priv.heading() );
  //std::cout << pose_diff << "=" << pose << "-" << pose_priv << " | " << pose_update << std::endl;
  pose_priv = pose;

}


void DockDrive::filterIRSensor(std::vector<unsigned char>& signal_filt,const std::vector<unsigned char> &signal)
{
  //dock_ir signals filtering
  past_signals.push_back(signal);
  while (past_signals.size() > signal_window) {
    past_signals.erase( past_signals.begin(), past_signals.begin() + past_signals.size() - signal_window);
  }

  for ( unsigned int i = 0; i < past_signals.size(); i++) {
    if (signal_filt.size() != past_signals[i].size())
      continue;
    for (unsigned int j = 0; j < signal_filt.size(); j++)
      signal_filt[j] |= past_signals[i][j];
  }
}


void DockDrive::velocityCommands(const double &vx_, const double &wz_) {
  // vx: in m/s
  // wz: in rad/s
  vx = vx_;
  wz = wz_;
}

/****************************************************
 * @brief process bumper and charge event. If robot is charging, terminates auto dokcing process. If it bumps something, Set the next state as bumped and go backward
 *
 * @bumper - indicates whether bumper has pressed
 * @charger - indicates whether robot is charging
 *
 ****************************************************/
void DockDrive::processBumpChargeEvent(const unsigned char& bumper, const unsigned char& charger) {
  RobotDockingState::State new_state;
  if(charger && bumper) {
    new_state = RobotDockingState::BUMPED_DOCK;
    setStateVel(new_state, -0.01, 0.0);
  }
  else if(charger) {
    if(dock_stabilizer++ == 0) {
      new_state = RobotDockingState::DOCKED_IN;
      setStateVel(new_state, 0.0, 0.0);
    }
    else if(dock_stabilizer > 20) {
      dock_stabilizer = 0;
      is_enabled = false;
      can_run = false;
      new_state = RobotDockingState::DONE;
      setStateVel(new_state, 0.0, 0.0);
    }
    else {
      new_state = RobotDockingState::DOCKED_IN;
      setStateVel(new_state, 0.0, 0.0);
    }
  }
  else if(bumper) {
    new_state = RobotDockingState::BUMPED;
    setStateVel(new_state, -0.05, 0.0);
    bump_remainder = 0;
  }
  state_str = ROBOT_STATE_STR[new_state];
}

/*************************
 * @breif processing. algorithms; transforma to velocity command
 *
 * @param dock_ir signal
 * @param bumper sensor
 * @param charger sensor
 * @param pose_update
 *
 *************************/
void DockDrive::updateVelocity(const std::vector<unsigned char>& signal_filt, const ecl::LegacyPose2D<double>& pose_update, std::string& debug_str)
{
  std::ostringstream oss;
  RobotDockingState::State current_state, new_state;
  double new_vx = 0.0;
  double new_wz = 0.0;

  // determine the current state based on ir and the previous state
  // common transition. idle -> scan -> find_stream -> get_stream -> scan -> aligned_far -> aligned_near -> docked_in -> done

  current_state = new_state = state;
  switch((unsigned int)current_state) {
    case RobotDockingState::IDLE:
      idle(new_state, new_vx, new_wz);
      break;
    case RobotDockingState::SCAN:
      scan(new_state, new_vx, new_wz, signal_filt, pose_update, debug_str);
      break;
    case RobotDockingState::FIND_STREAM:
      find_stream(new_state, new_vx, new_wz, signal_filt);
      break;
    case RobotDockingState::GET_STREAM:
      get_stream(new_state, new_vx, new_wz, signal_filt);
      break;
    case RobotDockingState::ALIGNED:
    case RobotDockingState::ALIGNED_FAR:
    case RobotDockingState::ALIGNED_NEAR:
      aligned(new_state, new_vx, new_wz, signal_filt, debug_str);
      break;
    case RobotDockingState::BUMPED:
      bumped(new_state, new_vx, new_wz, bump_remainder);
      break;
    default:
      oss << "Wrong state : " << current_state;
      debug_str = oss.str();
      break;
  }

  setStateVel(new_state, new_vx, new_wz);
  state_str = ROBOT_STATE_STR[new_state];
}

/*************************
 * @breif Check if any ir sees the given state signal from dock station
 *
 * @param filtered signal
 * @param dock ir state
 *
 * @ret true or false
 *************************/
bool DockDrive::validateSignal(const std::vector<unsigned char>& signal_filt, const unsigned int state)
{
  unsigned int i;
  for(i = 0; i < signal_filt.size(); i++)
  {
    if(signal_filt[i] & state)
      return true;
  }
  return false;
}

} // kobuki namespace