caowangrenbo
7ba2bba450
- MotionState 增加 error_code，新增 invicta_msgs/error_code.h 共用位定义；SafeController 写入、remote 展开成协议 ErrorList（component 暂为 0） - InvictaInterface 增加 IMU 断流/恢复判定（imu_timeout_s / imu_recover_s），covariance=-1 视为不可用；RL 丢失 IMU 走正常趴下 - invicta_hardware 不再发布硬编码单位四元数 IMU（含错误的 covariance[0]=0） - 更新 README 与 docs/TODO.md：记录已完成项、反馈帧 CANID 1~4 已确认、[joint_limits] 重复维护入待办
265 lines
12 KiB
C++
265 lines
12 KiB
C++
/**
|
||
* @file invicta_interface.hpp
|
||
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题来自 invicta_deploy.toml
|
||
*
|
||
* 本层只做**模型帧**的透传：/joint_states 与 /joint_commands 上都是模型量（rad / rad/s / Nm）。
|
||
* 硬件的方向 `dir` 与零点 `offset` 由 invicta_hardware 在 raw↔model 的唯一边界处施加
|
||
* （见 invicta_hardware/README.md「标定边界」），deploy 不再参与标定。
|
||
* 上层状态机与策略只看到模型约定下的量。
|
||
*
|
||
* 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
|
||
* 本构型摆臂行程只有 ±80°，无回绕），也删掉了本地 dir/offset 换算（已上移到 hardware）。
|
||
*/
|
||
#pragma once
|
||
|
||
#include <ros/ros.h>
|
||
#include <sensor_msgs/Imu.h>
|
||
#include <sensor_msgs/JointState.h>
|
||
|
||
#include <limits>
|
||
#include <string>
|
||
#include <unordered_map>
|
||
#include <vector>
|
||
|
||
#include "basic_function.hpp"
|
||
#include "common_types.h"
|
||
#include "config/deploy_config.h"
|
||
#include "invicta_msgs/JointCommandArray.h"
|
||
#include "invicta_msgs/control_word.h"
|
||
#include "robot_interface.h"
|
||
|
||
namespace interface {
|
||
|
||
class InvictaInterface : public RobotInterface {
|
||
public:
|
||
InvictaInterface(const std::string& robot_name, int dof_num)
|
||
: RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
|
||
if (dof_num_ != cfg_.dof) {
|
||
ROS_WARN("[InvictaInterface] passed dof=%d does not match configured %d, using config value", dof_num_, cfg_.dof);
|
||
}
|
||
joint_pos_ = VecXf::Zero(dof_num_);
|
||
joint_vel_ = VecXf::Zero(dof_num_);
|
||
joint_tau_ = VecXf::Zero(dof_num_);
|
||
joint_data_id_.resize(size_t(dof_num_), 0);
|
||
driver_status_.assign(size_t(dof_num_), 1);
|
||
|
||
for (int i = 0; i < dof_num_; ++i) name2slot_[cfg_.joint_names[size_t(i)]] = i;
|
||
|
||
cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cfg_.topic_joint_commands,
|
||
cfg_.queue_joint_commands);
|
||
joint_state_sub_ = nh_.subscribe(cfg_.topic_joint_states, cfg_.queue_state,
|
||
&InvictaInterface::JointStateHandler, this);
|
||
imu_sub_ = nh_.subscribe(cfg_.topic_imu, cfg_.queue_state, &InvictaInterface::ImuHandler, this);
|
||
}
|
||
|
||
virtual ~InvictaInterface() {}
|
||
|
||
// ------------------------------------------------------------------ 接口实现
|
||
virtual void Start() {
|
||
SendStartupSequence();
|
||
std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
|
||
<< cfg_.topic_joint_commands << std::endl;
|
||
std::cout << "[InvictaInterface] model frame passthrough: dir/offset applied by invicta_hardware" << std::endl;
|
||
}
|
||
|
||
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 VecXf GetContactForce() { return VecXf::Zero(4); }
|
||
virtual double GetImuTimestamp() { return imu_ts_; }
|
||
/// 距最近一帧**可用** IMU 的时长 [s]；从未收到过返回极大值（安全线程用，纯读）
|
||
virtual double GetImuAgeSec() {
|
||
if (!has_imu_valid_) return std::numeric_limits<double>::max();
|
||
return (ros::Time::now() - imu_valid_arrival_).toSec();
|
||
}
|
||
/// IMU 是否健康（控制线程用；状态由 RefreshRobotData() 每周期更新）
|
||
virtual bool IsImuHealthy() { return imu_healthy_; }
|
||
virtual std::vector<uint16_t> GetJointDataID() { return joint_data_id_; }
|
||
virtual std::vector<uint16_t> GetDriverStatusWord() { return driver_status_; }
|
||
|
||
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
|
||
joint_cmd_ = input;
|
||
invicta_msgs::JointCommandArray msg;
|
||
msg.header.stamp = ros::Time::now();
|
||
msg.name = cfg_.joint_names;
|
||
msg.position.resize(size_t(dof_num_));
|
||
msg.velocity.resize(size_t(dof_num_));
|
||
msg.effort.resize(size_t(dof_num_));
|
||
msg.kp.resize(size_t(dof_num_));
|
||
msg.kd.resize(size_t(dof_num_));
|
||
for (int i = 0; i < dof_num_; ++i) {
|
||
const auto& j = cfg_.Joint(i);
|
||
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
|
||
msg.position[i] = input(i, 1);
|
||
msg.velocity[i] = input(i, 3);
|
||
msg.effort[i] = input(i, 4);
|
||
msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
|
||
msg.kd[i] = input(i, 2);
|
||
}
|
||
msg.control_word = invicta_msgs::kMotorControl;
|
||
cmd_pub_.publish(msg);
|
||
}
|
||
|
||
/// 每个控制周期由状态机调一次：刷新 IMU 连续性判定（断流判坏 + 恢复去抖，见 RefreshImuHealth）
|
||
virtual void RefreshRobotData() { RefreshImuHealth(); }
|
||
|
||
protected:
|
||
// ------------------------------------------------------------------ 启动序列
|
||
void PublishRawCommand(uint16_t control_word) {
|
||
invicta_msgs::JointCommandArray msg;
|
||
msg.header.stamp = ros::Time::now();
|
||
msg.name = cfg_.joint_names;
|
||
msg.position.assign(size_t(dof_num_), 0.);
|
||
msg.velocity.assign(size_t(dof_num_), 0.);
|
||
msg.effort.assign(size_t(dof_num_), 0.);
|
||
msg.kp.assign(size_t(dof_num_), 0.);
|
||
msg.kd.assign(size_t(dof_num_), 0.);
|
||
msg.control_word = control_word;
|
||
cmd_pub_.publish(msg);
|
||
}
|
||
|
||
/// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
|
||
/// ⚠ ROS1 的发布没有 latch：订阅端连接建立前发出去的消息会被直接丢掉。
|
||
/// 若 hardware 节点后启动（或本节点重启），一次性时序帧会整段丢失，
|
||
/// 所以这里先等订阅者出现再发。
|
||
void SendStartupSequence() {
|
||
for (int i = 0; i < 200 && cmd_pub_.getNumSubscribers() == 0 && ros::ok(); ++i) {
|
||
ros::Duration(0.01).sleep();
|
||
}
|
||
if (cmd_pub_.getNumSubscribers() == 0) {
|
||
std::cout << "[InvictaInterface] warning: joint_commands has no subscribers yet, startup sequence may be lost" << std::endl;
|
||
}
|
||
const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
|
||
for (uint16_t word : seq) {
|
||
PublishRawCommand(word);
|
||
ros::Duration(0.01).sleep();
|
||
}
|
||
std::cout << "[InvictaInterface] startup control sequence complete (disable -> error_reset -> enable)" << std::endl;
|
||
}
|
||
|
||
// ------------------------------------------------------------------ 回调
|
||
void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
|
||
const size_t n = size_t(dof_num_);
|
||
if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
|
||
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s length should be %d, got pos=%zu vel=%zu eff=%zu, dropping frame",
|
||
cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
|
||
msg->effort.size());
|
||
return;
|
||
}
|
||
|
||
const bool by_name = msg->name.size() == n;
|
||
int unknown = 0;
|
||
for (size_t k = 0; k < n; ++k) {
|
||
const int i = by_name ? LookupSlot(msg->name[k]) : int(k);
|
||
if (i < 0) {
|
||
++unknown;
|
||
continue;
|
||
}
|
||
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
|
||
joint_pos_(i) = float(msg->position[k]);
|
||
joint_vel_(i) = float(msg->velocity[k]);
|
||
joint_tau_(i) = float(msg->effort[k]);
|
||
}
|
||
if (!by_name && !warned_name_mismatch_) {
|
||
warned_name_mismatch_ = true;
|
||
ROS_WARN("[InvictaInterface] %s did not provide the agreed %d names, resolving by array index",
|
||
cfg_.topic_joint_states.c_str(), dof_num_);
|
||
}
|
||
if (unknown > 0) {
|
||
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s has %d unknown joint names, ignored",
|
||
cfg_.topic_joint_states.c_str(), unknown);
|
||
}
|
||
|
||
for (int i = 0; i < dof_num_; ++i) joint_data_id_[i] = joint_data_frame_;
|
||
++joint_data_frame_;
|
||
ri_ts_ = msg->header.stamp.toSec();
|
||
}
|
||
|
||
int LookupSlot(const std::string& name) const {
|
||
auto it = name2slot_.find(name);
|
||
return it == name2slot_.end() ? -1 : it->second;
|
||
}
|
||
|
||
void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
|
||
imu_arrival_ = ros::Time::now();
|
||
has_imu_ = true;
|
||
// orientation_covariance[0] == -1 表示「不提供姿态」
|
||
if (msg->orientation_covariance[0] == -1.0) {
|
||
// 帧在来但没有可用姿态：对 RL 而言等同断流（base_rpy 会停在旧值，不能当有效观测）
|
||
has_imu_valid_ = false;
|
||
if (!warned_imu_no_orientation_) {
|
||
warned_imu_no_orientation_ = true;
|
||
ROS_WARN("[InvictaInterface] %s did not provide orientation (covariance=-1), attitude observation invalid",
|
||
cfg_.topic_imu.c_str());
|
||
}
|
||
return;
|
||
}
|
||
Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
|
||
float(msg->orientation.z));
|
||
if (q.norm() < 1e-6f) {
|
||
has_imu_valid_ = false;
|
||
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s quaternion norm near zero, ignoring this frame", cfg_.topic_imu.c_str());
|
||
return;
|
||
}
|
||
q.normalize();
|
||
rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>(); // ZYX，与 RpyToRm 一致
|
||
omega_body_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
|
||
acc_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
|
||
imu_ts_ = msg->header.stamp.toSec();
|
||
imu_valid_arrival_ = imu_arrival_;
|
||
has_imu_valid_ = true;
|
||
}
|
||
|
||
/**
|
||
* IMU 连续性判定（只在控制线程里由 RefreshRobotData() 调一次）：
|
||
* 判坏：最近一帧可用 IMU 的龄期 > rl_imu_timeout_s（或从未收到过、或帧不提供姿态）
|
||
* 恢复：断流之后需要连续 rl_imu_recover_s 的新鲜可用帧才重新判为健康（去抖）
|
||
* 之所以要恢复去抖：否则 IMU 抖动一帧就会立刻放行重新进 RL。
|
||
*/
|
||
void RefreshImuHealth() {
|
||
const ros::Time now = ros::Time::now();
|
||
const bool fresh = has_imu_valid_ && (now - imu_valid_arrival_).toSec() <= cfg_.rl_imu_timeout_s;
|
||
if (!fresh) {
|
||
imu_good_since_valid_ = false;
|
||
imu_healthy_ = false;
|
||
return;
|
||
}
|
||
if (!imu_good_since_valid_) {
|
||
imu_good_since_ = now;
|
||
imu_good_since_valid_ = true;
|
||
}
|
||
if ((now - imu_good_since_).toSec() >= cfg_.rl_imu_recover_s) imu_healthy_ = true;
|
||
}
|
||
|
||
protected:
|
||
const invicta::DeployConfig& cfg_;
|
||
ros::NodeHandle nh_;
|
||
std::unordered_map<std::string, int> name2slot_;
|
||
|
||
double ri_ts_ = 0., imu_ts_ = 0.;
|
||
|
||
// ---- IMU 连续性（见 RefreshImuHealth）----
|
||
ros::Time imu_arrival_, imu_valid_arrival_, imu_good_since_;
|
||
bool has_imu_ = false; ///< 是否收到过任何 IMU 帧（哪怕不提供姿态）
|
||
bool has_imu_valid_ = false; ///< 最近一帧是否提供了可用姿态
|
||
bool imu_good_since_valid_ = false, imu_healthy_ = false;
|
||
Vec3f omega_body_, rpy_, acc_;
|
||
VecXf joint_pos_, joint_vel_, joint_tau_;
|
||
uint16_t joint_data_frame_ = 0;
|
||
std::vector<uint16_t> joint_data_id_;
|
||
std::vector<uint16_t> driver_status_;
|
||
bool warned_name_mismatch_ = false;
|
||
bool warned_imu_no_orientation_ = false;
|
||
|
||
ros::Publisher cmd_pub_;
|
||
ros::Subscriber joint_state_sub_, imu_sub_;
|
||
};
|
||
|
||
}; // namespace interface
|