Public Member Functions | Protected Member Functions | Private Member Functions | Private Attributes
kobuki::DockDrive Class Reference

#include <dock_drive.hpp>

List of all members.

Public Member Functions

bool canRun () const
void disable ()
 DockDrive ()
void enable ()
std::string getDebugStr () const
std::string getDebugStream ()
RobotDockingState::State getState () const
std::string getStateStr () const
double getVX () const
double getWZ () const
bool init ()
bool isEnabled () const
void modeShift (const std::string &mode)
void setMinAbsV (double mav)
void setMinAbsW (double maw)
void update (const std::vector< unsigned char > &signal, const unsigned char &bumper, const unsigned char &charger, const ecl::LegacyPose2D< double > &pose)
 Updates the odometry from firmware stamps and encoders.
void velocityCommands (const double &vx, const double &wz)
 ~DockDrive ()

Protected Member Functions

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)
void computePoseUpdate (ecl::LegacyPose2D< double > &pose_update, const ecl::LegacyPose2D< double > &pose)
 compute pose update from previouse pose and current pose
RobotDockingState::State determineRobotLocation (const std::vector< unsigned char > &signal_filt, const unsigned char &charger)
void filterIRSensor (std::vector< unsigned char > &signal_filt, const std::vector< unsigned char > &signal)
void find_stream (RobotDockingState::State &state, double &vx, double &wz, const std::vector< unsigned char > &signal_filt)
void generateDebugMessage (const std::vector< unsigned char > &signal_filt, const unsigned char &bumper, const unsigned char &charger, const ecl::LegacyPose2D< double > &pose_update, const std::string &debug_str)
void get_stream (RobotDockingState::State &state, double &vx, double &wz, const std::vector< unsigned char > &signal_filt)
void idle (RobotDockingState::State &state, double &vx, double &wz)
void processBumpChargeEvent (const unsigned char &bumper, const unsigned char &charger)
void scan (RobotDockingState::State &state, double &vx, double &wz, const std::vector< unsigned char > &signal_filt, const ecl::LegacyPose2D< double > &pose_update, std::string &debug_str)
void updateVelocity (const std::vector< unsigned char > &signal_filt, const ecl::LegacyPose2D< double > &pose_update, std::string &debug_str)
bool validateSignal (const std::vector< unsigned char > &signal_filt, const unsigned int state)

Private Member Functions

std::string binary (unsigned char number) const
void setVel (double v, double w)

Private Attributes

int bump_remainder
bool can_run
std::string debug_output
std::string debug_str
int dock_detector
int dock_stabilizer
bool is_enabled
double min_abs_v
double min_abs_w
std::vector< std::vector
< unsigned char > > 
past_signals
ecl::LegacyPose2D< double > pose_priv
std::vector< std::string > ROBOT_STATE_STR
double rotated
unsigned int signal_window
RobotDockingState::State state
std::string state_str
double vx
double wz

Detailed Description

Definition at line 66 of file dock_drive.hpp.


Constructor & Destructor Documentation

Definition at line 58 of file dock_drive.cpp.

Definition at line 87 of file dock_drive.cpp.


Member Function Documentation

void kobuki::DockDrive::aligned ( RobotDockingState::State state,
double &  vx,
double &  wz,
const std::vector< unsigned char > &  signal_filt,
std::string &  debug_str 
) [protected]

Definition at line 250 of file dock_drive_states.cpp.

std::string kobuki::DockDrive::binary ( unsigned char  number) const [private]

Definition at line 100 of file dock_drive_debug.cpp.

void kobuki::DockDrive::bumped ( RobotDockingState::State nstate,
double &  nvx,
double &  nwz,
int &  bump_count 
) [protected]

Definition at line 323 of file dock_drive_states.cpp.

bool kobuki::DockDrive::canRun ( ) const [inline]

Definition at line 73 of file dock_drive.hpp.

void kobuki::DockDrive::computePoseUpdate ( ecl::LegacyPose2D< double > &  pose_update,
const ecl::LegacyPose2D< double > &  pose 
) [protected]

compute pose update from previouse pose and current pose

Parameters:
poseupdate. this variable get filled after this function
pose- current pose

Definition at line 148 of file dock_drive.cpp.

RobotDockingState::State kobuki::DockDrive::determineRobotLocation ( const std::vector< unsigned char > &  signal_filt,
const unsigned char &  charger 
) [protected]
void kobuki::DockDrive::disable ( ) [inline]

Definition at line 76 of file dock_drive.hpp.

void kobuki::DockDrive::enable ( ) [inline]

Definition at line 75 of file dock_drive.hpp.

void kobuki::DockDrive::filterIRSensor ( std::vector< unsigned char > &  signal_filt,
const std::vector< unsigned char > &  signal 
) [protected]

pushing into signal into signal window. and go through the signal window to find what has detected

Parameters:
signal_filt- this get filled out after the function.
signal- the raw data from robot

Definition at line 167 of file dock_drive.cpp.

void kobuki::DockDrive::find_stream ( RobotDockingState::State state,
double &  vx,
double &  wz,
const std::vector< unsigned char > &  signal_filt 
) [protected]

Definition at line 147 of file dock_drive_states.cpp.

