The core kobuki driver class. More...
#include <kobuki.hpp>
Public Member Functions | |
Battery | batteryStatus () const |
bool | disable () |
bool | enable () |
void | fixPayload (ecl::PushAndPop< unsigned char > &byteStream) |
double | getAngularVelocity () const |
Cliff::Data | getCliffData () const |
bool | getControllerGain () |
ControllerInfo::Data | getControllerInfoData () const |
CoreSensors::Data | getCoreSensorData () const |
Current::Data | getCurrentData () const |
DockIR::Data | getDockIRData () const |
GpInput::Data | getGpInputData () const |
ecl::Angle< double > | getHeading () const |
Inertia::Data | getInertiaData () const |
ThreeAxisGyro::Data | getRawInertiaData () const |
void | getWheelJointStates (double &wheel_left_angle, double &wheel_left_angle_rate, double &wheel_right_angle, double &wheel_right_angle_rate) |
void | init (Parameters ¶meters) |
Kobuki () | |
void | lockDataAccess () |
void | playSoundSequence (const enum SoundSequences &number) |
void | printSigSlotConnections () const |
Print a list of all relevant sigslot connections. More... | |
void | resetOdometry () |
void | setBaseControl (const double &linear_velocity, const double &angular_velocity) |
bool | setControllerGain (const unsigned char &type, const unsigned int &p_gain, const unsigned int &i_gain, const unsigned int &d_gain) |
void | setDigitalOutput (const DigitalOutput &digital_output) |
void | setExternalPower (const DigitalOutput &digital_output) |
void | setLed (const enum LedNumber &number, const enum LedColour &colour) |
void | spin () |
Performs a scan looking for incoming data packets. More... | |
void | unlockDataAccess () |
void | updateOdometry (ecl::LegacyPose2D< double > &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates) |
Use the current sensor data (encoders and gyro) to calculate an update for the odometry. More... | |
VersionInfo | versionInfo () const |
~Kobuki () | |
Private Member Functions | |
std::vector< std::string > | log (std::string level, std::string msg) |
std::vector< std::string > | log (std::string level, std::string name, std::string msg) |
std::vector< std::string > | log (std::string msg) |
void | sendBaseControlCommand () |
void | sendCommand (Command command) |
Send the prepared command to the serial port. More... | |
The core kobuki driver class.
This connects to the outside world via sigslots and get accessors.
Definition at line 95 of file kobuki.hpp.
kobuki::Kobuki::Kobuki | ( | ) |
Definition at line 55 of file kobuki.cpp.
kobuki::Kobuki::~Kobuki | ( | ) |
Shutdown the driver - make sure we wait for the thread to finish.
Definition at line 70 of file kobuki.cpp.
|
inline |
Definition at line 134 of file kobuki.hpp.
bool kobuki::Kobuki::disable | ( | ) |
Disable power to the motors.
Definition at line 606 of file kobuki.cpp.
bool kobuki::Kobuki::enable | ( | ) |
Enable power to the motors.
Definition at line 600 of file kobuki.cpp.
void kobuki::Kobuki::fixPayload | ( | ecl::PushAndPop< unsigned char > & | byteStream | ) |
Definition at line 391 of file kobuki.cpp.
double kobuki::Kobuki::getAngularVelocity | ( | ) | const |
Definition at line 438 of file kobuki.cpp.
|
inline |
Definition at line 143 of file kobuki.hpp.
bool kobuki::Kobuki::getControllerGain | ( | ) |
Definition at line 521 of file kobuki.cpp.
|
inline |
Definition at line 148 of file kobuki.hpp.
|
inline |
Definition at line 141 of file kobuki.hpp.
|
inline |
Definition at line 144 of file kobuki.hpp.
|
inline |
Definition at line 142 of file kobuki.hpp.
|
inline |
Definition at line 146 of file kobuki.hpp.
ecl::Angle< double > kobuki::Kobuki::getHeading | ( | ) | const |
Definition at line 430 of file kobuki.cpp.
|
inline |
Definition at line 145 of file kobuki.hpp.
|
inline |
Definition at line 147 of file kobuki.hpp.
void kobuki::Kobuki::getWheelJointStates | ( | double & | wheel_left_angle, |
double & | wheel_left_angle_rate, | ||
double & | wheel_right_angle, | ||
double & | wheel_right_angle_rate | ||
) |
Definition at line 456 of file kobuki.cpp.
void kobuki::Kobuki::init | ( | Parameters & | parameters | ) |
Definition at line 78 of file kobuki.cpp.
void kobuki::Kobuki::lockDataAccess | ( | ) |
Usually you should call the getXXX functions from within slot callbacks connected to this driver's signals. This ensures that data is not overwritten inbetween getXXX calls as it all happens in the serial device's reading thread (aye, convoluted - apologies for the multiple robot and multiple developer adhoc hacking over 4-5 years for hasty demos on pre-kobuki robots. This has generated such wonderful spaghetti ;).
If instead you just want to poll kobuki, then you should lock and unlock the data access around any getXXX calls.
Definition at line 160 of file kobuki.cpp.
|
inlineprivate |
Definition at line 255 of file kobuki.hpp.
|
inlineprivate |
Definition at line 256 of file kobuki.hpp.
|
inlineprivate |
Definition at line 254 of file kobuki.hpp.
void kobuki::Kobuki::playSoundSequence | ( | const enum SoundSequences & | number | ) |
Definition at line 501 of file kobuki.cpp.
void kobuki::Kobuki::printSigSlotConnections | ( | ) | const |
Print a list of all relevant sigslot connections.
This includes both the kobuki driver signals as well as externally connected slots. Useful for when you need to check if any of your connections are dangling (often happens when you typo the name of the sigslots connection).
Definition at line 622 of file kobuki.cpp.
void kobuki::Kobuki::resetOdometry | ( | ) |
Definition at line 448 of file kobuki.cpp.
|
private |
Definition at line 540 of file kobuki.cpp.
|
private |
Send the prepared command to the serial port.
Need to be a bit careful here, because we have no control over how the user is calling this - they may be calling from different threads (this is so for kobuki_node), so we mutex protect it here rather than relying on the user to do so above.
command | : prepared command template (see Command's static member functions). |
Definition at line 570 of file kobuki.cpp.
void kobuki::Kobuki::setBaseControl | ( | const double & | linear_velocity, |
const double & | angular_velocity | ||
) |
Definition at line 535 of file kobuki.cpp.
bool kobuki::Kobuki::setControllerGain | ( | const unsigned char & | type, |
const unsigned int & | p_gain, | ||
const unsigned int & | i_gain, | ||
const unsigned int & | d_gain | ||
) |
Definition at line 506 of file kobuki.cpp.
void kobuki::Kobuki::setDigitalOutput | ( | const DigitalOutput & | digital_output | ) |
Definition at line 488 of file kobuki.cpp.
void kobuki::Kobuki::setExternalPower | ( | const DigitalOutput & | digital_output | ) |
Definition at line 492 of file kobuki.cpp.
Definition at line 483 of file kobuki.cpp.
void kobuki::Kobuki::spin | ( | ) |
Performs a scan looking for incoming data packets.
Sits on the device waiting for incoming and then parses it, and signals that an update has occured.
Or, if in simulation, just loopsback the motor devices.
Definition at line 181 of file kobuki.cpp.
void kobuki::Kobuki::unlockDataAccess | ( | ) |
Unlock a previously locked data access privilege.
Definition at line 168 of file kobuki.cpp.
void kobuki::Kobuki::updateOdometry | ( | ecl::LegacyPose2D< double > & | pose_update, |
ecl::linear_algebra::Vector3d & | pose_update_rates | ||
) |
Use the current sensor data (encoders and gyro) to calculate an update for the odometry.
This fuses current sensor data with the last updated odometry state to produce the new odometry state. This will be usually done in the slot callback to the stream_data signal.
It is important that this is called every time a data packet is received from the robot.
pose_update | : return the pose updates in this variable. |
pose_update_rates | : return the pose update rates in this variable. |
Definition at line 473 of file kobuki.cpp.
|
inline |
Definition at line 133 of file kobuki.hpp.
|
private |
Definition at line 207 of file kobuki.hpp.
|
private |
Definition at line 215 of file kobuki.hpp.
|
private |
Definition at line 243 of file kobuki.hpp.
|
private |
Definition at line 237 of file kobuki.hpp.
|
private |
Definition at line 222 of file kobuki.hpp.
|
private |
Definition at line 230 of file kobuki.hpp.
|
private |
Definition at line 212 of file kobuki.hpp.
|
private |
Definition at line 216 of file kobuki.hpp.
|
private |
Definition at line 226 of file kobuki.hpp.
|
private |
Definition at line 241 of file kobuki.hpp.
|
private |
Definition at line 190 of file kobuki.hpp.
|
private |
Definition at line 214 of file kobuki.hpp.
|
private |
Definition at line 249 of file kobuki.hpp.
|
private |
Definition at line 219 of file kobuki.hpp.
|
private |
Definition at line 217 of file kobuki.hpp.
|
private |
Definition at line 218 of file kobuki.hpp.
|
private |
Definition at line 196 of file kobuki.hpp.
|
private |
Definition at line 213 of file kobuki.hpp.
|
private |
Definition at line 227 of file kobuki.hpp.
|
private |
Definition at line 202 of file kobuki.hpp.
|
private |
Definition at line 191 of file kobuki.hpp.
|
private |
Definition at line 242 of file kobuki.hpp.
|
private |
Definition at line 225 of file kobuki.hpp.
|
private |
Definition at line 201 of file kobuki.hpp.
|
private |
Definition at line 224 of file kobuki.hpp.
|
private |
Definition at line 185 of file kobuki.hpp.
|
private |
Definition at line 267 of file kobuki.hpp.
|
private |
Definition at line 269 of file kobuki.hpp.
|
private |
Definition at line 269 of file kobuki.hpp.
|
private |
Definition at line 269 of file kobuki.hpp.
|
private |
Definition at line 270 of file kobuki.hpp.
|
private |
Definition at line 273 of file kobuki.hpp.
|
private |
Definition at line 271 of file kobuki.hpp.
|
private |
Definition at line 272 of file kobuki.hpp.
|
private |
Definition at line 267 of file kobuki.hpp.
|
private |
Definition at line 268 of file kobuki.hpp.
|
private |
Definition at line 269 of file kobuki.hpp.
|
private |
Definition at line 184 of file kobuki.hpp.
|
private |
Definition at line 221 of file kobuki.hpp.
|
private |
Definition at line 220 of file kobuki.hpp.
|
private |
Definition at line 244 of file kobuki.hpp.
|
private |
Definition at line 229 of file kobuki.hpp.