Public Member Functions | Private Member Functions | Private Attributes | List of all members
rm_hw::RmRobotHW Class Reference

#include <hardware_interface.h>

Inheritance diagram for rm_hw::RmRobotHW:
Inheritance graph
[legend]

Public Member Functions

bool init (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override
 Get necessary params from param server. Init hardware_interface. More...
 
void read (const ros::Time &time, const ros::Duration &period) override
 Comunicate with hardware. Get datas, status of robot. More...
 
 RmRobotHW ()=default
 
void setCanBusThreadPriority (int thread_priority)
 
void write (const ros::Time &time, const ros::Duration &period) override
 Comunicate with hardware. Publish command to robot. 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)
 

Private Member Functions

bool enableImuTrigger (rm_msgs::EnableImuTrigger::Request &req, rm_msgs::EnableImuTrigger::Response &res)
 
bool loadUrdf (ros::NodeHandle &root_nh)
 Load urdf of robot from param server. More...
 
bool parseActCoeffs (XmlRpc::XmlRpcValue &act_coeffs)
 Check whether some coefficients that are related to actuator are set up and load these coefficients. More...
 
bool parseActData (XmlRpc::XmlRpcValue &act_datas, ros::NodeHandle &robot_hw_nh)
 Check whether actuator is specified and load specified params. More...
 
bool parseGpioData (XmlRpc::XmlRpcValue &gpio_datas, ros::NodeHandle &robot_hw_nh)
 Check whether somme params that are related to gpio are set up and load these params. More...
 
bool parseImuData (XmlRpc::XmlRpcValue &imu_datas, ros::NodeHandle &robot_hw_nh)
 Check whether some params that are related to imu are set up and load these params. More...
 
bool parseTofData (XmlRpc::XmlRpcValue &tof_datas, ros::NodeHandle &robot_hw_nh)
 
void publishActuatorState (const ros::Time &time)
 Publish actuator's state to a topic named "/actuator_states". More...
 
bool setupJointLimit (ros::NodeHandle &root_nh)
 Set up joint limit. More...
 
bool setupTransmission (ros::NodeHandle &root_nh)
 Set up transmission. More...
 

Private Attributes

rm_control::ActuatorExtraInterface act_extra_interface_
 
hardware_interface::ActuatorStateInterface act_state_interface_
 
transmission_interface::ActuatorToJointStateInterfaceact_to_jnt_state_ {}
 
std::shared_ptr< realtime_tools::RealtimePublisher< rm_msgs::ActuatorState > > actuator_state_pub_
 
std::unordered_map< std::string, std::unordered_map< int, ActData > > bus_id2act_data_ {}
 
std::unordered_map< std::string, std::unordered_map< int, ImuData > > bus_id2imu_data_ {}
 
std::unordered_map< std::string, std::unordered_map< int, TofData > > bus_id2tof_data_ {}
 
std::vector< CanBus * > can_buses_ {}
 
hardware_interface::EffortActuatorInterface effort_act_interface_
 
joint_limits_interface::EffortJointSaturationInterface effort_jnt_saturation_interface_
 
joint_limits_interface::EffortJointSoftLimitsInterface effort_jnt_soft_limits_interface_
 
std::vector< hardware_interface::JointHandleeffort_joint_handles_ {}
 
rm_control::GpioCommandInterface gpio_command_interface_
 
GpioManager gpio_manager_ {}
 
rm_control::GpioStateInterface gpio_state_interface_
 
hardware_interface::ImuSensorInterface imu_sensor_interface_
 
bool is_actuator_specified_ = false
 
transmission_interface::JointToActuatorEffortInterfacejnt_to_act_effort_ {}
 
ros::Time last_publish_time_
 
rm_control::RmImuSensorInterface rm_imu_sensor_interface_
 
rm_control::RobotStateInterface robot_state_interface_
 
transmission_interface::RobotTransmissions robot_transmissions_
 
ros::ServiceServer service_server_
 
