dock_drive.hpp

 
/*****************************************************************************
** Ifdefs
*****************************************************************************/
 
#ifndef KOBUKI_CORE_DOCK_DRIVE_HPP_
#define KOBUKI_CORE_DOCK_DRIVE_HPP_
 
/*****************************************************************************
** Includes
*****************************************************************************/
 
#include <iostream>
#include <sstream>
#include <iomanip>
#include <cmath>
#include <vector>
// Version 3.4.0 of Eigen in Ubuntu 22.04 has a bug that causes -Wclass-memaccess warnings on
// aarch64.  Upstream Eigen has already fixed this in
// https://gitlab.com/libeigen/eigen/-/merge_requests/645 .  The Debian fix for this is in
// https://salsa.debian.org/science-team/eigen3/-/merge_requests/1 .
// However, it is not clear that that fix is going to make it into Ubuntu 22.04 before it
// freezes, so disable the warning here.
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wclass-memaccess"
#endif
#include <ecl/geometry.hpp>
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif
 
#include <ecl/linear_algebra.hpp>
 
/*****************************************************************************
** Namespaces
*****************************************************************************/
 
namespace kobuki {
 
/*****************************************************************************
** Definitions
*****************************************************************************/
 
// indicates the ir sensor from docking station
struct DockStationIRState {
  enum State {
    INVISIBLE=0,
    NEAR_LEFT=1,
    NEAR_CENTER=2,
    NEAR_RIGHT=4,
    FAR_CENTER=8,
    FAR_LEFT=16,
    FAR_RIGHT=32,
    NEAR = 7, // NEAR_LEFT + NEAR_CENTER + NEAR_RIGHT
    FAR = 56, // FAR_CENTER + FAR_LEFT + FAR_RIGHT
  };
};
 
// the current robot states
struct RobotDockingState {
  enum State {
    IDLE,
    DONE,
    DOCKED_IN,
    BUMPED_DOCK,
    BUMPED,
    SCAN,
    FIND_STREAM,
    GET_STREAM,
    ALIGNED,
    ALIGNED_FAR,
    ALIGNED_NEAR,
    UNKNOWN,
    LOST
  };
 
};
 
/*****************************************************************************
** Interfaces
*****************************************************************************/
 
class DockDrive {
public:
  DockDrive();
  ~DockDrive();
 
  bool init(){ return true; }
  bool isEnabled() const { return is_enabled; }
  bool canRun() const { return can_run; }
 
  void enable() { modeShift("enable"); }
  void disable() { modeShift("disable"); }
  void modeShift(const std::string& mode);
 
  void update(const std::vector<unsigned char> &signal /* dock_ir signal*/
                , const unsigned char &bumper
                , const unsigned char &charger
                , const ecl::linear_algebra::Vector3d &pose);
 
  void velocityCommands(const double &vx, const double &wz);
 
  /*********************
  ** Command Accessors
  **********************/
  double getVX() const { return vx; }
  double getWZ() const { return wz; }
 
  /*********************
  ** Mode Accessors
  **********************/
  RobotDockingState::State getState() const { return state; }
  std::string getStateStr() const { return state_str; }
  std::string getDebugStr() const { return debug_str; }
 
  /*********************
  ** Parameters Mutators
  **********************/
  void setMinAbsV(double mav) { min_abs_v = mav; }
  void setMinAbsW(double maw) { min_abs_w = maw; }
 
  //debugging
  std::string getDebugStream() { return debug_output; } //stream.str(); }
  //std::string getDebugStream() { return debug_stream.str(); }
  //std::ostringstream debug_stream;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  void processBumpChargeEvent(const unsigned char& bumper, const unsigned char& charger);
  void computePoseUpdate(ecl::linear_algebra::Vector3d& pose_update, const ecl::linear_algebra::Vector3d& pose);
  void filterIRSensor(std::vector<unsigned char>& signal_filt,const std::vector<unsigned char> &signal );
  void generateDebugMessage(const std::vector<unsigned char>& signal_filt, const unsigned char &bumper, const unsigned char &charger, const std::string& debug_str);
  void updateVelocity(const std::vector<unsigned char>& signal_filt, const ecl::linear_algebra::Vector3d& pose_update, std::string& debug_str);
  RobotDockingState::State determineRobotLocation(const std::vector<unsigned char>& signal_filt,const unsigned char& charger);
  bool validateSignal(const std::vector<unsigned char>& signal_filt, const unsigned int state);
 
 
  // States
  void idle(RobotDockingState::State& state,double& vx, double& wz);
  void scan(RobotDockingState::State& state,double& vx, double& wz, const std::vector<unsigned char>& signal_filt, const ecl::linear_algebra::Vector3d& pose_update, std::string& debug_str);
  void find_stream(RobotDockingState::State& state,double& vx, double& wz, const std::vector<unsigned char>& signal_filt);
  void get_stream(RobotDockingState::State& state,double& vx, double& wz, const std::vector<unsigned char>& signal_filt);
  void aligned(RobotDockingState::State& state,double& vx, double& wz, const std::vector<unsigned char>& signal_filt, std::string& debug_str);
  void bumped(RobotDockingState::State& nstate,double& nvx, double& nwz, int& bump_count);
 
 
private:
  bool is_enabled, can_run;
 
  RobotDockingState::State state;
  std::string state_str, debug_str;
  double vx, wz;
  std::vector<std::vector<unsigned char> > past_signals;
  unsigned int signal_window;
  int bump_remainder;
  int dock_stabilizer;
  int dock_detector;
  double rotated;
  double min_abs_v;
  double min_abs_w;
  ecl::linear_algebra::Vector3d pose_priv;
 
  void setVel(double v, double w);
 
  std::string binary(unsigned char number) const;
 
  std::string debug_output;
  std::vector<std::string> ROBOT_STATE_STR;
};
 
} // namespace kobuki
 
#endif /* KOBUKI_CORE_DOCK_DRIVE_HPP_ */