Program Listing for File dock_drive.hpp
↰ Return to documentation for file (include/kobuki_core/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_ */