int thread_priority_
 
rm_control::TofRadarInterface tof_radar_interface_
 
std::unique_ptr< transmission_interface::TransmissionInterfaceLoadertransmission_loader_ {}
 
std::unordered_map< std::string, ActCoefftype2act_coeffs_ {}
 
std::shared_ptr< urdf::Modelurdf_model_
 
std::string urdf_string_
 

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
 
- 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_
 

Detailed Description

Definition at line 101 of file hardware_interface.h.

Constructor & Destructor Documentation

◆ RmRobotHW()

rm_hw::RmRobotHW::RmRobotHW ( )
default

Member Function Documentation

◆ enableImuTrigger()

bool rm_hw::RmRobotHW::enableImuTrigger ( rm_msgs::EnableImuTrigger::Request &  req,
rm_msgs::EnableImuTrigger::Response &  res 
)
private

Definition at line 256 of file hardware_interface.cpp.

◆ init()

bool rm_hw::RmRobotHW::init ( ros::NodeHandle root_nh,
ros::NodeHandle robot_hw_nh 
)
overridevirtual

Get necessary params from param server. Init hardware_interface.

Get params from param server and check whether these params are set. Load urdf of robot. Set up transmission and joint limit. Get configuration of can bus and create data pointer which point to data received from Can bus.

Parameters
root_nhRoot node-handle of a ROS node.
robot_hw_nhNode-handle for robot hardware.
Returns
True when init successful, False when failed.

Reimplemented from hardware_interface::RobotHW.

Definition at line 75 of file hardware_interface.cpp.

◆ loadUrdf()

bool rm_hw::RmRobotHW::loadUrdf ( ros::NodeHandle root_nh)
private

Load urdf of robot from param server.

Load urdf of robot from param server.

Parameters
root_nhRoot node-handle of a ROS node
Returns
True if successful.

Definition at line 525 of file parse.cpp.

◆ parseActCoeffs()

bool rm_hw::RmRobotHW::parseActCoeffs ( XmlRpc::XmlRpcValue act_coeffs)
private

Check whether some coefficients that are related to actuator are set up and load these coefficients.

Check whether some coefficients that are related to actuator are set up and load these coefficients.

Parameters
act_coeffsCoefficients you want to check and load.
Returns
True if all coefficients are set up.

Definition at line 81 of file parse.cpp.

◆ parseActData()

bool rm_hw::RmRobotHW::parseActData ( XmlRpc::XmlRpcValue act_datas,
ros::NodeHandle robot_hw_nh 
)
private

Check whether actuator is specified and load specified params.

Check whether actuator is specified and load specified params.

Parameters
act_datasParams you want to check and load.
robot_hw_nhRoot node-handle of a ROS node.
Returns
True if all params are set up.

Definition at line 159 of file parse.cpp.

◆ parseGpioData()

bool rm_hw::RmRobotHW::parseGpioData ( XmlRpc::XmlRpcValue gpio_datas,
ros::NodeHandle robot_hw_nh 
)
private

Check whether somme params that are related to gpio are set up and load these params.

Check whether somme params that are related to gpio are set up and load these params.

Parameters
gpio_datasParams you want to check and load
robot_hw_nhA handle of a ROS node
Returns
True if all params are set up.

Definition at line 431 of file parse.cpp.

◆ parseImuData()

bool rm_hw::RmRobotHW::parseImuData ( XmlRpc::XmlRpcValue imu_datas,
ros::NodeHandle robot_hw_nh 
)
private

Check whether some params that are related to imu are set up and load these params.

Check whether some params that are related to imu are set up and load these params.

Parameters
imu_datasParams you want to check
robot_hw_nhRoot node-handle of a ROS node
Returns
True if all params are set up.

Definition at line 269 of file parse.cpp.

◆ parseTofData()

bool rm_hw::RmRobotHW::parseTofData ( XmlRpc::XmlRpcValue tof_datas,
ros::NodeHandle robot_hw_nh 
)
private

