Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
diffbot_base::DiffBotHWInterface Class Reference

Hardware interface for a robot. More...

#include <diffbot_hw_interface.h>

Inheritance diagram for diffbot_base::DiffBotHWInterface:
Inheritance graph
[legend]

Public Member Functions

 DiffBotHWInterface (ros::NodeHandle &nh, urdf::Model *urdf_model=NULL)
 Constructor. More...
 
virtual bool init (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh)
 The init function is called to initialize the RobotHW from a non-realtime thread. More...
 
bool isReceivingMeasuredJointStates (const ros::Duration &timeout=ros::Duration(1))
 Check if measured joint states (position and velocity) are received. More...
 
std::string printCommandHelper ()
 Helper for debugging a joint's command. More...
 
virtual void printState ()
 Helper for debugging a joint's state. More...
 
std::string printStateHelper ()
 
virtual void read (const ros::Time &time, const ros::Duration &period) override
 Read data from the robot hardware. More...
 
virtual void write (const ros::Time &time, const ros::Duration &period)
 Write commands to the robot hardware. More...
 
virtual ~DiffBotHWInterface ()
 Destructor. More...
 
- Public Member Functions inherited from hardware_interface::RobotHW
virtual bool checkForConflict (const std::list< ControllerInfo > &info) const
 
virtual bool checkForConflict (const std::list< ControllerInfo > &info) const
 
