底层控制完成
This commit is contained in:
parent
040a6d8040
commit
761ede8596
@ -17,8 +17,8 @@ def generate_launch_description():
|
|||||||
name='unitree_motor_serial_driver_lf',
|
name='unitree_motor_serial_driver_lf',
|
||||||
parameters=[{'serial_port': '/dev/ttyACM0'}],
|
parameters=[{'serial_port': '/dev/ttyACM0'}],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'LF/data_leg_control'),
|
('motor_cmds', 'LF/motor_cmds'),
|
||||||
('data_leg_motor', 'LF/data_leg_motor')
|
('data_motors', 'LF/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -37,8 +37,8 @@ def generate_launch_description():
|
|||||||
name='unitree_motor_serial_driver_rf',
|
name='unitree_motor_serial_driver_rf',
|
||||||
parameters=[{'serial_port': '/dev/ttyACM1'}],
|
parameters=[{'serial_port': '/dev/ttyACM1'}],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'RF/data_leg_control'),
|
('motor_cmds', 'RF/motor_cmds'),
|
||||||
('data_leg_motor', 'RF/data_leg_motor')
|
('data_motors', 'RF/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -57,8 +57,8 @@ def generate_launch_description():
|
|||||||
name='unitree_motor_serial_driver_lr',
|
name='unitree_motor_serial_driver_lr',
|
||||||
parameters=[{'serial_port': '/dev/ttyACM2'}],
|
parameters=[{'serial_port': '/dev/ttyACM2'}],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'LR/data_leg_control'),
|
('motor_cmds', 'LR/motor_cmds'),
|
||||||
('data_leg_motor', 'LR/data_leg_motor')
|
('data_motors', 'LR/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -77,8 +77,8 @@ def generate_launch_description():
|
|||||||
name='unitree_motor_serial_driver_rr',
|
name='unitree_motor_serial_driver_rr',
|
||||||
parameters=[{'serial_port': '/dev/ttyACM3'}],
|
parameters=[{'serial_port': '/dev/ttyACM3'}],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'RR/data_leg_control'),
|
('motor_cmds', 'RR/motor_cmds'),
|
||||||
('data_leg_motor', 'RR/data_leg_motor')
|
('data_motors', 'RR/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
|
@ -6,7 +6,8 @@
|
|||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "serialPort/SerialPort.h"
|
#include "serialPort/SerialPort.h"
|
||||||
#include "unitreeMotor/unitreeMotor.h"
|
#include "unitreeMotor/unitreeMotor.h"
|
||||||
#include "rc_msgs/msg/data_leg.hpp"
|
#include "rc_msgs/msg/motor_cmds.hpp"
|
||||||
|
#include "rc_msgs/msg/data_motors.hpp"
|
||||||
|
|
||||||
namespace unitree_motor_serial_driver
|
namespace unitree_motor_serial_driver
|
||||||
{
|
{
|
||||||
@ -26,12 +27,12 @@ public:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
void control_motor();
|
void control_motor();
|
||||||
void data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg);
|
void motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg);
|
||||||
void Motor_Ctrl_Offline(Motor_t *motor_s);
|
void Motor_Ctrl_Offline(Motor_t *motor_s);
|
||||||
SerialPort serial_;
|
SerialPort serial_;
|
||||||
rclcpp::TimerBase::SharedPtr timer_;
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
rclcpp::Subscription<rc_msgs::msg::DataLeg>::SharedPtr data_control_sub_;
|
rclcpp::Subscription<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmd_sub_;
|
||||||
rclcpp::Publisher<rc_msgs::msg::DataLeg>::SharedPtr data_motor_pub_;
|
rclcpp::Publisher<rc_msgs::msg::DataMotors>::SharedPtr data_motor_pub_;
|
||||||
int timer_count_ = 0;
|
int timer_count_ = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -18,8 +18,8 @@ def generate_launch_description():
|
|||||||
{'serial_port': '/dev/ttyACM0'}
|
{'serial_port': '/dev/ttyACM0'}
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'LF/data_leg_control'),
|
('motor_cmds', 'LF/motor_cmds'),
|
||||||
('data_leg_motor', 'LF/data_leg_motor')
|
('data_motors', 'LF/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -41,8 +41,8 @@ def generate_launch_description():
|
|||||||
{'serial_port': '/dev/ttyACM1'}
|
{'serial_port': '/dev/ttyACM1'}
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'RF/data_leg_control'),
|
('motor_cmds', 'RF/motor_cmds'),
|
||||||
('data_leg_motor', 'RF/data_leg_motor')
|
('data_motors', 'RF/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -64,8 +64,8 @@ def generate_launch_description():
|
|||||||
{'serial_port': '/dev/ttyACM2'}
|
{'serial_port': '/dev/ttyACM2'}
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'LR/data_leg_control'),
|
('motor_cmds', 'LR/motor_cmds'),
|
||||||
('data_leg_motor', 'LR/data_leg_motor')
|
('data_motors', 'LR/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
@ -87,8 +87,8 @@ def generate_launch_description():
|
|||||||
{'serial_port': '/dev/ttyACM3'}
|
{'serial_port': '/dev/ttyACM3'}
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'RR/data_leg_control'),
|
('motor_cmds', 'RR/motor_cmds'),
|
||||||
('data_leg_motor', 'RR/data_leg_motor')
|
('data_motors', 'RR/data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
|
@ -19,8 +19,8 @@ def generate_launch_description():
|
|||||||
{'serial_retry_delay_ms': 1000}
|
{'serial_retry_delay_ms': 1000}
|
||||||
],
|
],
|
||||||
remappings=[
|
remappings=[
|
||||||
('data_leg_control', 'data_leg_control'),
|
('motor_cmds', 'motor_cmds'),
|
||||||
('data_leg_motor', 'data_leg_motor')
|
('data_motors', 'data_motors')
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
],
|
],
|
||||||
|
@ -4,10 +4,10 @@
|
|||||||
namespace unitree_motor_serial_driver
|
namespace unitree_motor_serial_driver
|
||||||
{
|
{
|
||||||
|
|
||||||
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
|
MotorControlNode::MotorControlNode(const rclcpp::NodeOptions & options)
|
||||||
: Node("unitree_motor_serial_driver", options),
|
: Node("unitree_motor_serial_driver", options),
|
||||||
serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
|
serial_(this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0")) // 从参数中获取初始值
|
||||||
{
|
{
|
||||||
// 声明其他参数
|
// 声明其他参数
|
||||||
auto retry_count = this->declare_parameter("serial_retry_count", 5); // 默认重试5次
|
auto retry_count = this->declare_parameter("serial_retry_count", 5); // 默认重试5次
|
||||||
auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000); // 默认延迟1秒
|
auto retry_delay_ms = this->declare_parameter("serial_retry_delay_ms", 1000); // 默认延迟1秒
|
||||||
@ -15,32 +15,46 @@ namespace unitree_motor_serial_driver
|
|||||||
bool connection_success = false;
|
bool connection_success = false;
|
||||||
int attempts = 0;
|
int attempts = 0;
|
||||||
timer_count_ = 0;
|
timer_count_ = 0;
|
||||||
// 其余代码保持不变
|
|
||||||
|
// 定时器
|
||||||
timer_ = this->create_wall_timer(
|
timer_ = this->create_wall_timer(
|
||||||
std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
|
std::chrono::microseconds(1000), std::bind(&MotorControlNode::control_motor, this));
|
||||||
data_control_sub_ = this->create_subscription<rc_msgs::msg::DataLeg>(
|
|
||||||
"data_leg_control", 10, std::bind(&MotorControlNode::data_control_callback, this, std::placeholders::_1));
|
|
||||||
data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataLeg>("data_leg_motor", 10);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
// 订阅 MotorCmds 消息
|
||||||
|
motor_cmd_sub_ = this->create_subscription<rc_msgs::msg::MotorCmds>(
|
||||||
|
"motor_cmds", 10, std::bind(&MotorControlNode::motor_cmd_callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
void MotorControlNode::data_control_callback(const rc_msgs::msg::DataLeg::SharedPtr msg)
|
// 发布 DataMotors 消息
|
||||||
|
data_motor_pub_ = this->create_publisher<rc_msgs::msg::DataMotors>("data_motors", 10);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotorControlNode::motor_cmd_callback(const rc_msgs::msg::MotorCmds::SharedPtr msg)
|
||||||
{
|
{
|
||||||
// 将接收到的 DataLeg 消息分别赋值给 motor_t 数组
|
// 将接收到的 MotorCmds 消息分别赋值给 motor_t 数组
|
||||||
for (int i = 0; i < 3; ++i)
|
motor[0].cmd.tau = msg->motor_cmd_0.tau;
|
||||||
{
|
motor[0].cmd.dq = msg->motor_cmd_0.vw;
|
||||||
motor[i].cmd.kp = msg->data_control_0.kp;
|
motor[0].cmd.q = msg->motor_cmd_0.pos;
|
||||||
motor[i].cmd.kd = msg->data_control_0.kd;
|
motor[0].cmd.kp = msg->motor_cmd_0.kp;
|
||||||
motor[i].cmd.q = msg->data_control_0.pos * queryGearRatio(MotorType::GO_M8010_6);
|
motor[0].cmd.kd = msg->motor_cmd_0.kd;
|
||||||
motor[i].cmd.dq = msg->data_control_0.vw * queryGearRatio(MotorType::GO_M8010_6);
|
|
||||||
motor[i].cmd.tau = msg->data_control_0.tau;
|
motor[1].cmd.tau = msg->motor_cmd_1.tau;
|
||||||
}
|
motor[1].cmd.dq = msg->motor_cmd_1.vw;
|
||||||
|
motor[1].cmd.q = msg->motor_cmd_1.pos;
|
||||||
|
motor[1].cmd.kp = msg->motor_cmd_1.kp;
|
||||||
|
motor[1].cmd.kd = msg->motor_cmd_1.kd;
|
||||||
|
|
||||||
|
motor[2].cmd.tau = msg->motor_cmd_2.tau;
|
||||||
|
motor[2].cmd.dq = msg->motor_cmd_2.vw;
|
||||||
|
motor[2].cmd.q = msg->motor_cmd_2.pos;
|
||||||
|
motor[2].cmd.kp = msg->motor_cmd_2.kp;
|
||||||
|
motor[2].cmd.kd = msg->motor_cmd_2.kd;
|
||||||
|
|
||||||
timer_count_ = 0;
|
timer_count_ = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotorControlNode::control_motor()
|
void MotorControlNode::control_motor()
|
||||||
{
|
{
|
||||||
rc_msgs::msg::DataLeg motor_msg;
|
rc_msgs::msg::DataMotors motor_msg;
|
||||||
|
|
||||||
for (int i = 0; i < 3; ++i)
|
for (int i = 0; i < 3; ++i)
|
||||||
{
|
{
|
||||||
@ -52,11 +66,11 @@ void MotorControlNode::control_motor()
|
|||||||
cmd.id = i;
|
cmd.id = i;
|
||||||
cmd.kp = motor[i].cmd.kp;
|
cmd.kp = motor[i].cmd.kp;
|
||||||
cmd.kd = motor[i].cmd.kd;
|
cmd.kd = motor[i].cmd.kd;
|
||||||
cmd.q = motor[i].cmd.q;
|
cmd.q = motor[i].cmd.q * queryGearRatio(MotorType::GO_M8010_6);
|
||||||
cmd.dq = motor[i].cmd.dq;
|
cmd.dq = motor[i].cmd.dq * queryGearRatio(MotorType::GO_M8010_6);
|
||||||
cmd.tau = motor[i].cmd.tau;
|
cmd.tau = motor[i].cmd.tau;
|
||||||
|
|
||||||
if (timer_count_ = 100)
|
if (timer_count_ == 100)
|
||||||
{
|
{
|
||||||
Motor_Ctrl_Offline(&motor[i]);
|
Motor_Ctrl_Offline(&motor[i]);
|
||||||
}
|
}
|
||||||
@ -71,7 +85,7 @@ void MotorControlNode::control_motor()
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 填充 DataLeg 消息
|
// 填充 DataMotors 消息
|
||||||
if (i == 0)
|
if (i == 0)
|
||||||
{
|
{
|
||||||
motor_msg.data_motor_0.tau = data.tau;
|
motor_msg.data_motor_0.tau = data.tau;
|
||||||
@ -110,7 +124,7 @@ void MotorControlNode::control_motor()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 发布 DataLeg 消息
|
// 发布 DataMotors 消息
|
||||||
data_motor_pub_->publish(motor_msg);
|
data_motor_pub_->publish(motor_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user