Definition at line 476 of file parse.cpp.

◆ publishActuatorState()

void rm_hw::RmRobotHW::publishActuatorState ( const ros::Time time)
private

Publish actuator's state to a topic named "/actuator_states".

Publish actuator's state to a topic named "/actuator_states".

Parameters
timeCurrent time

Definition at line 216 of file hardware_interface.cpp.

◆ read()

void rm_hw::RmRobotHW::read ( const ros::Time time,
const ros::Duration period 
)
overridevirtual

Comunicate with hardware. Get datas, status of robot.

Call rm_hw::CanBus::read(). Check whether temperature of actuator is too high and whether actuator is offline. Propagate actuator state to joint state for the stored transmission. Set all cmd to zero to avoid crazy soft limit oscillation when not controller loaded(all controllers update after read()).

Parameters
timeCurrent time
periodCurrent time - last time

Reimplemented from hardware_interface::RobotHW.

Definition at line 152 of file hardware_interface.cpp.

◆ setCanBusThreadPriority()

void rm_hw::RmRobotHW::setCanBusThreadPriority ( int  thread_priority)

Definition at line 211 of file hardware_interface.cpp.

◆ setupJointLimit()

bool rm_hw::RmRobotHW::setupJointLimit ( ros::NodeHandle root_nh)
private

Set up joint limit.

Set up joint limit.

Parameters
root_nhRoot node-handle of a ROS node.
Returns
True if successful.

Definition at line 575 of file parse.cpp.

◆ setupTransmission()

bool rm_hw::RmRobotHW::setupTransmission ( ros::NodeHandle root_nh)
private

Set up transmission.

Set up transmission

Parameters
root_nhRoot node-handle of a ROS node.
Returns
True if successful.

Definition at line 534 of file parse.cpp.

◆ write()

void rm_hw::RmRobotHW::write ( const ros::Time time,
const ros::Duration period 
)
overridevirtual

Comunicate with hardware. Publish command to robot.

Propagate joint state to actuator state for the stored transmission. Limit cmd_effort into suitable value. Call rm_hw::CanBus::write(). Publish actuator current state.

Parameters
timeCurrent time
periodCurrent time - last time

Reimplemented from hardware_interface::RobotHW.

Definition at line 183 of file hardware_interface.cpp.

Member Data Documentation

◆ act_extra_interface_

rm_control::ActuatorExtraInterface rm_hw::RmRobotHW::act_extra_interface_
private

Definition at line 251 of file hardware_interface.h.

◆ act_state_interface_

hardware_interface::ActuatorStateInterface rm_hw::RmRobotHW::act_state_interface_
private

Definition at line 250 of file hardware_interface.h.

◆ act_to_jnt_state_

transmission_interface::ActuatorToJointStateInterface* rm_hw::RmRobotHW::act_to_jnt_state_ {}
private

Definition at line 258 of file hardware_interface.h.

◆ actuator_state_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ActuatorState> > rm_hw::RmRobotHW::actuator_state_pub_
private

Definition at line 280 of file hardware_interface.h.

◆ bus_id2act_data_

std::unordered_map<std::string, std::unordered_map<int, ActData> > rm_hw::RmRobotHW::bus_id2act_data_ {}
private

Definition at line 271 of file hardware_interface.h.

◆ bus_id2imu_data_

std::unordered_map<std::string, std::unordered_map<int, ImuData> > rm_hw::RmRobotHW::bus_id2imu_data_ {}
private

Definition at line 274 of file hardware_interface.h.

◆ bus_id2tof_data_

std::unordered_map<std::string, std::unordered_map<int, TofData> > rm_hw::RmRobotHW::bus_id2tof_data_ {}
private

Definition at line 277 of file hardware_interface.h.

◆ can_buses_

std::vector<CanBus*> rm_hw::RmRobotHW::can_buses_ {}
private

Definition at line 246 of file hardware_interface.h.

◆ effort_act_interface_

