LinkNexus/huoyi_deploy
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
huangweifeng
2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
interface/robot/hardware/dds_interface.hpp
+311
View File
@@ -0,0 +1,311 @@
/**
* @file dds_hardware_interface.hpp
* @brief hardware interface for dds
* @author DeepRobotics
* @version 1.0
* @date 2025-11-07
*
* @copyright Copyright (c) 2025 DeepRobotics
*
*/
#pragma once
#include "common_types.h"
#include "robot_interface.h"
#include "dds_types.h"
#include "drdds/msg/joints_data.hpp"
#include "drdds/msg/joints_data_cmd.hpp"
#include "drdds/msg/imu_data.hpp"
#include "drdds/msg/battery_data.hpp"
using namespace dds;
struct JointConfig {
float dir;
float offset;
};
class DdsInterface : public RobotInterface{
protected:
double ri_ts_, imu_ts_;
Vec3f omega_body_, rpy_, acc_, base_lin_vel_;
VecXf joint_pos_, joint_vel_, joint_tau_;
VecXf contact_force_;
VecXf motor_temperture_, driver_temperture_;
float *pos_offset_;
float *joint_dir_;
bool *data_updated_;
uint16_t *control_word;
JointConfig *joint_config_;
std::vector<uint16_t> driver_status_;
std::vector<uint16_t> joint_data_id_;
BatteryInfo_dds battery_info_[2];
std::vector<uint16_t> battery_data_;
double time_stamp_;
float remain_bat;
rclcpp::Publisher<drdds::msg::JointsDataCmd>::SharedPtr joint_cmd_pub_;
rclcpp::Subscription<drdds::msg::JointsData>::SharedPtr joint_data_sub_;
rclcpp::Subscription<drdds::msg::ImuData>::SharedPtr imu_data_sub_;
rclcpp::Subscription<drdds::msg::BatteryData>::SharedPtr health_data_sub_;
bool IsDataUpdatedFinished() {
bool res = data_updated_[0];
for (int i = 0; i < dof_num_; ++i) {
res = res & data_updated_[i];
}
return res;
}
void ResetPositionOffset() {
memset(data_updated_, 0, dof_num_ * sizeof(bool));
this->SetJointCommand(MatXf::Zero(dof_num_, 5));
usleep(100 * 1000);
VecXf last_joint_pos = this->GetJointPosition();
VecXf current_joint_pos = this->GetJointPosition();
int cnt = 0;
while (!IsDataUpdatedFinished()) {
++cnt;
this->RefreshRobotData();
rclcpp::spin_some(this->get_node());
current_joint_pos = this->GetJointPosition();
for (int i = 0; i < dof_num_; ++i) {
if (!data_updated_[i] && current_joint_pos(i) != last_joint_pos(i)) {
data_updated_[i] = true;
}
}
last_joint_pos = current_joint_pos;
usleep(1000);
if (cnt == 10000) {
std::cout << data_updated_ << std::endl;
std::cout << "joint data update is not finished\n";
}
}
for (int i = 1; i < dof_num_; i += 4) {
if (current_joint_pos(i) < -210. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] + 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
} else if (current_joint_pos(i) > 90. / 180. * M_PI) {
pos_offset_[i] = pos_offset_[i] - 360.;
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
}
}
for (int i = 0; i < dof_num_; ++i) {
joint_config_[i].dir = joint_dir_[i];
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
}
}
public:
int run_cnt_ = 0;
DdsInterface(const std::string &robot_name, const int &dof_num) : RobotInterface(robot_name, dof_num) {
std::cout << robot_name << " is using DDS Interface" << std::endl;
joint_pos_ = VecXf::Zero(dof_num_);
joint_vel_ = VecXf::Zero(dof_num_);
joint_tau_ = VecXf::Zero(dof_num_);
motor_temperture_ = VecXf::Zero(dof_num_);
driver_temperture_ = VecXf::Zero(dof_num_);
omega_body_.setZero();
rpy_.setZero();
acc_.setZero();
base_lin_vel_.setZero();
contact_force_ = VecXf::Zero(9);
ri_ts_ = 0.0;
imu_ts_ = 0.0;
time_stamp_ = 0.0;
remain_bat = 0.0f;
pos_offset_ = new float[dof_num_];
joint_dir_ = new float[dof_num_];
data_updated_ = new bool[dof_num_];
joint_config_ = new JointConfig[dof_num_];
control_word = new uint16_t[4];
for (int i = 0; i < dof_num_; ++i) {
pos_offset_[i] = 0.0f;
joint_dir_[i] = 1.0f;
data_updated_[i] = false;
joint_config_[i].dir = 1.0f;
joint_config_[i].offset = 0.0f;
}
driver_status_.assign(dof_num_, 1);
joint_data_id_.resize(dof_num_);
battery_data_.resize(BATTERY_DATA_SIZE);
joint_cmd_pub_ = node_->create_publisher<drdds::msg::JointsDataCmd>("/JOINTS_CMD", 10);
joint_data_sub_ = node_->create_subscription<drdds::msg::JointsData>("/JOINTS_DATA", 10,
std::bind(&DdsInterface::Handler, this,std::placeholders::_1));
imu_data_sub_ = node_->create_subscription<drdds::msg::ImuData>("/IMU_DATA", 10,
std::bind(&DdsInterface::HandlerIMU, this, std::placeholders::_1));
health_data_sub_ = node_->create_subscription<drdds::msg::BatteryData>("/BATTERY_DATA", 10,
std::bind(&DdsInterface::HandlerHealth, this, std::placeholders::_1));
sleep(1);
control_word[0] = kIndexDisable;
control_word[1] = kIndexErrorReset;
control_word[2] = kIndexEnable;
control_word[3] = kIndexGetStatusWord;
ResetJointError();
}
virtual ~DdsInterface() {
delete[] pos_offset_;
delete[] joint_dir_;
delete[] data_updated_;
delete[] joint_config_;
}
virtual void Start() {
}
virtual void Stop() {
}
virtual double GetInterfaceTimeStamp() {
return ri_ts_;
}
virtual VecXf GetJointPosition() {
return joint_pos_;
}
virtual VecXf GetJointVelocity() {
return joint_vel_;
}
virtual VecXf GetJointTorque() {
return joint_tau_;
}
virtual Vec3f GetImuRpy() {
return rpy_;
}
virtual Vec3f GetImuAcc() {
return acc_;
}
virtual Vec3f GetImuOmega() {
return omega_body_;
}
virtual Vec3f GetBaseLinearVelocity() {
return base_lin_vel_;
}
virtual VecXf GetContactForce() {
return contact_force_;
}
virtual void ResetJointError() {
auto msg = drdds::msg::JointsDataCmd();
for (int j = 0; j < 4; ++j) {
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position = 0;
msg.data.joints_data[i].velocity = 0;
msg.data.joints_data[i].torque = 0;
msg.data.joints_data[i].kp = 0;
msg.data.joints_data[i].kd = 0;
msg.data.joints_data[i].control_word = control_word[j];
}
std::cout << "ResetJointError publish!! \n" << std::endl;
joint_cmd_pub_->publish(msg);
usleep(10 * 1000);
}
}
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
joint_cmd_ = input;
auto msg = drdds::msg::JointsDataCmd();
for (int i = 0; i < dof_num_; ++i) {
msg.data.joints_data[i].position =
(input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
msg.data.joints_data[i].kp = input(i, 0);
msg.data.joints_data[i].kd = input(i, 2);
msg.data.joints_data[i].control_word = kIndexMotorControl;
}
std::cout << "SetJointCommand publish!! \n" << std::endl;
joint_cmd_pub_->publish(msg);
}
virtual VecXf GetMotorTemperture() {
return motor_temperture_;
}
virtual VecXf GetDriverTemperture() {
return driver_temperture_;
}
virtual double GetImuTimestamp() {
return imu_ts_;
}
virtual std::vector<uint16_t> GetBatteryData() {
return battery_data_;
}
virtual std::vector<uint16_t> GetDriverStatusWord() {
return driver_status_;
}
virtual std::vector<uint16_t> GetJointDataID() {
return joint_data_id_;
}
virtual void RefreshRobotData() {
}
virtual void Handler(const drdds::msg::JointsData::SharedPtr msg) {
++run_cnt_;
for (int i = 0; i < dof_num_; ++i) {
joint_pos_(i) = msg->data.joints_data[i].position * joint_dir_[i] + pos_offset_[i] / 180. * M_PI;
joint_vel_(i) = msg->data.joints_data[i].velocity * joint_dir_[i];
joint_tau_(i) = msg->data.joints_data[i].torque * joint_dir_[i];
motor_temperture_(i) = float(msg->data.joints_data[i].motion_temp);
driver_temperture_(i) = float(msg->data.joints_data[i].driver_temp);
driver_status_[i] = msg->data.joints_data[i].status_word;
joint_data_id_[i] = uint16_t(run_cnt_);
}
// FireAnt sim2sim telemetry is packed into unused entries of the fixed
// 16-joint DDS message by mujoco_simulation_ros2.py.
if (dof_num_ == 8 && msg->data.joints_data.size() >= 16) {
for (int i = 0; i < 4; ++i) {
contact_force_(i) = msg->data.joints_data[8 + i].position;
contact_force_(5 + i) = msg->data.joints_data[12 + i].position;
}
contact_force_(4) = msg->data.joints_data[12].velocity;
for (int i = 0; i < 3; ++i) {
base_lin_vel_(i) = msg->data.joints_data[8 + i].torque;
}
}
ri_ts_ = rclcpp::Time(msg->header.stamp).seconds();
}
void HandlerIMU(const drdds::msg::ImuData::SharedPtr msg) {
rpy_ = Vec3f(Deg2Rad(msg->data.roll), Deg2Rad(msg->data.pitch), Deg2Rad(msg->data.yaw));
acc_ << msg->data.acc_x, msg->data.acc_y, msg->data.acc_z;
omega_body_ << msg->data.omega_x, msg->data.omega_y, msg->data.omega_z;
imu_ts_ = rclcpp::Time(msg->header.stamp).seconds();
}
void HandlerHealth(const drdds::msg::BatteryData::SharedPtr msg) {
battery_data_[0] = uint8_t(msg->data[0].battery_level);
remain_bat = battery_data_[0] / 100;
battery_info_[0].battery_level = msg->data[0].battery_level;
}
};