kobuki.hpp

Go to the documentation of this file.

 
/*****************************************************************************
 ** Ifdefs
 *****************************************************************************/
 
#ifndef KOBUKI_CORE_HPP_
#define KOBUKI_CORE_HPP_
 
/*****************************************************************************
 ** Includes
 *****************************************************************************/
 
#include <string>
#include <iomanip>
#include <ecl/config.hpp>
#include <ecl/threads.hpp>
#include <ecl/devices.hpp>
#include <ecl/threads/mutex.hpp>
#include <ecl/exceptions/standard_exception.hpp>
// 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 "version_info.hpp"
 
#include "logging.hpp"
#include "parameters.hpp"
#include "event_manager.hpp"
#include "command.hpp"
#include "modules.hpp"
#include "packets.hpp"
#include "packet_handler/packet_finder.hpp"
#include "macros.hpp"
 
/*****************************************************************************
** Extern Templates
*****************************************************************************/
 
#ifdef ECL_IS_WIN32
  /* Help windows create common instances of sigslots across kobuki dll
   * and end user program (otherwise it creates two separate variables!) */
  EXP_TEMPLATE template class kobuki_PUBLIC ecl::SigSlotsManager<>;
  EXP_TEMPLATE template class kobuki_PUBLIC ecl::SigSlotsManager<const kobuki::VersionInfo&>;
  EXP_TEMPLATE template class kobuki_PUBLIC ecl::SigSlotsManager<const std::string&>;
  EXP_TEMPLATE template class kobuki_PUBLIC ecl::SigSlotsManager<kobuki::Command::Buffer&>;
  EXP_TEMPLATE template class kobuki_PUBLIC ecl::SigSlotsManager<kobuki::PacketFinderBase::BufferType&>;
#endif
 
/*****************************************************************************
 ** Namespaces
 *****************************************************************************/
 
namespace kobuki
{
/*****************************************************************************
 ** Definitions
 *****************************************************************************/
 
union union_sint16
{
  short word;
  unsigned char byte[2];
};
 
/*****************************************************************************
** Parent Interface
*****************************************************************************/
 
class PacketFinder : public PacketFinderBase
{
public:
  virtual ~PacketFinder() {}
  bool checkSum();
};
 
/*****************************************************************************
 ** Interface [Kobuki]
 *****************************************************************************/
class kobuki_PUBLIC Kobuki
{
public:
  /*********************
   ** C&D
   **********************/
  Kobuki();
  ~Kobuki();
 
  /*********************
   ** Configuration
   **********************/
  void init(Parameters &parameters);
  bool isAlive() const { return is_alive; } 
  bool isShutdown() const { return shutdown_requested; } 
  bool isEnabled() const { return is_enabled; } 
  bool enable(); 
  bool disable(); 
  void shutdown() { shutdown_requested = true; } 
  /******************************************
  ** Packet Processing
  *******************************************/
  void spin();
  void fixPayload(ecl::PushAndPop<unsigned char> & byteStream);
 
  /******************************************
  ** Getters - Data Protection
  *******************************************/
  void lockDataAccess();
  void unlockDataAccess();
 
  /******************************************
  ** Getters - User Friendly Api
  *******************************************/
  /* Be sure to lock/unlock the data access (lockDataAccess and unlockDataAccess)
   * around any getXXX calls - see the doxygen notes for lockDataAccess. */
  ecl::Angle<double> getHeading() const;
  double getAngularVelocity() const;
  VersionInfo versionInfo() const { return VersionInfo(firmware.version(), hardware.data.version, unique_device_id.data.udid0, unique_device_id.data.udid1, unique_device_id.data.udid2); }
  Battery batteryStatus() const { return Battery(core_sensors.data.battery, core_sensors.data.charger); }
 
  /******************************************
  ** Getters - Raw Data Api
  *******************************************/
  /* Be sure to lock/unlock the data access (lockDataAccess and unlockDataAccess)
   * around any getXXX calls - see the doxygen notes for lockDataAccess. */
  CoreSensors::Data getCoreSensorData() const { return core_sensors.data; }
  DockIR::Data getDockIRData() const { return dock_ir.data; }
  Cliff::Data getCliffData() const { return cliff.data; }
  Current::Data getCurrentData() const { return current.data; }
  Inertia::Data getInertiaData() const { return inertia.data; }
  GpInput::Data getGpInputData() const { return gp_input.data; }
  ThreeAxisGyro::Data getRawInertiaData() const { return three_axis_gyro.data; }
  ControllerInfo::Data getControllerInfoData() const { return controller_info.data; }
 
