Hardware interface for a robot.
More...
#include <diffbot_hw_interface.h>
Hardware interface for a robot.
Definition at line 32 of file diffbot_hw_interface.h.
◆ DiffBotHWInterface()
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 |
◆ angularToLinear()
double diffbot_base::DiffBotHWInterface::angularToLinear |
( |
const double & |
angle | ) |
const |
|
protected |
◆ 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()
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_nh | A NodeHandle in the root of the caller namespace. |
robot_hw_nh | A 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()
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
-
timeout | Minimum 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 |
◆ 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 |
◆ printCommandHelper()
std::string diffbot_base::DiffBotHWInterface::printCommandHelper |
( |
| ) |
|
◆ printState()
void diffbot_base::DiffBotHWInterface::printState |
( |
| ) |
|
|
virtual |
◆ printStateHelper()
std::string diffbot_base::DiffBotHWInterface::printStateHelper |
( |
| ) |
|
◆ read()
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
-
time | The current time |
period | The 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 |
◆ write()
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
-
time | The current time |
period | The time passed since the last call to write |
Reimplemented from hardware_interface::RobotHW.
Definition at line 163 of file diffbot_hw_interface.cpp.
◆ debug_
bool diffbot_base::DiffBotHWInterface::debug_ |
|
protected |
◆ encoder_resolution_
double diffbot_base::DiffBotHWInterface::encoder_resolution_ |
|
protected |
◆ encoder_ticks_
int diffbot_base::DiffBotHWInterface::encoder_ticks_[NUM_JOINTS] |
|
protected |
◆ gain_
double diffbot_base::DiffBotHWInterface::gain_ |
|
protected |
◆ joint_efforts_
double diffbot_base::DiffBotHWInterface::joint_efforts_[NUM_JOINTS] |
|
protected |
◆ joint_names_
std::vector<std::string> diffbot_base::DiffBotHWInterface::joint_names_ |
|
protected |
◆ joint_positions_
double diffbot_base::DiffBotHWInterface::joint_positions_[NUM_JOINTS] |
|
protected |
◆ joint_state_interface_
◆ joint_velocities_
double diffbot_base::DiffBotHWInterface::joint_velocities_[NUM_JOINTS] |
|
protected |
◆ joint_velocity_commands_
double diffbot_base::DiffBotHWInterface::joint_velocity_commands_[NUM_JOINTS] |
|
protected |
◆ max_velocity_
double diffbot_base::DiffBotHWInterface::max_velocity_ |
|
protected |
◆ measured_joint_states_
◆ motor_constant_
double diffbot_base::DiffBotHWInterface::motor_constant_ |
|
protected |
◆ name_
std::string diffbot_base::DiffBotHWInterface::name_ |
|
protected |
◆ nh_
◆ num_joints_
std::size_t diffbot_base::DiffBotHWInterface::num_joints_ |
|
protected |
◆ pids_
◆ pub_left_motor_value_
ros::Publisher diffbot_base::DiffBotHWInterface::pub_left_motor_value_ |
|
protected |
◆ pub_reset_encoders_
◆ pub_right_motor_value_
ros::Publisher diffbot_base::DiffBotHWInterface::pub_right_motor_value_ |
|
protected |
◆ pub_wheel_cmd_velocities_
ros::Publisher diffbot_base::DiffBotHWInterface::pub_wheel_cmd_velocities_ |
|
protected |
◆ pwm_limit_
double diffbot_base::DiffBotHWInterface::pwm_limit_ |
|
protected |
◆ srv_start_
◆ srv_stop_
◆ sub_encoder_ticks_
◆ sub_measured_joint_states_
ros::Subscriber diffbot_base::DiffBotHWInterface::sub_measured_joint_states_ |
|
protected |
◆ trim_
double diffbot_base::DiffBotHWInterface::trim_ |
|
protected |
◆ urdf_model_
urdf::Model* diffbot_base::DiffBotHWInterface::urdf_model_ |
|
protected |
◆ velocity_joint_interface_
◆ wheel_diameter_
double diffbot_base::DiffBotHWInterface::wheel_diameter_ |
|
protected |
◆ wheel_radius_
double diffbot_base::DiffBotHWInterface::wheel_radius_ |
|
protected |
The documentation for this class was generated from the following files: