diff --git a/ecrt_dev/CMakeLists.txt b/ecrt_dev/CMakeLists.txt index ee17977..46d7e50 100644 --- a/ecrt_dev/CMakeLists.txt +++ b/ecrt_dev/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -###find_package(ethercat_interface REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(control_msgs REQUIRED) find_library(ECRT_LIB NAMES ethercat ecrt PATHS /opt/etherlab/lib /usr/local/lib /usr/lib @@ -40,6 +42,7 @@ ament_target_dependencies( add_executable(test_motor src/test_motor.cpp) ament_target_dependencies(test_motor rclcpp + rclcpp_action geometry_msgs sensor_msgs trajectory_msgs diff --git a/ecrt_dev/config/controllers.yaml b/ecrt_dev/config/controllers.yaml index bf5ab02..2a14f08 100644 --- a/ecrt_dev/config/controllers.yaml +++ b/ecrt_dev/config/controllers.yaml @@ -31,7 +31,14 @@ trajectory_controller: state_interfaces: - position - velocity - + state_publish_rate: 200.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + goal_time: 0.5 + stopped_velocity_tolerance: 0.02 gpio_command_controller: ros__parameters: gpios: @@ -52,62 +59,50 @@ gpio_command_controller: - interfaces: - fault - enable - - position joint_2: - interfaces: - fault - enable - - position joint_3: - interfaces: - fault - enable - - position joint_4: - interfaces: - fault - enable - - position joint_5: - interfaces: - fault - enable - - position joint_6: - interfaces: - fault - enable - - position joint_7: - interfaces: - fault - enable - - position joint_8: - interfaces: - fault - enable - - position joint_9: - interfaces: - fault - enable - - position joint_10: - interfaces: - fault - enable - - position joint_11: - interfaces: - fault - enable - - position joint_12: - interfaces: - fault - enable - - position state_interfaces: joint_1: - interfaces: diff --git a/ecrt_dev/description/motor_drive.ros2_control.xacro b/ecrt_dev/description/motor_drive.ros2_control.xacro index e6a3a99..d397780 100644 --- a/ecrt_dev/description/motor_drive.ros2_control.xacro +++ b/ecrt_dev/description/motor_drive.ros2_control.xacro @@ -15,6 +15,7 @@ + diff --git a/ecrt_dev/include/ethercat_driver/zer_config.hpp b/ecrt_dev/include/ethercat_driver/zer_config.hpp index 02f2234..6490ebe 100644 --- a/ecrt_dev/include/ethercat_driver/zer_config.hpp +++ b/ecrt_dev/include/ethercat_driver/zer_config.hpp @@ -1,4 +1,5 @@ #include "ecrt.h" +#include #define NUM_SLAVES 12 #define ZER_VID_PID 0x5a65726f,0x00029252 typedef struct { @@ -20,6 +21,8 @@ typedef struct { static zer_offset_t zer_offsets[12]; +constexpr double rad_to_count= 524288.0/(2*M_PI); +constexpr double count_to_rad=2*M_PI/524288.0; #define PDO_ENTRY(alias,position,index) \ {alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \ diff --git a/ecrt_dev/launch/motor_drive.launch.py b/ecrt_dev/launch/motor_drive.launch.py index af2067f..00d2352 100644 --- a/ecrt_dev/launch/motor_drive.launch.py +++ b/ecrt_dev/launch/motor_drive.launch.py @@ -77,12 +77,12 @@ def generate_launch_description(): arguments=["gpio_command_controller", "-c", "/controller_manager"], ) - position_controller_spawner = Node( + trajectory_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["position_controller", "-c", "/controller_manager"], + arguments=["trajectory_controller", "-c", "/controller_manager"], ) - delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner]) + delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner]) nodes = [ control_node, robot_state_pub_node, diff --git a/ecrt_dev/package.xml b/ecrt_dev/package.xml index 633a29b..a9bd1a1 100644 --- a/ecrt_dev/package.xml +++ b/ecrt_dev/package.xml @@ -12,6 +12,7 @@ hardware_interface pluginlib rclcpp + rclcpp_lifecycle ethercat_interface diff --git a/ecrt_dev/src/ethercat_driver.cpp b/ecrt_dev/src/ethercat_driver.cpp index 6d7968b..6309926 100644 --- a/ecrt_dev/src/ethercat_driver.cpp +++ b/ecrt_dev/src/ethercat_driver.cpp @@ -401,28 +401,30 @@ void EthercatDriver::readData(){ } else { // do this at 1 Hz counter = FREQUENCY; // check for master state (optional) - check_master_state(); //检查主站状态 + //check_master_state(); //检查主站状态 // check for slave configuration state(s) - check_slave_config_states(); //检查从站状态 + //check_slave_config_states(); //检查从站状态 } if (!inited) { for (int i = 0; i < NUM_SLAVES; ++i) { command[i] = 0x004F; status[i] = 0x000F; last_status[i] = status[i]; + EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080); } inited = 1; } for (int i = 0; i < NUM_SLAVES; ++i) { uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word); - int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value); - int32_t vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value); + int32_t pos = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value); + int32_t vel = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value); 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]=pv; - hw_joint_states_[i][1]=err; + hw_joint_states_[i][0]=pos*count_to_rad; + hw_joint_states_[i][1]=vel; + hw_joint_states_[i][2]=40960;//err //hw_joint_states_[i][2]=err; ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],state:%.1f,%.1f,0x%04x",i,hw_joint_states_[i][0],hw_joint_states_[i][1],err); status[i] = sw; //侦测子站状态字变化 @@ -443,8 +445,11 @@ void EthercatDriver::readData(){ ///static double last_enable[12]; double enable=hw_joint_commands_[i][1]; - if(enable!=1) + if(enable!=1){ + EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0); + EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); continue; + } //write if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) { @@ -465,14 +470,13 @@ void EthercatDriver::readData(){ //printf("VVVVVVVV:%d\n",i); } else if ((status[i] & command[i]) == 0x0027) { // ---- 4) 运行态写目标（按模式/运行意图） ---- - //if(enable==1) if(true) { //EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要 switch (mode_cmd) { case CYCLIC_VELOCITY: // 9 - ////////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd); + ////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd); break; case CYCLIC_POSITION: { // 8 // ---- 绝对式目标生成，不再用 pv 做增量基准 ---- @@ -486,21 +490,23 @@ void EthercatDriver::readData(){ // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击 if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) { - csp_cmd_pos[i] = pv; // 先对齐 + csp_cmd_pos[i] = pos; // 先对齐 vmax_acc[i] = 0; csp_inited[i] = 1; } - double target_pos=hw_joint_commands_[i][2]; + double target_pos=hw_joint_commands_[i][2]*rad_to_count; static double last_pos[12]; if(target_pos!=last_pos[i]){ last_pos[i]=target_pos; - } - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],cur:%d,dst:%.1f",i,pv,target_pos); + // 期望的“最终绝对目标” const int32_t goal = target_pos;///g_cmd.target_pos[i].load(std::memory_order_relaxed); ///if(i==0) ///printf("%d,goal:%d,pv:%d,flag:%d\n",i,goal,pv,goal_flag[i]); + static int pos_cnt=0; + //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 vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s] int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长 diff --git a/ecrt_dev/src/test_motor.cpp b/ecrt_dev/src/test_motor.cpp index 4f33cf0..4758296 100644 --- a/ecrt_dev/src/test_motor.cpp +++ b/ecrt_dev/src/test_motor.cpp @@ -4,9 +4,14 @@ #include #include "std_msgs/msg/string.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" #include "control_msgs/msg/dynamic_interface_group_values.hpp" +#include "control_msgs/action/follow_joint_trajectory.hpp" #include "sensor_msgs/msg/joint_state.hpp" +#include "rclcpp_action/rclcpp_action.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; class TestMotor : public rclcpp::Node { public: @@ -19,14 +24,14 @@ namespace fs = std::filesystem; void motor_enable(int id,double enable); void motor_pos(int id,double pos); void motor_loop(int motor_id,int cnt); - void motor_ctl(int id,double pos,double reset,double enable); + void motor_action(int id,double angle); void all_motor(); void ControlLoop(); private: rclcpp::Publisher::SharedPtr cmdPub_; rclcpp::Publisher::SharedPtr gpioPub_; rclcpp::Subscription::SharedPtr jointStatesSub_; - + rclcpp_action::Client::SharedPtr client_; // 文件流相关 std::ofstream data_file_; // 用于写入数据的文件流 std::string data_file_path_; // 数据文件路径 @@ -57,17 +62,21 @@ TestMotor::TestMotor() : Node("test_motor_node") std::cout << "TestMotor Node is created!" << std::endl; // 创建发布者 cmdPub_ = this->create_publisher("/trajectory_controller/joint_trajectory", 10); + //action + client_=rclcpp_action::create_client(this,"/trajectory_controller/follow_joint_trajectory"); // 创建发布者 gpioPub_ = this->create_publisher("/gpio_command_controller/commands", 10); // 订阅仿真状态 jointStatesSub_ = this->create_subscription("/joint_states", 10,std::bind(&TestMotor::JointStatesCallback, this, std::placeholders::_1)); lastTime_ = this->now(); // 初始化时间 // 创建定时器，每10ms执行一次控制逻辑（频率100Hz） - controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(20000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数); + controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(5000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数); curMap_.clear(); dstMap_.clear(); //posMsg_.interface_groups.resize(12); //posMsg_.interface_values.resize(12); + motor_enable(6,1); + motor_enable(13,1); std::cout << "TestMotor Node is created finished!" << std::endl; } @@ -85,7 +94,7 @@ void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPt for(int i=0;iname.size();i++) { curMap_[msg->name[i]] = msg->position[i]; - ///std::cout<name[i]<<":"<name[i]]<name[i]<<":"<name[i]]<publish(posMsg_); - usleep(10000); -} - -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:"; - for(int i=0;i<12;i++){ - std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); - posMsg_.interface_groups.push_back(tempInterfaceGroup); - control_msgs::msg::InterfaceValue tempValue; - //enable - tempValue.interface_names = {"enable"}; - if(id==0){ - tempValue.values = {enable}; - }else{ - if(tempInterfaceGroup!=target_motor) - tempValue.values = {0}; - else - tempValue.values = {enable}; - } - std::cout<publish(posMsg_); - usleep(10000); -} void TestMotor::motor_pos(int id,double delta) { control_msgs::msg::DynamicInterfaceGroupValues posMsg_; @@ -197,40 +151,135 @@ void TestMotor::motor_pos(int id,double delta) gpioPub_->publish(posMsg_); usleep(10000); } -void TestMotor::motor_ctl(int id,double pos,double reset,double enable) +void TestMotor::motor_fault(int id,double fault) { control_msgs::msg::DynamicInterfaceGroupValues posMsg_; - std::string tempInterfaceGroup = "joint_" + std::to_string(id+1); - posMsg_.interface_groups.push_back(tempInterfaceGroup); - control_msgs::msg::InterfaceValue tempValue; - - tempValue.interface_names = {"position"}; - tempValue.values = {pos}; - posMsg_.interface_values.push_back(tempValue); - - tempValue.interface_names = {"fault"}; - tempValue.values = {reset}; - posMsg_.interface_values.push_back(tempValue); - //enable - tempValue.interface_names = {"enable"}; - tempValue.values = {enable}; - std::cout<publish(posMsg_); + usleep(50000); +} +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:"; + for(int i=0;i<12;i++){ + std::string tempInterfaceGroup = "joint_" + std::to_string(i+1); + posMsg_.interface_groups.push_back(tempInterfaceGroup); + control_msgs::msg::InterfaceValue tempValue; + //enable + tempValue.interface_names = {"enable"}; + #if 1 + if(ipublish(posMsg_); + usleep(1000000); +} +void TestMotor::motor_action(int id,double delta) +{ + //std::cout<<"motor_action:"<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); + //if(id==(i+1)) + if(i<12) + point.positions.push_back(curMap_[joint]+delta); + else + point.positions.push_back(curMap_[joint]); + 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::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_loop(int motor_id,int loop_cnt){ std::cout<<"start test:"<0) + motor_loop(mid,sw); + else{ + loop_cnt=0; + sw+=1; + motor_loop(1,sw); + } + + #endif + #if 0 motor_loop(2,loop_cnt); motor_loop(3,loop_cnt); motor_loop(4,loop_cnt); @@ -242,6 +291,7 @@ void TestMotor::all_motor(){ motor_loop(10,loop_cnt); motor_loop(11,loop_cnt); motor_loop(12,loop_cnt); + #endif } int main(int argc,char* argv[]){ rclcpp::init(argc,argv);