  /*********************
  ** Feedback
  **********************/
  void getWheelJointStates(double &wheel_left_angle, double &wheel_left_angle_rate,
                           double &wheel_right_angle, double &wheel_right_angle_rate);
  void updateOdometry(ecl::linear_algebra::Vector3d &pose_update,
                      ecl::linear_algebra::Vector3d &pose_update_rates);
 
  /*********************
  ** Soft Commands
  **********************/
  void resetOdometry();
 
  /*********************
  ** Hard Commands
  **********************/
  void setBaseControl(const double &linear_velocity, const double &angular_velocity);
  void setLed(const enum LedNumber &number, const enum LedColour &colour);
  void setDigitalOutput(const DigitalOutput &digital_output);
  void setExternalPower(const DigitalOutput &digital_output);
  void playSoundSequence(const enum SoundSequences &number);
  bool setControllerGain(const unsigned char &type, const unsigned int &p_gain,
                         const unsigned int &i_gain, const unsigned int &d_gain);
  bool getControllerGain();
 
  /*********************
  ** Debugging
  **********************/
  void printSigSlotConnections() const;
 
private:
  /*********************
  ** Thread
  **********************/
  ecl::Thread thread;
  bool shutdown_requested; // helper to shutdown the worker thread.
 
  /*********************
  ** Odometry
  **********************/
  DiffDrive diff_drive;
  bool is_enabled;
 
  /*********************
  ** Inertia
  **********************/
  double heading_offset;
 
  /*********************
  ** Driver Paramters
  **********************/
  Parameters parameters;
  bool is_connected;
 
  /*********************
  ** Acceleration Limiter
  **********************/
  AccelerationLimiter acceleration_limiter;
 
  /*********************
  ** Packet Handling
  **********************/
  CoreSensors core_sensors;
  Inertia inertia;
  DockIR dock_ir;
  Cliff cliff;
  Current current;
  GpInput gp_input;
  Hardware hardware; // requestable
  Firmware firmware; // requestable
  UniqueDeviceID unique_device_id; // requestable
  ThreeAxisGyro three_axis_gyro;
  ControllerInfo controller_info; // requestable
 
  ecl::Serial serial;
  PacketFinder packet_finder;
  PacketFinder::BufferType data_buffer;
  bool is_alive; // used as a flag set by the data stream watchdog
 
  int version_info_reminder;
  int controller_info_reminder;
 
  /*********************
  ** Commands
  **********************/
  void sendBaseControlCommand();
  void sendCommand(Command command);
  ecl::Mutex command_mutex; // protection against the user calling the command functions from multiple threads
  // data_mutex is protection against reading and writing data structures simultaneously as well as
  // ensuring multiple get*** calls are synchronised to the same data update
  // refer to https://github.com/yujinrobot/kobuki/issues/240
  ecl::Mutex data_mutex;
  Command kobuki_command; // used to maintain some state about the command history
  Command::Buffer command_buffer;
  std::vector<short> velocity_commands_debug;
 
  /*********************
  ** Events
  **********************/
  EventManager event_manager;
 
  /*********************
  ** Logging
  **********************/
  std::vector<std::string> log(std::string msg) { return log("", "", msg); }
  std::vector<std::string> log(std::string level, std::string msg) { return log(level, "", msg); }
  std::vector<std::string> log(std::string level, std::string name, std::string msg) {
    std::vector<std::string> ret;
    if( level != "" ) ret.push_back(level);
    if( name != "" ) ret.push_back(name);
    if( msg != "" ) ret.push_back(msg);
    return ret;
  }
 
  /*********************
  ** Signals
  **********************/
  ecl::Signal<> sig_stream_data, sig_controller_info;
  ecl::Signal<const VersionInfo&> sig_version_info;
  ecl::Signal<const std::string&> sig_debug, sig_info, sig_warn, sig_error;
  ecl::Signal<Command::Buffer&> sig_raw_data_command; // should be const, but pushnpop is not fully realised yet for const args in the formatters.
  ecl::Signal<PacketFinder::BufferType&> sig_raw_data_stream; // should be const, but pushnpop is not fully realised yet for const args in the formatters.
  ecl::Signal<const std::vector<short>&> sig_raw_control_command;
 
  /*********************
  ** Slots
  **********************/
  ecl::Slot<const std::string&> slot_log_debug, slot_log_info, slot_log_warning, slot_log_error;
 
};
 
} // namespace kobuki
 
#endif /* KOBUKI_CORE_HPP_ */