hardware_interface::EffortActuatorInterface rm_hw::RmRobotHW::effort_act_interface_
private

Definition at line 252 of file hardware_interface.h.

◆ effort_jnt_saturation_interface_

joint_limits_interface::EffortJointSaturationInterface rm_hw::RmRobotHW::effort_jnt_saturation_interface_
private

Definition at line 260 of file hardware_interface.h.

◆ effort_jnt_soft_limits_interface_

joint_limits_interface::EffortJointSoftLimitsInterface rm_hw::RmRobotHW::effort_jnt_soft_limits_interface_
private

Definition at line 261 of file hardware_interface.h.

◆ effort_joint_handles_

std::vector<hardware_interface::JointHandle> rm_hw::RmRobotHW::effort_joint_handles_ {}
private

Definition at line 262 of file hardware_interface.h.

◆ gpio_command_interface_

rm_control::GpioCommandInterface rm_hw::RmRobotHW::gpio_command_interface_
private

Definition at line 249 of file hardware_interface.h.

◆ gpio_manager_

GpioManager rm_hw::RmRobotHW::gpio_manager_ {}
private

Definition at line 247 of file hardware_interface.h.

◆ gpio_state_interface_

rm_control::GpioStateInterface rm_hw::RmRobotHW::gpio_state_interface_
private

Definition at line 248 of file hardware_interface.h.

◆ imu_sensor_interface_

hardware_interface::ImuSensorInterface rm_hw::RmRobotHW::imu_sensor_interface_
private

Definition at line 254 of file hardware_interface.h.

◆ is_actuator_specified_

bool rm_hw::RmRobotHW::is_actuator_specified_ = false
private

Definition at line 243 of file hardware_interface.h.

◆ jnt_to_act_effort_

transmission_interface::JointToActuatorEffortInterface* rm_hw::RmRobotHW::jnt_to_act_effort_ {}
private

Definition at line 259 of file hardware_interface.h.

◆ last_publish_time_

ros::Time rm_hw::RmRobotHW::last_publish_time_
private

Definition at line 279 of file hardware_interface.h.

◆ rm_imu_sensor_interface_

rm_control::RmImuSensorInterface rm_hw::RmRobotHW::rm_imu_sensor_interface_
private

Definition at line 255 of file hardware_interface.h.

◆ robot_state_interface_

rm_control::RobotStateInterface rm_hw::RmRobotHW::robot_state_interface_
private

Definition at line 253 of file hardware_interface.h.

◆ robot_transmissions_

transmission_interface::RobotTransmissions rm_hw::RmRobotHW::robot_transmissions_
private

Definition at line 257 of file hardware_interface.h.

◆ service_server_

ros::ServiceServer rm_hw::RmRobotHW::service_server_
private

Definition at line 241 of file hardware_interface.h.

◆ thread_priority_

int rm_hw::RmRobotHW::thread_priority_
private

Definition at line 244 of file hardware_interface.h.

◆ tof_radar_interface_

rm_control::TofRadarInterface rm_hw::RmRobotHW::tof_radar_interface_
private

Definition at line 263 of file hardware_interface.h.

◆ transmission_loader_

std::unique_ptr<transmission_interface::TransmissionInterfaceLoader> rm_hw::RmRobotHW::transmission_loader_ {}
private

Definition at line 256 of file hardware_interface.h.

◆ type2act_coeffs_

std::unordered_map<std::string, ActCoeff> rm_hw::RmRobotHW::type2act_coeffs_ {}
private

Definition at line 270 of file hardware_interface.h.

◆ urdf_model_

std::shared_ptr<urdf::Model> rm_hw::RmRobotHW::urdf_model_
private

Definition at line 267 of file hardware_interface.h.

◆ urdf_string_

std::string rm_hw::RmRobotHW::urdf_string_
private

Definition at line 266 of file hardware_interface.h.


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


rm_hw
Author(s): Qiayuan Liao
autogenerated on Tue May 6 2025 02:23:44