HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add controller switch & angle calibration & add effort

Browse Source
This commit is contained in:
hehe
2026-01-08 18:40:11 +08:00
parent 0fc09edd73
commit 93f31592d8
8 changed files with 256 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ecrt_dev/CMakeLists.txt
View File

@@ -14,6 +14,7 @@ find_package(rclcpp_lifecycle REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(controller_manager_msgs REQUIRED)
find_library(ECRT_LIB
NAMES ethercat ecrt
PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
@@ -48,6 +49,7 @@ ament_target_dependencies(test_motor
sensor_msgs
trajectory_msgs
control_msgs
controller_manager_msgs
)
install(TARGETS test_motor DESTINATION lib/${PROJECT_NAME})

4
ecrt_dev/config/controllers.yaml
View File

@@ -32,11 +32,13 @@ trajectory_controller:
state_interfaces:
- position
- velocity
allow_nonzero_velocity_at_trajectory_end: true
allow_nonzero_velocity_at_trajectory_end: false
state_publish_rate: 200.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
open_loop_control: true
set_last_command_interface_value_as_state_on_activation: false
cmd_timeout: 0.0
allow_integration_in_goal_trajectories: true
constraints:
goal_time: 0.5

1
ecrt_dev/description/motor_drive.ros2_control.xacro
View File

@@ -16,6 +16,7 @@
<!-- 状态接口（位置/速度/力矩） -->
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<state_interface name="fault"/>
<!-- 命令接口（位置控制/故障重置/使能） -->

18
ecrt_dev/include/ethercat_driver/ethercat_driver.hpp
View File

@@ -33,7 +33,7 @@
#include <pthread.h>
#include <sched.h>
#include <sys/mman.h>
#include <atomic>
//#include "ethercat_interface/ec_slave.hpp"
//#include "ethercat_interface/ec_master.hpp"
#include "zer_config.hpp"
@@ -118,12 +118,18 @@ private:
ec_domain_state_t domain1_state = {};
ec_master_state_t master_state = {};
ec_slave_config_state_t sc_state[NUM_SLAVES] = {};
bool all_motor_op=false;
std::mutex ec_mutex_;
bool activated_;
///bool activated_;
std::atomic<bool> activated_{false};
#define FREQUENCY 1000
#define CSP_MAX_VEL_COUNTS_PER_S 65536
#define CSP_POS_DEADBAND 10 //CSP允许的误差(计数)
#define CSP_MAX_VEL_COUNTS_PER_S 10000//65536
#define CSP_POS_DEADBAND 10
#define CSP_DEADBAND1 100 //CSP允许的误差(计数),原来是10
#define CSP_DEADBAND2 500
#define CSP_SPEED1 20
#define CSP_SPEED2 50
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms
@@ -135,6 +141,8 @@ private:
uint16_t status[NUM_SLAVES]; //状态字
uint16_t last_status[NUM_SLAVES]; //上个循环的状态字
int8_t last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};
uint8_t reach_target[NUM_SLAVES];
uint8_t op_states[NUM_SLAVES]={0};
int8_t mode_cmd=8;
int inited = 0; //初始化
unsigned int counter = 0;

6
ecrt_dev/include/ethercat_driver/zer_config.hpp
View File

@@ -20,10 +20,12 @@ typedef struct {
} zer_offset_t;
static zer_offset_t zer_offsets[NUM_SLAVES];
static double rated_currents[NUM_SLAVES]={12.5,12.5,6.3,5.4,5.4,5.4,12.5,12.5,6.3,5.4,5.4,5.4};
static double pos_offsets[NUM_SLAVES]={248214.0,358098.0,251704.0,240977.0,280113.0,54646.0, 211980.0,262157.0,281128.0,270017.0,263697.0,275363.0};
constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
constexpr double speed_to_count=262144.0;
constexpr double speed_to_count=524288.0/(2*M_PI);//262144.0/(2*M_PI);
constexpr double count_to_speed=2*M_PI/524288.0;//2*M_PI/262144.0;
#define PDO_ENTRY(alias,position,index) \
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \

2
ecrt_dev/launch/motor_drive.launch.py
View File

@@ -80,7 +80,7 @@ def generate_launch_description():
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["trajectory_controller", "-c", "/controller_manager"],
arguments=["trajectory_controller", "-c", "/controller_manager", '--inactive'],
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
#delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner])

169
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -22,12 +22,13 @@ CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo &
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
int cpu_core=2;
int cpu_core=3;
CPU_SET(cpu_core, &cpuset);
if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset) != 0) {
RCLCPP_ERROR(rclcpp::get_logger("hehe"), "Failed to set CPU affinity to core %d!", cpu_core);
return CallbackReturn::ERROR;
}
struct sched_param param;
param.sched_priority = 99;
@@ -42,7 +43,7 @@ CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo &
"Failed to lock memory: %s", strerror(errno));
}
const std::lock_guard<std::mutex> lock(ec_mutex_);
activated_ = false;
activated_.store(false,std::memory_order_relaxed);
for(int i=0;i<NUM_SLAVES;i++){
set_motor_enable(i+1,false);
}
@@ -185,6 +186,8 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info
CallbackReturn EthercatDriver::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(rclcpp::get_logger("hehe"),"on configure......");
return CallbackReturn::SUCCESS;
}
@@ -282,7 +285,7 @@ CallbackReturn EthercatDriver::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
const std::lock_guard<std::mutex> lock(ec_mutex_);
if (activated_) {
if (activated_.load(std::memory_order_relaxed)) {
RCLCPP_FATAL(rclcpp::get_logger("EthercatDriver"), "Double on_activate()");
return CallbackReturn::ERROR;
}
@@ -303,12 +306,15 @@ CallbackReturn EthercatDriver::on_activate(
for(int i=0;i<NUM_SLAVES;i++){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"slave:%d,ctl offset:%d,sta offset:%d\n",i,zer_offsets[i].ctrl_word,zer_offsets[i].status_word);
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"activate finish");
for(int i=0;i<info_.joints.size();i++){
for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
hw_joint_commands_[i][j]=0.0;
}
activated_ = true;
}
for(int i=0;i<NUM_SLAVES;i++)
RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.1f",hw_joint_commands_[i][2]);
activated_.store(true,std::memory_order_relaxed);
return CallbackReturn::SUCCESS;
}
@@ -318,6 +324,7 @@ CallbackReturn EthercatDriver::on_deactivate(
const std::lock_guard<std::mutex> lock(ec_mutex_);
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "SSSSSSSSSSSSSSSSSSSSSSSS...");
for(int i=0;i<NUM_SLAVES;i++){
///RCLCPP_INFO(rclcpp::get_logger("hehe"),"close state:%.1f",hw_joint_states_[i][0]);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
// CSV 安全：速度清零
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
@@ -328,7 +335,7 @@ CallbackReturn EthercatDriver::on_deactivate(
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
}
activated_ = false;
activated_.store(false,std::memory_order_relaxed);
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
return CallbackReturn::SUCCESS;
}
@@ -340,9 +347,10 @@ hardware_interface::return_type EthercatDriver::read(
const rclcpp::Duration & /*period*/)
{
// try to lock so we can avoid blocking the read/write loop on the lock.
const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
////const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
//printf("read data...\n");
if (lock.owns_lock() && activated_) {
//if (lock.owns_lock() && activated_) {
if (activated_.load(std::memory_order_relaxed)){
////master_.readData();
auto start=std::chrono::high_resolution_clock::now();
readData();
@@ -393,8 +401,12 @@ bool EthercatDriver::check_slave_config_states(void)
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[S%02d] %s,State 0x%02X,OP %s", i, s.online ? "online" : "offline",s.al_state,s.operational ? "OK" : "ERR");
}
sc_state[i] = s;
if(!s.operational)
if(!s.operational){
op_states[i]=0;
all_op=false;
}else{
op_states[i]=1;
}
}
return all_op;
}
@@ -412,6 +424,8 @@ struct timespec EthercatDriver::timespec_add(struct timespec time1, struct times
return result;
}
#include<sstream>
static std::stringstream logStream;
void EthercatDriver::readData(){
static int print_cnt=0;
@@ -428,7 +442,17 @@ void EthercatDriver::readData(){
}
#endif
wakeupTime = timespec_add(wakeupTime, cycletime);
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
int ret = clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
if (ret != 0) {
// 睡眠失败（如wakeupTime落后于当前时间），重置唤醒时间
clock_gettime(CLOCK_TO_USE, &wakeupTime); // 获取当前时间
wakeupTime = timespec_add(wakeupTime, cycletime); // 重新计算下一个周期
RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"),
"clock_nanosleep failed: %s, reset wakeupTime", strerror(ret));
}
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间
ecrt_master_receive(master); //接收 EtherCAT 帧
ecrt_domain_process(domain1); //处理域数据
@@ -440,11 +464,12 @@ void EthercatDriver::readData(){
// check for master state (optional)
check_master_state(); //检查主站状态
// check for slave configuration state(s)
bool all_op=check_slave_config_states(); //检查从站状态
if(all_op){
all_motor_op=check_slave_config_states(); //检查从站状态
if(all_motor_op){
if(!has_clear_all){
has_clear_all=true;
for (int i = 0; i < NUM_SLAVES; ++i) {
reach_target[i]=1;
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
if(err>0){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],first code 0x%04x",i,err);
@@ -467,6 +492,11 @@ void EthercatDriver::readData(){
}
inited = 1;
}
// if(print_cnt%1000==1){
// ////for(int i=0;i<NUM_SLAVES;i++)
// ///RCLCPP_INFO(rclcpp::get_logger("hehe"),"new cmd %d:%.1f",i,hw_joint_commands_[i][2]);
// }
logStream.str("");
for (int i = 0; i < NUM_SLAVES; ++i)
{
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
@@ -475,11 +505,37 @@ void EthercatDriver::readData(){
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
hw_joint_states_[i][0]=std::round(pos*count_to_rad * 1000.0) / 1000.0;
hw_joint_states_[i][1]=std::round(vel);
hw_joint_states_[i][2]=err;
double up_pos=pos-pos_offsets[i];
double angle=std::round(up_pos*count_to_rad * 1000.0) / 1000.0;
if(op_states[i]==1){
hw_joint_states_[i][0]=angle;
hw_joint_states_[i][1]=std::round(vel*count_to_speed*1000.0)/1000.0;
double amp=tv*rated_currents[i];
hw_joint_states_[i][2]=std::round(amp*10.0)/10.0;
}else{
hw_joint_states_[i][0]=9999;
hw_joint_states_[i][1]=9999;
hw_joint_states_[i][2]=9999;
}
hw_joint_states_[i][3]=err;
//hw_joint_states_[i][2]=err;
///
// if(print_cnt%1000==0){
// logStream<<i<<":"<<pos<<"=>"<<up_pos<<","<<angle<<std::endl;
// // if(i==5){
// // std::cout<<"X "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }else if(i==11){
// // std::cout<<"Z "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }
// if(i==11){
// std::cout<<logStream.str()<<std::endl;
// logStream.str("");
// }
// }
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],p:%d,v:%d,0x%04x,%d",i,pos,vel,err,(sw&0x006f)==0x0008);
status[i] = sw; //侦测子站状态字变化
@@ -533,7 +589,7 @@ void EthercatDriver::readData(){
}
}
#endif
if(errStaArr[i]!=ERR_NONE)
if(errStaArr[i]!=ERR_NONE||op_states[i]!=1)
continue;
double enable=hw_joint_commands_[i][1];
//if(print_cnt%500==0)
@@ -541,6 +597,9 @@ void EthercatDriver::readData(){
if(enable!=1.0){
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
hw_joint_commands_[i][2]=0;
//if(print_cnt%1000==1)
// RCLCPP_INFO(rclcpp::get_logger("hehe"),"force cmd %d:%.1f",i,hw_joint_commands_[i][2]);
continue;
}
//write
@@ -549,19 +608,20 @@ void EthercatDriver::readData(){
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
///printf("SS:%d\n",i);
//hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
// 同步当前位置为目标位置
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
///hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
/////g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
//printf("TTTTTTTTT:%d\n",i);
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
//printf("VVVVVVVV:%d\n",i);
//hw_joint_commands_[i][2]=0;//hw_joint_states_[i][0];
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
//if(enable==1)
@@ -586,36 +646,33 @@ void EthercatDriver::readData(){
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
double target_pos=hw_joint_commands_[i][2]*rad_to_count;
if(target_pos<0.001)
double down_pos=hw_joint_commands_[i][2]*rad_to_count;
double target_pos=down_pos+pos_offsets[i];
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,cmd:%.1f,%.1f,dst:%.1f",i,pos,hw_joint_commands_[i][2],down_pos,target_pos);
if(target_pos>550000)
continue;
// static double last_pos[12];
// if(target_pos!=last_pos[i]){f
// last_pos[i]=target_pos;
// double diff=std::abs(target_pos-234567.0);
// if(diff>40000){
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value too big [%d],cur:%d,dst:%.1f",i,pos,target_pos);
// continue;
// }else{
// //if(print_cnt%500==0)
// // RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,dst:%.1f",i,pos,target_pos);
// }
double diff=std::abs(target_pos-234567.0);
if(diff>40000){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value too big [%d],cur:%d,dst:%.1f",i,pos,target_pos);
continue;
}else{
//if(print_cnt%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,dst:%.1f",i,pos,target_pos);
}
// 期望的“最终绝对目标”
const int32_t goal = target_pos;
///const int32_t goal = target_pos;
///
//if(pos_cnt++%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pos,target_pos);
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
#if 0
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
// 相对“命令轨迹”的误差（不是相对 pv）
const int32_t err_cmd = goal - csp_cmd_pos[i];
// 死区：足够近就贴合到 goal
const int32_t goal=target_pos;
const int32_t err_cmd = goal -csp_cmd_pos[i];
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
csp_cmd_pos[i] = goal;
} else {
@@ -623,7 +680,32 @@ void EthercatDriver::readData(){
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
}
#else
const int32_t err_cmd=target_pos-pos;
if (abs(err_cmd)<CSP_DEADBAND1) {
csp_cmd_pos[i] = target_pos;
reach_target[i]=1;
}else if(abs(err_cmd)<CSP_DEADBAND2){
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED1;
else
csp_cmd_pos[i]-=CSP_SPEED1;
reach_target[i]=0;
} else {
if(reach_target[i]==1){
reach_target[i]=0;
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED1;
else
csp_cmd_pos[i]-=CSP_SPEED1;
}else{
if(err_cmd>0)
csp_cmd_pos[i]+=CSP_SPEED2;
else
csp_cmd_pos[i]-=CSP_SPEED2;
}
}
#endif
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
@@ -683,9 +765,10 @@ hardware_interface::return_type EthercatDriver::write(
const rclcpp::Duration & /*period*/)
{
// try to lock so we can avoid blocking the read/write loop on the lock.
const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
/// const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
//printf("write data...\n");
if (lock.owns_lock() && activated_) {
///if (lock.owns_lock() && activated_)
{
//writeData();
}
return hardware_interface::return_type::OK;

148
ecrt_dev/src/test_motor.cpp
View File

@@ -13,6 +13,8 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <controller_manager_msgs/srv/switch_controller.hpp>
namespace fs = std::filesystem;
constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
@@ -31,12 +33,16 @@ constexpr double count_to_rad=2*M_PI/524288.0;
void motor_action(int id,double angle);
void all_motor();
void ControlLoop();
bool key_available();
bool activateController(const std::string& controller_name);
void setJointValueTarget(double angle);
bool motor_drv_on=false;
bool all_motor_op=false;
private:
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr client_;
rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client;
// 文件流相关
std::ofstream data_file_; // 用于写入数据的文件流
std::string data_file_path_; // 数据文件路径
@@ -44,18 +50,15 @@ constexpr double count_to_rad=2*M_PI/524288.0;
rclcpp::TimerBase::SharedPtr controlTimer_;
rclcpp::Time lastTime_; // 移至类成员
struct termios original_termios_;
bool isRunning_;
bool isPaused_;
bool isFinished_;
bool isRobotEnabled_;
bool enableCommandExecuted_;
int loop_cnt=0;
int jogDirection_;
//add by hehe
std::unordered_map<std::string,double> curMap_;
std::unordered_map<std::string,double> dstMap_;
//control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
sensor_msgs::msg::JointState curJointSta_;
int loop_cnt=0;
//add by hehe end
// 执行当前状态对应的动作
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
@@ -69,6 +72,7 @@ TestMotor::TestMotor() : Node("test_motor_node")
cmdPub_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/trajectory_controller/joint_trajectory", 10);
//action
client_=rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(this,"/trajectory_controller/follow_joint_trajectory");
switch_client = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
// 创建发布者
gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
// 订阅仿真状态
@@ -80,14 +84,8 @@ TestMotor::TestMotor() : Node("test_motor_node")
dstMap_.clear();
//posMsg_.interface_groups.resize(12);
//posMsg_.interface_values.resize(12);
std::cout << "start enblex ..." << std::endl;
// motor_enable(3,1);
// sleep(2);
// motor_enable(6,1);
// sleep(2);
// //motor_enable(9,1);
// sleep(2);
//motor_enable(13,1);
///std::cout << "start enblex ..." << std::endl;
std::cout << "TestMotor Node is created finished!" << std::endl;
}
@@ -95,37 +93,37 @@ TestMotor::~TestMotor()
{
std::cout << "Robot controller stopped." << std::endl;
}
bool TestMotor::key_available() {
struct termios new_termios;
memcpy(&new_termios, &original_termios_, sizeof(termios));
// 将终端设置为非规范模式，立即读取输入
new_termios.c_lflag &= ~(ICANON | ECHO);
new_termios.c_cc[VMIN] = 0; // 读取时不需要等待最小字符数
new_termios.c_cc[VTIME] = 0; // 读取等待时间为0
tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
int bytes_waiting;
ioctl(STDIN_FILENO, FIONREAD, &bytes_waiting);
// 立即恢复原始终端设置
tcsetattr(STDIN_FILENO, TCSANOW, &original_termios_);
return bytes_waiting > 0;
}
void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
if (!msg) { // 检查消息是否有效
std::cout << "get null joint states!" << std::endl;
return;
}
all_motor_op=true;
for(int i=0;i<msg->name.size();i++)
{
curMap_[msg->name[i]] = msg->position[i];
///std::cout<<"cur:"<<msg->name[i]<<":"<<curMap_[msg->name[i]]<<std::endl;
if(curMap_[msg->name[i]]==9999.0)
all_motor_op=false;
}
if(msg->name.size()>0)
motor_drv_on=true;
}
bool TestMotor::activateController(const std::string& controller_name) {
///std::cout<<"激活控制器"<<controller_name<<std::endl;
auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
request->activate_controllers = {controller_name};
request->deactivate_controllers = {};
request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
request->activate_asap = true;
request->timeout = rclcpp::Duration::from_seconds(5.0);
auto future = switch_client->async_send_request(request);
auto result = future.get();
return result->ok;
}
int TestMotor::motor_dst(std::string name,double dst){
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
double val=curMap_[name];
@@ -152,7 +150,7 @@ int TestMotor::motor_dst(std::string name,double dst){
// 状态机主循环
void TestMotor::ControlLoop() {
//Gpio_publish_joint_trajectory();
all_motor();
////all_motor();
}
void TestMotor::motor_pos(int id,double delta)
{
@@ -210,7 +208,7 @@ void TestMotor::motor_enable(int id,double enable)
{
control_msgs::msg::DynamicInterfaceGroupValues posMsg_;
std::string target_motor="joint_" + std::to_string(id);
std::cout<<"enable:";
////std::cout<<"enable:";
for(int i=0;i<12;i++){
std::string tempInterfaceGroup = "joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
@@ -232,15 +230,50 @@ void TestMotor::motor_enable(int id,double enable)
tempValue.values = {enable};
}
#endif
std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
///std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
///std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(1000000);
}
void TestMotor::setJointValueTarget(double angle){
if(client_->wait_for_action_server(std::chrono::seconds(5))){
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
trajectory_msgs::msg::JointTrajectoryPoint point;
for(int i=0;i<12;i++){
std::string joint="joint_"+std::to_string(i+1);
point.positions.push_back(angle);
//////printf("%s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
}
goal.trajectory.points.push_back(point);
auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
send_goal_option.goal_response_callback=[this](auto res){
auto goal_handle=res.get();
if(goal_handle){
printf("goal has be accept!!!\n");
}
};
send_goal_option.feedback_callback=[this](auto,auto feedback){
for(int i=0;i<12;i++){
printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
}
};
send_goal_option.result_callback=[this](auto ret){
if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
printf("action ret succeed\n");
else if(ret.code==rclcpp_action::ResultCode::ABORTED)
printf("action ret aborted\n");
};
client_->async_send_goal(goal,send_goal_option);
}else{
printf("wait action server error\n");
}
}
void TestMotor::motor_action(int id,double delta)
{
//return;
//std::cout<<"motor_action:"<<id<<","<<delta<<std::endl;
if(client_->wait_for_action_server(std::chrono::seconds(5))){
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
@@ -286,10 +319,10 @@ void TestMotor::motor_loop(int motor_id,int loop_cnt){
motor_enable(13,1);
if(loop_cnt%2==0)
//motor_pos(motor_id,3000);
motor_action(motor_id,-0.04);
motor_action(motor_id,0.1);
else
//motor_pos(motor_id,-3000);
motor_action(motor_id,0.04);
motor_action(motor_id,-0.1);
///usleep(3*1000000);
}
void TestMotor::all_motor(){
@@ -322,9 +355,40 @@ void TestMotor::all_motor(){
int main(int argc,char* argv[]){
rclcpp::init(argc,argv);
auto node=std::make_shared<TestMotor>();
auto spin_thread=std::thread([=](){
rclcpp::spin(node);
});
std::cout<<"等待电机驱动启动...\n";
while(!node->motor_drv_on){
sleep(1);
}
std::cout<<"电机驱动启动完毕!\n等待电机进入OP...\n";
//std::cout<<""<<std::endl;
while(!node->all_motor_op){
sleep(1);
}
std::cout<<"电机进入OP!\n切换控制器...\n";
bool ret=node->activateController("trajectory_controller");
//if(ret)
{
std::cout<<"切换控制器成功!\n准备电机使能...\n";
node->motor_enable(13,1);
std::cout<<"电机使能完毕!\n开始执行角度0...\n";
node->setJointValueTarget(0);
sleep(10);
std::cout<<"角度0执行完毕!\n开始执行角度0.785...\n";
node->setJointValueTarget(0.785);
sleep(10);
std::cout<<"角度0.785执行完毕!\n开始执行复位...\n";
node->setJointValueTarget(0);
sleep(10);
std::cout<<"复位完毕!\n禁能机械臂电机...\n";
node->motor_enable(13,0);
std::cout<<"禁能机械臂电机完毕!\n";
}
///usleep(10000000);
///node->all_motor();
rclcpp::spin(node);
rclcpp::shutdown();
spin_thread.join();
return 0;
}