The Rokubimini Serial class. More...
#include <RokubiminiSerial.hpp>
Public Member Functions | |
void | createRosDiagnostics () override |
Adds ROS diagnostics related to the operation of rokubimini. More... | |
void | createRosPublishers () override |
Adds ROS publishers related to the device. More... | |
void | createRosServices () override |
Adds ROS services related to the device. More... | |
bool | deviceIsMissing () const override |
Checks if the device is missing. More... | |
bool | firmwareUpdateCallback (rokubimini_msgs::FirmwareUpdateSerial::Request &request, rokubimini_msgs::FirmwareUpdateSerial::Response &response) |
The callback for the firmware update ROS service. More... | |
bool | getForceTorqueSamplingRate (int &samplingRate) override |
Gets the force torque sampling rate of the device. More... | |
bool | getSerialNumber (unsigned int &serialNumber) override |
Gets the serial number of the device. More... | |
bool | init () |
Initializes communication with a Rokubimini Serial device. More... | |
bool | loadConfig () |
Loads the configuration of the device. More... | |
void | parseCommunicationMsgs () |
Parses the incoming communication msgs from the device. More... | |
void | postSetupConfiguration () override |
Post-setup configuration hook. More... | |
void | preSetupConfiguration () override |
Post-setup configuration hook. More... | |
bool | printUserConfig () |
Prints all the user configurable parameters. More... | |
void | publishConnectionStatusDiagnostics () |
Publishes the Connection Status Diagnostics to the "/diagnostics" topic. More... | |
void | publishDataFlagsDiagnostics (const ros::TimerEvent &event) |
Publishes the Data Flags Diagnostics to the "/diagnostics" topic. More... | |
void | publishRosDiagnostics () override |
Publish ROS diagnostics related to the operation of rokubimini. More... | |
void | publishRosMessages () override |
Publishes ROS messages with data from the rokubimini device. More... | |
bool | resetWrenchCallback (rokubimini_msgs::ResetWrench::Request &request, rokubimini_msgs::ResetWrench::Response &response) |
The callback for the reset wrench ROS service. More... | |
RokubiminiSerial ()=default | |
Default constructor. More... | |
RokubiminiSerial (const std::string &name, NodeHandlePtr nh) | |
bool | saveConfigParameter () override |
Saves the current configuration to the device. More... | |
bool | setAccelerationFilter (const unsigned int filter) override |
Sets an acceleration filter to the device. More... | |
bool | setAccelerationRange (const unsigned int range) override |
Sets an acceleration range to the device. More... | |
bool | setAngularRateFilter (const unsigned int filter) override |
Sets an angular rate filter to the device. More... | |
bool | setAngularRateRange (const unsigned int range) override |
Sets an angular rate range to the device. More... | |
bool | setConfigMode () |
Sets the device in config mode. More... | |
bool | setForceTorqueFilter (const configuration::ForceTorqueFilter &filter) override |
Sets a force torque filter to the device. More... | |
bool | setForceTorqueOffset (const Eigen::Matrix< double, 6, 1 > &forceTorqueOffset) override |
Sets a force torque offset to the device. More... | |
bool | setHardwareReset () |
Triggers a hardware reset of the sensor. More... | |
void | setImplPointer (const RokubiminiSerialImplPtr &implPtr) |
Sets a pointer to the implementation. More... | |
bool | setInitMode () |
Triggers a software reset of the sensor bringing it to a known state. More... | |
bool | setPublishMode (double timeStep) |
Sets the publishing mode (synchronous or async) of the serial device. More... | |
bool | setRunMode () |
Sets the device in run mode. More... | |
bool | setSensorCalibration (const calibration::SensorCalibration &sensorCalibration) override |
Sets a sensor calibration to the device. More... | |
bool | setSensorConfiguration (const configuration::SensorConfiguration &sensorConfiguration) override |
Sets a sensor configuration to the device. More... | |
void | shutdownWithCommunication () override |
Shuts down a Rokubimini Serial device before communication has been closed. More... | |
void | signalShutdown () |
Signals shutdown for the ROS node. It's used if a firmware update was successful. More... | |
void | update () |
Updates the internal Reading with new measurements and publishes them to ROS. More... | |
void | updateConnectionStatus (diagnostic_updater::DiagnosticStatusWrapper &stat) |
Updates the Connection Status of the device and adds it to the diagnostics status. More... | |
void | updateProcessReading () override |
Updates the RokubiminiSerial object with new measurements. More... | |
~RokubiminiSerial () override=default | |
![]() | |
void | addDeviceDisconnectedCb (const DeviceDisconnectedCb &deviceDisconnectedCb, const int priority=0) |
void | addDeviceReconnectedCb (const DeviceReconnectedCb &deviceReconnectedCb, const int priority=0) |
void | addErrorCb (const ErrorCb &errorCb, const int priority=0) |
void | addErrorRecoveredCb (const ErrorRecoveredCb &errorRecoveredCb, const int priority=0) |
void | addFatalCb (const FatalCb &fatalCb, const int priority=0) |
void | addFatalRecoveredCb (const FatalRecoveredCb &fatalRecoveredCb, const int priority=0) |
void | addReadingCb (const ReadingCb &readingCb, const int priority=0) |
void | clearGoalStateEnum () |
void | deviceDisconnectedCb () |
bool | deviceIsInErrorState () |
bool | deviceIsInFatalState () |
void | deviceReconnectedCb () |
void | errorCb () |
void | errorRecoveredCb () |
void | fatalCb () |
void | fatalRecoveredCb () |
configuration::Configuration & | getConfiguration () |
const configuration::Configuration & | getConfiguration () const |
std::string | getName () const |
std::string | getProductName () const |
Reading | getReading () const |
void | getReading (Reading &reading) const |
Statusword | getStatusword () const |
virtual void | load () |
Rokubimini ()=default | |
Rokubimini (const std::string &name, NodeHandlePtr nh) | |
void | setNodeHandle (const NodeHandlePtr &nh) |
virtual void | shutdownWithoutCommunication () |
void | startupWithCommunication () |
void | startupWithoutCommunication () |
virtual | ~Rokubimini ()=default |
Protected Types | |
using | RosPublisherPtr = std::shared_ptr< ros::Publisher > |
Protected Attributes | |
std::atomic< bool > | computeMeanWrenchFlag_ { false } |
The flag for enabling the computation of the mean of the wrench measurements. Used withing the "reset
service" callback. More... | |
ros::ServiceServer | firmwareUpdateService_ |
The service for firmware updates. More... | |
RokubiminiSerialImplPtr | implPtr_ { nullptr } |
The pointer to implementation. More... | |
std::recursive_mutex | meanWrenchOffsetMutex_ |
The mutex for accessing the mean of the wrench measurements. Used for the "reset service" callback. More... | |
uint64_t | noFrameSyncCounter_ { 0 } |
The counter for measuring failed frame synchronizations. More... | |
bool | publishersSet_ = false |
Flag to check whether publishers are ready. More... | |
std::thread | publishingThread_ |
The thread that publishes the sensor messages to ROS. More... | |
RosPublisherPtr | readingPublisher_ |
The rokubimini_msgs::Reading publisher. More... | |
Eigen::Matrix< double, 6, Eigen::Dynamic > | resetServiceWrenchSamples_ |
The wrench samples used by the "reset service" callback for calculating the mean wrench offset. More... | |
ros::ServiceServer | resetWrenchService_ |
The service for resetting the sensor wrench measurements. More... | |
bool | rosDiagnosticsSet_ = false |
Flag to check whether ros diagnostics are ready. More... | |
RosPublisherPtr | temperaturePublisher_ |
The sensor_msgs::Temperature publisher. More... | |
std::atomic< uint32_t > | wrenchMessageCount_ { 0 } |
The counter that is used for the computation of the mean of the wrench measurements. Used for the "reset
service" callback. More... | |
RosPublisherPtr | wrenchPublisher_ |
The sensor_msgs::Wrench publisher. More... | |
![]() | |
configuration::Configuration | configuration_ |
DiagnosticsUpdaterPtr | connectionStatusUpdater_ |
RosPublisherPtr | dataFlagsDiagnosticsPublisher_ |
TimerPtr | dataFlagsDiagnosticsTimer_ |
std::multimap< int, DeviceDisconnectedCb, std::greater< int > > | deviceDisconnectedCbs_ |
std::multimap< int, DeviceReconnectedCb, std::greater< int > > | deviceReconnectedCbs_ |
std::multimap< int, ErrorCb, std::greater< int > > | errorCbs_ |
std::multimap< int, ErrorRecoveredCb, std::greater< int > > | errorRecoveredCbs_ |
std::multimap< int, FatalCb, std::greater< int > > | fatalCbs_ |
std::multimap< int, FatalRecoveredCb, std::greater< int > > | fatalRecoveredCbs_ |
std::string | name_ |
NodeHandlePtr | nh_ |
std::string | productName_ |
Reading | reading_ |
std::multimap< int, ReadingCb, std::greater< int > > | readingCbs_ |
std::recursive_mutex | readingMutex_ |
Statusword | statusword_ |
std::atomic< bool > | statuswordRequested_ |
Static Protected Attributes | |
const static uint32_t | TOTAL_NUMBER_OF_WRENCH_MESSAGES = 50 |
![]() | |
static constexpr double | NAN_VALUE |
Additional Inherited Members | |
![]() | |
typedef std::function< void(void)> | AnonymousErrorCb |
typedef std::function< void(const Reading &)> | AnonymousReadingCb |
typedef std::function< void(const std::string &)> | DeviceDisconnectedCb |
typedef std::function< void(const std::string &)> | DeviceReconnectedCb |
typedef std::shared_ptr< diagnostic_updater::Updater > | DiagnosticsUpdaterPtr |
typedef std::function< void(const std::string &)> | ErrorCb |
typedef std::function< void(const std::string &)> | ErrorRecoveredCb |
typedef std::function< void(const std::string &)> | FatalCb |
typedef std::function< void(const std::string &)> | FatalRecoveredCb |
typedef std::shared_ptr< ros::NodeHandle > | NodeHandlePtr |
typedef std::function< void(const std::string &, const Reading &)> | ReadingCb |
typedef std::shared_ptr< ros::Publisher > | RosPublisherPtr |
typedef std::shared_ptr< ros::Timer > | TimerPtr |
![]() | |
void | setStatusword (Statusword &statusword) |
The Rokubimini Serial class.
Inherits from the Rokubimini class. It's the interface in the BRIDGE pattern used. It provides the API to be called by client code and is used for interfacing with the implementation class called RokubiminiSerialImpl.
Definition at line 27 of file RokubiminiSerial.hpp.
|
protected |
Definition at line 471 of file RokubiminiSerial.hpp.
|
default |
Default constructor.
The default constructor of the RokubiminiSerial class.
|
inline |
Definition at line 45 of file RokubiminiSerial.hpp.
|
overridedefault |
|
overridevirtual |
Adds ROS diagnostics related to the operation of rokubimini.
Implements rokubimini::Rokubimini.
Definition at line 424 of file RokubiminiSerial.cpp.
|
overridevirtual |
Adds ROS publishers related to the device.
Implements rokubimini::Rokubimini.
Definition at line 223 of file RokubiminiSerial.cpp.
|
overridevirtual |
Adds ROS services related to the device.
Implements rokubimini::Rokubimini.
Definition at line 412 of file RokubiminiSerial.cpp.
|
overridevirtual |
Checks if the device is missing.
Implements rokubimini::Rokubimini.
Definition at line 120 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::firmwareUpdateCallback | ( | rokubimini_msgs::FirmwareUpdateSerial::Request & | request, |
rokubimini_msgs::FirmwareUpdateSerial::Response & | response | ||
) |
The callback for the firmware update ROS service.
request | The request of the ROS service. |
response | The response of the ROS service. |
Definition at line 293 of file RokubiminiSerial.cpp.
|
overridevirtual |
Gets the force torque sampling rate of the device.
samplingRate | The force torque sampling rate to be fetched. |
Implements rokubimini::Rokubimini.
Definition at line 130 of file RokubiminiSerial.cpp.
|
overridevirtual |
Gets the serial number of the device.
serialNumber | The serial number to be fetched. |
Implements rokubimini::Rokubimini.
Definition at line 125 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::init | ( | ) |
Initializes communication with a Rokubimini Serial device.
This method is called by the SerialBusManager to establish communication with the device.
Definition at line 68 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::loadConfig | ( | ) |
Loads the configuration of the device.
Definition at line 200 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::parseCommunicationMsgs | ( | ) |
Parses the incoming communication msgs from the device.
Definition at line 38 of file RokubiminiSerial.cpp.
|
overridevirtual |
Post-setup configuration hook.
Implements rokubimini::Rokubimini.
Definition at line 12 of file RokubiminiSerial.cpp.
|
overridevirtual |
Post-setup configuration hook.
Implements rokubimini::Rokubimini.
Definition at line 33 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::printUserConfig | ( | ) |
Prints all the user configurable parameters.
Definition at line 205 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::publishConnectionStatusDiagnostics | ( | ) |
Publishes the Connection Status Diagnostics to the "/diagnostics" topic.
Definition at line 442 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::publishDataFlagsDiagnostics | ( | const ros::TimerEvent & | event | ) |
Publishes the Data Flags Diagnostics to the "/diagnostics" topic.
Definition at line 447 of file RokubiminiSerial.cpp.
|
overridevirtual |
Publish ROS diagnostics related to the operation of rokubimini.
Implements rokubimini::Rokubimini.
Definition at line 437 of file RokubiminiSerial.cpp.
|
overridevirtual |
Publishes ROS messages with data from the rokubimini device.
Implements rokubimini::Rokubimini.
Definition at line 238 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::resetWrenchCallback | ( | rokubimini_msgs::ResetWrench::Request & | request, |
rokubimini_msgs::ResetWrench::Response & | response | ||
) |
The callback for the reset wrench ROS service.
request | The request of the ROS service. |
response | The response of the ROS service. |
Definition at line 306 of file RokubiminiSerial.cpp.
|
overridevirtual |
Saves the current configuration to the device.
Implements rokubimini::Rokubimini.
Definition at line 195 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets an acceleration filter to the device.
filter | The filter to be set. |
Implements rokubimini::Rokubimini.
Definition at line 140 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets an acceleration range to the device.
range | The range to be set. |
Implements rokubimini::Rokubimini.
Definition at line 150 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets an angular rate filter to the device.
filter | The filter to be set. |
Implements rokubimini::Rokubimini.
Definition at line 145 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets an angular rate range to the device.
range | The range to be set. |
Implements rokubimini::Rokubimini.
Definition at line 155 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::setConfigMode | ( | ) |
Sets the device in config mode.
Definition at line 185 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets a force torque filter to the device.
filter | The filter to be set. |
Implements rokubimini::Rokubimini.
Definition at line 135 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets a force torque offset to the device.
forceTorqueOffset | The offset to be set. |
Implements rokubimini::Rokubimini.
Definition at line 160 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::setHardwareReset | ( | ) |
Triggers a hardware reset of the sensor.
Definition at line 210 of file RokubiminiSerial.cpp.
|
inline |
Sets a pointer to the implementation.
This method realizes the pimpl paradigm. Through it, the RokubiminiSerial object holds a pointer to its implementation, a RokubiminiSerialImpl object.
implPtr | The pointer to the RokubiminiSerialImpl implementation. |
Definition at line 135 of file RokubiminiSerial.hpp.
bool rokubimini::serial::RokubiminiSerial::setInitMode | ( | ) |
Triggers a software reset of the sensor bringing it to a known state.
Definition at line 215 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::setPublishMode | ( | double | timeStep | ) |
Sets the publishing mode (synchronous or async) of the serial device.
timeStep | The time step to set for synchronous publishing, provided it's not 0. If it's 0, this means asynchronous publishing. |
Definition at line 52 of file RokubiminiSerial.cpp.
bool rokubimini::serial::RokubiminiSerial::setRunMode | ( | ) |
Sets the device in run mode.
Definition at line 190 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets a sensor calibration to the device.
sensorCalibration | The calibration to be set. |
Implements rokubimini::Rokubimini.
Definition at line 175 of file RokubiminiSerial.cpp.
|
overridevirtual |
Sets a sensor configuration to the device.
sensorConfiguration | The configuration to be set. |
Implements rokubimini::Rokubimini.
Definition at line 165 of file RokubiminiSerial.cpp.
|
overridevirtual |
Shuts down a Rokubimini Serial device before communication has been closed.
This method shuts down a Rokubimini Serial device before the SerialBusManager has terminated communication with the device.
Implements rokubimini::Rokubimini.
Definition at line 105 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::signalShutdown | ( | ) |
Signals shutdown for the ROS node. It's used if a firmware update was successful.
Definition at line 286 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::update | ( | ) |
Updates the internal Reading with new measurements and publishes them to ROS.
Definition at line 73 of file RokubiminiSerial.cpp.
void rokubimini::serial::RokubiminiSerial::updateConnectionStatus | ( | diagnostic_updater::DiagnosticStatusWrapper & | stat | ) |
Updates the Connection Status of the device and adds it to the diagnostics status.
Definition at line 419 of file RokubiminiSerial.cpp.
|
overridevirtual |
Updates the RokubiminiSerial object with new measurements.
This method updates the internal Reading variable of RokubiminiSerial, by getting the new values from its implementation RokubiminiSerialImpl.
Implements rokubimini::Rokubimini.
Definition at line 85 of file RokubiminiSerial.cpp.
|
protected |
The flag for enabling the computation of the mean of the wrench measurements. Used withing the "reset service" callback.
Definition at line 528 of file RokubiminiSerial.hpp.
|
protected |
The service for firmware updates.
Definition at line 503 of file RokubiminiSerial.hpp.
|
protected |
The pointer to implementation.
Definition at line 469 of file RokubiminiSerial.hpp.
|
mutableprotected |
The mutex for accessing the mean of the wrench measurements. Used for the "reset service" callback.
Definition at line 545 of file RokubiminiSerial.hpp.
|
protected |
The counter for measuring failed frame synchronizations.
Definition at line 519 of file RokubiminiSerial.hpp.
|
protected |
Flag to check whether publishers are ready.
Definition at line 578 of file RokubiminiSerial.hpp.
|
protected |
The thread that publishes the sensor messages to ROS.
Definition at line 570 of file RokubiminiSerial.hpp.
|
protected |
The rokubimini_msgs::Reading publisher.
Definition at line 479 of file RokubiminiSerial.hpp.
|
protected |
The wrench samples used by the "reset service" callback for calculating the mean wrench offset.
Definition at line 562 of file RokubiminiSerial.hpp.
|
protected |
The service for resetting the sensor wrench measurements.
Definition at line 511 of file RokubiminiSerial.hpp.
|
protected |
Flag to check whether ros diagnostics are ready.
Definition at line 586 of file RokubiminiSerial.hpp.
|
protected |
The sensor_msgs::Temperature publisher.
Definition at line 495 of file RokubiminiSerial.hpp.
|
staticprotected |
Definition at line 554 of file RokubiminiSerial.hpp.
|
protected |
The counter that is used for the computation of the mean of the wrench measurements. Used for the "reset service" callback.
Definition at line 537 of file RokubiminiSerial.hpp.
|
protected |
The sensor_msgs::Wrench publisher.
Definition at line 487 of file RokubiminiSerial.hpp.