virtual void doSwitch (const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
 
virtual bool prepareSwitch (const std::list< ControllerInfo > &, const std::list< ControllerInfo > &)
 
virtual SwitchState switchResult () const
 
virtual SwitchState switchResult (const ControllerInfo &) const
 
virtual ~RobotHW ()=default
 
- Public Member Functions inherited from hardware_interface::InterfaceManager
T * get ()
 
std::vector< std::string > getInterfaceResources (std::string iface_type) const
 
std::vector< std::string > getNames () const
 
void registerInterface (T *iface)
 
void registerInterfaceManager (InterfaceManager *iface_man)
 

Protected Member Functions

double angularToLinear (const double &angle) const
 RobotHW provides velocity command in rad/s, DiffBot needs m/s. More...
 
void encoderTicksCallback (const diffbot_msgs::EncodersStamped::ConstPtr &msg_encoders)
 Callback to receive the encoder ticks from Teensy MCU. More...
 
double linearToAngular (const double &distance) const
 DiffBot reports travel distance in metres, need radians for ros_control RobotHW. More...
 
virtual void loadURDF (const ros::NodeHandle &nh, std::string param_name)
 Get the URDF XML from the parameter server. More...
 
void measuredJointStatesCallback (const sensor_msgs::JointState::ConstPtr &msg_joint_states)
 Callback to receive the measured joint states from Teensy MCU. More...
 
double normalizeAngle (double &angle) const
 Normalize angle in the range of [0, 360) More...
 
double ticksToAngle (const int &ticks) const
 Convert number of encoder ticks to angle in radians. More...
 

Protected Attributes

bool debug_
 
double encoder_resolution_
 
int encoder_ticks_ [NUM_JOINTS]
 
double gain_
 
double joint_efforts_ [NUM_JOINTS]
 
std::vector< std::string > joint_names_
 
double joint_positions_ [NUM_JOINTS]
 
hardware_interface::JointStateInterface joint_state_interface_
 
double joint_velocities_ [NUM_JOINTS]
 
double joint_velocity_commands_ [NUM_JOINTS]
 
double max_velocity_
 
JointState measured_joint_states_ [NUM_JOINTS]
 
double motor_constant_
 
std::string name_
 
ros::NodeHandle nh_
 
std::size_t num_joints_
 
PID pids_ [NUM_JOINTS]
 
ros::Publisher pub_left_motor_value_
 
ros::Publisher pub_reset_encoders_
 
ros::Publisher pub_right_motor_value_
 
ros::Publisher pub_wheel_cmd_velocities_
 
double pwm_limit_
 
ros::ServiceServer srv_start_
 
ros::ServiceServer srv_stop_
 
ros::Subscriber sub_encoder_ticks_
 
ros::Subscriber sub_measured_joint_states_
 
double trim_
 
urdf::Modelurdf_model_
 
hardware_interface::VelocityJointInterface velocity_joint_interface_
 
double wheel_diameter_
 
double wheel_radius_
 
- Protected Attributes inherited from hardware_interface::InterfaceManager
std::vector< ResourceManagerBase * > interface_destruction_list_
 
InterfaceManagerVector interface_managers_
 
InterfaceMap interfaces_
 
InterfaceMap interfaces_combo_
 
SizeMap num_ifaces_registered_
 
ResourceMap resources_
 

Additional Inherited Members

- Public Types inherited from hardware_interface::RobotHW
enum  SwitchState { SwitchState::DONE, SwitchState::ONGOING, SwitchState::ERROR }
 
- Protected Types inherited from hardware_interface::InterfaceManager
typedef std::vector< InterfaceManager * > InterfaceManagerVector
 
typedef std::map< std::string, void * > InterfaceMap
 
typedef std::map< std::string, std::vector< std::string > > ResourceMap
 
typedef std::map< std::string, size_t > SizeMap
 

Detailed Description

Hardware interface for a robot.

Definition at line 32 of file diffbot_hw_interface.h.

Constructor & Destructor Documentation

◆ DiffBotHWInterface()

diffbot_base::DiffBotHWInterface::DiffBotHWInterface ( ros::NodeHandle nh,
urdf::Model urdf_model = NULL 
)

Constructor.

Parameters
nh- Node handle for topics.
urdf_model- optional pointer to a parsed robot model

Definition at line 14 of file diffbot_hw_interface.cpp.

◆ ~DiffBotHWInterface()

virtual diffbot_base::DiffBotHWInterface::~DiffBotHWInterface ( )
inlinevirtual

Destructor.

Definition at line 43 of file diffbot_hw_interface.h.

Member Function Documentation

◆ angularToLinear()

double diffbot_base::DiffBotHWInterface::angularToLinear ( const double &  angle) const
protected

RobotHW provides velocity command in rad/s, DiffBot needs m/s.

Definition at line 406 of file diffbot_hw_interface.cpp.

◆ encoderTicksCallback()

void diffbot_base::DiffBotHWInterface::encoderTicksCallback ( const diffbot_msgs::EncodersStamped::ConstPtr &  msg_encoders)
protected

Callback to receive the encoder ticks from Teensy MCU.

Process updates from encoders.

Update current encoder ticks in encoders array

Definition at line 358 of file diffbot_hw_interface.cpp.

◆ init()

bool diffbot_base::DiffBotHWInterface::init ( ros::NodeHandle root_nh,
ros::NodeHandle robot_hw_nh 
)
virtual

The init function is called to initialize the RobotHW from a non-realtime thread.

Initialising a custom robot is done by registering joint handles (hardware_interface::ResourceManager::registerHandle) to hardware interfaces that group similar joints and registering those individual hardware interfaces with the class that represents the custom robot (derived from this hardware_interface::RobotHW)

Note
Registering of joint handles and interfaces can either be done in the constructor or this init method.
Parameters
root_nhA NodeHandle in the root of the caller namespace.
robot_hw_nhA NodeHandle in the namespace from which the RobotHW should read its configuration.
Returns
True if initialization was successful

Reimplemented from hardware_interface::RobotHW.

Definition at line 79 of file diffbot_hw_interface.cpp.

◆ isReceivingMeasuredJointStates()

bool diffbot_base::DiffBotHWInterface::isReceivingMeasuredJointStates ( const ros::Duration timeout = ros::Duration(1))

Check if measured joint states (position and velocity) are received.

This function blocks until the sub_measured_joint_states_ subscriber receives measured joint states (angular position (rad) and angular velocity (rad/s)) for each joint on the "measured_joint_states" topic of type sensor_msgs::JointState.

Parameters
timeoutMinimum time to wait for receiving encoder ticks

Definition at line 264 of file diffbot_hw_interface.cpp.

◆ linearToAngular()

double diffbot_base::DiffBotHWInterface::linearToAngular ( const double &  distance) const
protected

DiffBot reports travel distance in metres, need radians for ros_control RobotHW.

Definition at line 401 of file diffbot_hw_interface.cpp.

◆ loadURDF()

void diffbot_base::DiffBotHWInterface::loadURDF ( const ros::NodeHandle nh,
std::string  param_name 
)
protectedvirtual

Get the URDF XML from the parameter server.

Definition at line 292 of file diffbot_hw_interface.cpp.

◆ measuredJointStatesCallback()

void diffbot_base::DiffBotHWInterface::measuredJointStatesCallback ( const sensor_msgs::JointState::ConstPtr &  msg_joint_states)
protected

Callback to receive the measured joint states from Teensy MCU.

Update current encoder ticks in encoders array

Definition at line 367 of file diffbot_hw_interface.cpp.

◆ normalizeAngle()

double diffbot_base::DiffBotHWInterface::normalizeAngle ( double &  angle) const
protected

Normalize angle in the range of [0, 360)

Definition at line 388 of file diffbot_hw_interface.cpp.

◆ printCommandHelper()

std::string diffbot_base::DiffBotHWInterface::printCommandHelper ( )

Helper for debugging a joint's command.

Definition at line 344 of file diffbot_hw_interface.cpp.

◆ printState()

void diffbot_base::DiffBotHWInterface::printState ( )
virtual

Helper for debugging a joint's state.

Definition at line 323 of file diffbot_hw_interface.cpp.

◆ printStateHelper()

std::string diffbot_base::DiffBotHWInterface::printStateHelper ( )

Definition at line 330 of file diffbot_hw_interface.cpp.

◆ read()

void diffbot_base::DiffBotHWInterface::read ( const ros::Time time,
const ros::Duration period 
)
overridevirtual

Read data from the robot hardware.

The read method is part of the control loop cycle (read, update, write) and is used to populate the robot state from the robot's hardware resources (joints, sensors, actuators). This method should be called before controller_manager::ControllerManager::update() and write.

Note
The name read refers to reading state from the hardware. This complements write, which refers to writing commands to the hardware.

Querying WallTime inside read is not realtime safe. The parameters time and period make it possible to inject time from a realtime source.

Parameters
timeThe current time
periodThe time passed since the last call to read

Reimplemented from hardware_interface::RobotHW.

Definition at line 136 of file diffbot_hw_interface.cpp.

◆ ticksToAngle()

double diffbot_base::DiffBotHWInterface::ticksToAngle ( const int &  ticks) const
protected

Convert number of encoder ticks to angle in radians.

Definition at line 380 of file diffbot_hw_interface.cpp.

◆ write()

void diffbot_base::DiffBotHWInterface::write ( const ros::Time time,
const ros::Duration period 
)
virtual

Write commands to the robot hardware.

The write method is part of the control loop cycle (read, update, write) and is used to send out commands to the robot's hardware resources (joints, actuators). This method should be called after read and controller_manager::ControllerManager::update.

Note
The name write refers to writing commands to the hardware. This complements read, which refers to reading state from the hardware.

Querying WallTime inside write is not realtime safe. The parameters time and period make it possible to inject time from a realtime source.

Parameters
timeThe current time
periodThe time passed since the last call to write

Reimplemented from hardware_interface::RobotHW.

Definition at line 163 of file diffbot_hw_interface.cpp.

Member Data Documentation

◆ debug_

bool diffbot_base::DiffBotHWInterface::debug_
protected

Definition at line 187 of file diffbot_hw_interface.h.

◆ encoder_resolution_

double diffbot_base::DiffBotHWInterface::encoder_resolution_
protected

Definition at line 176 of file diffbot_hw_interface.h.

◆ encoder_ticks_

int diffbot_base::DiffBotHWInterface::encoder_ticks_[NUM_JOINTS]
protected

Definition at line 226 of file diffbot_hw_interface.h.

◆ gain_

double diffbot_base::DiffBotHWInterface::gain_
protected

Definition at line 178 of file diffbot_hw_interface.h.

◆ joint_efforts_

double diffbot_base::DiffBotHWInterface::joint_efforts_[NUM_JOINTS]
protected

Definition at line 202 of file diffbot_hw_interface.h.

◆ joint_names_

std::vector<std::string> diffbot_base::DiffBotHWInterface::joint_names_
protected

Definition at line 166 of file diffbot_hw_interface.h.

◆ joint_positions_

double diffbot_base::DiffBotHWInterface::joint_positions_[NUM_JOINTS]
protected

Definition at line 200 of file diffbot_hw_interface.h.

◆ joint_state_interface_

hardware_interface::JointStateInterface diffbot_base::DiffBotHWInterface::joint_state_interface_
protected

Definition at line 156 of file diffbot_hw_interface.h.

◆ joint_velocities_

double diffbot_base::DiffBotHWInterface::joint_velocities_[NUM_JOINTS]
protected

Definition at line 201 of file diffbot_hw_interface.h.

◆ joint_velocity_commands_

double diffbot_base::DiffBotHWInterface::joint_velocity_commands_[NUM_JOINTS]
protected

Definition at line 195 of file diffbot_hw_interface.h.

◆ max_velocity_

double diffbot_base::DiffBotHWInterface::max_velocity_
protected

Definition at line 172 of file diffbot_hw_interface.h.

◆ measured_joint_states_

JointState diffbot_base::DiffBotHWInterface::measured_joint_states_[NUM_JOINTS]
protected

Definition at line 227 of file diffbot_hw_interface.h.

◆ motor_constant_

double diffbot_base::DiffBotHWInterface::motor_constant_
protected

Definition at line 181 of file diffbot_hw_interface.h.

◆ name_

std::string diffbot_base::DiffBotHWInterface::name_
protected

Definition at line 148 of file diffbot_hw_interface.h.

◆ nh_

ros::NodeHandle diffbot_base::DiffBotHWInterface::nh_
protected

Definition at line 151 of file diffbot_hw_interface.h.

◆ num_joints_

std::size_t diffbot_base::DiffBotHWInterface::num_joints_
protected

Definition at line 167 of file diffbot_hw_interface.h.

◆ pids_

PID diffbot_base::DiffBotHWInterface::pids_[NUM_JOINTS]
protected

Definition at line 229 of file diffbot_hw_interface.h.

◆ pub_left_motor_value_

ros::Publisher diffbot_base::DiffBotHWInterface::pub_left_motor_value_
protected

Definition at line 208 of file diffbot_hw_interface.h.

◆ pub_reset_encoders_

ros::Publisher diffbot_base::DiffBotHWInterface::pub_reset_encoders_
protected

Definition at line 217 of file diffbot_hw_interface.h.

◆ pub_right_motor_value_

ros::Publisher diffbot_base::DiffBotHWInterface::pub_right_motor_value_
protected

Definition at line 209 of file diffbot_hw_interface.h.

◆ pub_wheel_cmd_velocities_

ros::Publisher diffbot_base::DiffBotHWInterface::pub_wheel_cmd_velocities_
protected

Definition at line 213 of file diffbot_hw_interface.h.

◆ pwm_limit_

double diffbot_base::DiffBotHWInterface::pwm_limit_
protected

Definition at line 182 of file diffbot_hw_interface.h.

◆ srv_start_

ros::ServiceServer diffbot_base::DiffBotHWInterface::srv_start_
protected

Definition at line 204 of file diffbot_hw_interface.h.

◆ srv_stop_

ros::ServiceServer diffbot_base::DiffBotHWInterface::srv_stop_
protected

Definition at line 205 of file diffbot_hw_interface.h.

◆ sub_encoder_ticks_

ros::Subscriber diffbot_base::DiffBotHWInterface::sub_encoder_ticks_
protected

Definition at line 220 of file diffbot_hw_interface.h.

◆ sub_measured_joint_states_

ros::Subscriber diffbot_base::DiffBotHWInterface::sub_measured_joint_states_
protected

Definition at line 223 of file diffbot_hw_interface.h.

◆ trim_

double diffbot_base::DiffBotHWInterface::trim_
protected

Definition at line 179 of file diffbot_hw_interface.h.

◆ urdf_model_

urdf::Model* diffbot_base::DiffBotHWInterface::urdf_model_
protected

Definition at line 168 of file diffbot_hw_interface.h.

◆ velocity_joint_interface_

hardware_interface::VelocityJointInterface diffbot_base::DiffBotHWInterface::velocity_joint_interface_
protected

Definition at line 163 of file diffbot_hw_interface.h.

◆ wheel_diameter_

double diffbot_base::DiffBotHWInterface::wheel_diameter_
protected

Definition at line 171 of file diffbot_hw_interface.h.

◆ wheel_radius_

double diffbot_base::DiffBotHWInterface::wheel_radius_
protected

Definition at line 170 of file diffbot_hw_interface.h.


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


diffbot_base
Author(s):
autogenerated on Thu Sep 7 2023 02:35:23