void kobuki::DockDrive::generateDebugMessage ( const std::vector< unsigned char > &  signal_filt,
const unsigned char &  bumper,
const unsigned char &  charger,
const ecl::LegacyPose2D< double > &  pose_update,
const std::string &  debug_str 
) [protected]

Definition at line 48 of file dock_drive_debug.cpp.

void kobuki::DockDrive::get_stream ( RobotDockingState::State state,
double &  vx,
double &  wz,
const std::vector< unsigned char > &  signal_filt 
) [protected]

Definition at line 198 of file dock_drive_states.cpp.

std::string kobuki::DockDrive::getDebugStr ( ) const [inline]

Definition at line 97 of file dock_drive.hpp.

std::string kobuki::DockDrive::getDebugStream ( ) [inline]

Definition at line 106 of file dock_drive.hpp.

Definition at line 95 of file dock_drive.hpp.

std::string kobuki::DockDrive::getStateStr ( ) const [inline]

Definition at line 96 of file dock_drive.hpp.

double kobuki::DockDrive::getVX ( ) const [inline]

Definition at line 89 of file dock_drive.hpp.

double kobuki::DockDrive::getWZ ( ) const [inline]

Definition at line 90 of file dock_drive.hpp.

void kobuki::DockDrive::idle ( RobotDockingState::State state,
double &  vx,
double &  wz 
) [protected]

Definition at line 61 of file dock_drive_states.cpp.

bool kobuki::DockDrive::init ( ) [inline]

Definition at line 71 of file dock_drive.hpp.

bool kobuki::DockDrive::isEnabled ( ) const [inline]

Definition at line 72 of file dock_drive.hpp.

void kobuki::DockDrive::modeShift ( const std::string &  mode)

Definition at line 95 of file dock_drive.cpp.

void kobuki::DockDrive::processBumpChargeEvent ( const unsigned char &  bumper,
const unsigned char &  charger 
) [protected]

Definition at line 198 of file dock_drive.cpp.

void kobuki::DockDrive::scan ( RobotDockingState::State state,
double &  vx,
double &  wz,
const std::vector< unsigned char > &  signal_filt,
const ecl::LegacyPose2D< double > &  pose_update,
std::string &  debug_str 
) [protected]

Definition at line 78 of file dock_drive_states.cpp.

void kobuki::DockDrive::setMinAbsV ( double  mav) [inline]

Definition at line 102 of file dock_drive.hpp.

void kobuki::DockDrive::setMinAbsW ( double  maw) [inline]

Definition at line 103 of file dock_drive.hpp.

void kobuki::DockDrive::setVel ( double  v,
double  w 
) [private]

Definition at line 89 of file dock_drive.cpp.

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

Updates the odometry from firmware stamps and encoders.

Really horrible - could do with an overhaul.

Parameters:
dock_irsignal
bumpersensor
chargersensor
currentpose

Definition at line 114 of file dock_drive.cpp.

void kobuki::DockDrive::updateVelocity ( const std::vector< unsigned char > &  signal_filt,
const ecl::LegacyPose2D< double > &  pose_update,
std::string &  debug_str 
) [protected]

Definition at line 238 of file dock_drive.cpp.

bool kobuki::DockDrive::validateSignal ( const std::vector< unsigned char > &  signal_filt,
const unsigned int  state 
) [protected]

Definition at line 288 of file dock_drive.cpp.

void kobuki::DockDrive::velocityCommands ( const double &  vx,
const double &  wz 
)

Definition at line 184 of file dock_drive.cpp.


Member Data Documentation

Definition at line 139 of file dock_drive.hpp.

Definition at line 132 of file dock_drive.hpp.

std::string kobuki::DockDrive::debug_output [private]

Definition at line 151 of file dock_drive.hpp.

std::string kobuki::DockDrive::debug_str [private]

Definition at line 135 of file dock_drive.hpp.

Definition at line 141 of file dock_drive.hpp.

Definition at line 140 of file dock_drive.hpp.

Definition at line 132 of file dock_drive.hpp.

double kobuki::DockDrive::min_abs_v [private]

Definition at line 143 of file dock_drive.hpp.

double kobuki::DockDrive::min_abs_w [private]

Definition at line 144 of file dock_drive.hpp.

std::vector<std::vector<unsigned char> > kobuki::DockDrive::past_signals [private]

Definition at line 137 of file dock_drive.hpp.

Definition at line 145 of file dock_drive.hpp.

std::vector<std::string> kobuki::DockDrive::ROBOT_STATE_STR [private]

Definition at line 152 of file dock_drive.hpp.

double kobuki::DockDrive::rotated [private]

Definition at line 142 of file dock_drive.hpp.

unsigned int kobuki::DockDrive::signal_window [private]

Definition at line 138 of file dock_drive.hpp.

Definition at line 134 of file dock_drive.hpp.

std::string kobuki::DockDrive::state_str [private]

Definition at line 135 of file dock_drive.hpp.

double kobuki::DockDrive::vx [private]

Definition at line 136 of file dock_drive.hpp.

double kobuki::DockDrive::wz [private]

Definition at line 136 of file dock_drive.hpp.


The documentation for this class was generated from the following files:


kobuki_dock_drive
Author(s): Younghun Ju
autogenerated on Thu Jun 6 2019 20:24:33