#include <husky_hardware.h>
Class representing Husky hardware, allows for ros_control to modify internal state via joint interfaces
Definition at line 51 of file husky_hardware.h.
husky_base::HuskyHardware::HuskyHardware | ( | ros::NodeHandle | nh, |
ros::NodeHandle | private_nh, | ||
double | target_control_freq | ||
) |
Initialize Husky hardware
Definition at line 46 of file husky_hardware.cpp.
double husky_base::HuskyHardware::angularToLinear | ( | const double & | angle | ) | const [private] |
RobotHW provides velocity command in rad/s, Husky needs m/s,
Definition at line 235 of file husky_hardware.cpp.
void husky_base::HuskyHardware::initializeDiagnostics | ( | ) | [private] |
Register diagnostic tasks with updater class
Definition at line 93 of file husky_hardware.cpp.
void husky_base::HuskyHardware::limitDifferentialSpeed | ( | double & | diff_speed_left, |
double & | diff_speed_right | ||
) | [private] |
Scale left and right speed outputs to maintain ros_control's desired trajectory without saturating the outputs
Definition at line 213 of file husky_hardware.cpp.
double husky_base::HuskyHardware::linearToAngular | ( | const double & | travel | ) | const [private] |
Husky reports travel in metres, need radians for ros_control RobotHW
Definition at line 227 of file husky_hardware.cpp.
void husky_base::HuskyHardware::registerControlInterfaces | ( | ) | [private] |
Register interfaces with the RobotHW interface manager, allowing ros_control operation
Definition at line 112 of file husky_hardware.cpp.
void husky_base::HuskyHardware::reportLoopDuration | ( | const ros::Duration & | duration | ) |
Update diagnostics with control loop timing information
Definition at line 205 of file husky_hardware.cpp.
void husky_base::HuskyHardware::resetTravelOffset | ( | ) | [private] |
Get current encoder travel offsets from MCU and bias future encoder readings against them
Definition at line 73 of file husky_hardware.cpp.
External hook to trigger diagnostic update
Definition at line 134 of file husky_hardware.cpp.
Pull latest speed and travel measurements from MCU, and store in joint structure for ros_control
Definition at line 144 of file husky_hardware.cpp.
Get latest velocity commands from ros_control via joint structure, and send to MCU
Definition at line 192 of file husky_hardware.cpp.
Definition at line 86 of file husky_hardware.h.
Definition at line 88 of file husky_hardware.h.
husky_msgs::HuskyStatus husky_base::HuskyHardware::husky_status_msg_ [private] |
Definition at line 87 of file husky_hardware.h.
Definition at line 82 of file husky_hardware.h.
struct husky_base::HuskyHardware::Joint husky_base::HuskyHardware::joints_[4] [private] |
double husky_base::HuskyHardware::max_accel_ [private] |
Definition at line 95 of file husky_hardware.h.
double husky_base::HuskyHardware::max_speed_ [private] |
Definition at line 95 of file husky_hardware.h.
Definition at line 79 of file husky_hardware.h.
double husky_base::HuskyHardware::polling_timeout_ [private] |
Definition at line 97 of file husky_hardware.h.
HuskyHardwareDiagnosticTask<clearpath::DataPowerSystem> husky_base::HuskyHardware::power_status_task_ [private] |
Definition at line 90 of file husky_hardware.h.
Definition at line 79 of file husky_hardware.h.
HuskyHardwareDiagnosticTask<clearpath::DataSafetySystemStatus> husky_base::HuskyHardware::safety_status_task_ [private] |
Definition at line 91 of file husky_hardware.h.
Definition at line 92 of file husky_hardware.h.
HuskyHardwareDiagnosticTask<clearpath::DataSystemStatus> husky_base::HuskyHardware::system_status_task_ [private] |
Definition at line 89 of file husky_hardware.h.
hardware_interface::VelocityJointInterface husky_base::HuskyHardware::velocity_joint_interface_ [private] |
Definition at line 83 of file husky_hardware.h.
double husky_base::HuskyHardware::wheel_diameter_ [private] |
Definition at line 95 of file husky_hardware.h.