Class DynamixelServoHardwareInterface

Inheritance Relationships

Base Type

Class Documentation

class bitbots_ros_control::DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInterface

Public Functions

explicit DynamixelServoHardwareInterface(rclcpp::Node::SharedPtr nh)
virtual bool init()
virtual void read(const rclcpp::Time &t, const rclcpp::Duration &dt)
virtual void write(const rclcpp::Time &t, const rclcpp::Duration &dt)
void addBusInterface(std::shared_ptr<ServoBusInterface> bus)
void writeROMRAM(bool first_time)

Private Functions

void setTorqueCb(std_msgs::msg::Bool::SharedPtr enabled)
void individualTorqueCb(bitbots_msgs::msg::JointTorque msg)
void commandCb(const bitbots_msgs::msg::JointCommand &command_msg)

Private Members

rclcpp::Node::SharedPtr nh_
std::vector<std::shared_ptr<ServoBusInterface>> bus_interfaces_
std::vector<int32_t> goal_torque_individual_
ControlMode control_mode_
unsigned int joint_count_
std::vector<std::string> joint_names_
std::vector<double> lower_joint_limits_
std::vector<double> upper_joint_limits_
std::vector<double> goal_position_
std::vector<double> goal_effort_
std::vector<double> goal_velocity_
std::vector<double> goal_acceleration_
std::vector<double> current_position_
std::vector<double> current_velocity_
std::vector<double> current_effort_
std::vector<double> current_pwm_
std::vector<double> current_input_voltage_
std::vector<double> current_temperature_
std::vector<uint8_t> current_error_
std::map<std::string, int> joint_map_
bool torqueless_mode_
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr set_torque_sub_
rclcpp::Subscription<bitbots_msgs::msg::JointTorque>::SharedPtr set_torque_indiv_sub_
rclcpp::Subscription<bitbots_msgs::msg::JointCommand>::SharedPtr sub_command_
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pwm_pub_
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_
sensor_msgs::msg::JointState joint_state_msg_
sensor_msgs::msg::JointState pwm_msg_