diff --git a/ecrt_dev/CMakeLists.txt b/ecrt_dev/CMakeLists.txt deleted file mode 100644 index bf4c8f7..0000000 --- a/ecrt_dev/CMakeLists.txt +++ /dev/null @@ -1,105 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ecrt_driver) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -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 -) -if(NOT ECRT_LIB) - message(FATAL_ERROR "ecrt not found; set CMAKE_PREFIX_PATH or system ld path.") -endif() -add_library(${PROJECT_NAME} SHARED src/ethercat_driver.cpp) -target_link_libraries(${PROJECT_NAME} ${ECRT_LIB} pthread) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include -) - -ament_target_dependencies( - ${PROJECT_NAME} - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - rclcpp_action -) - - -# 构建可执行文件 -add_executable(test_motor src/test_motor.cpp) -ament_target_dependencies(test_motor - rclcpp - rclcpp_action - geometry_msgs - sensor_msgs - trajectory_msgs - control_msgs - controller_manager_msgs -) -install(TARGETS test_motor DESTINATION lib/${PROJECT_NAME}) - -#add_executable(key_ctl src/key_ctl.cpp) -#ament_target_dependencies(key_ctl -# rclcpp -# rclcpp_action -# geometry_msgs -# sensor_msgs -# trajectory_msgs -# control_msgs -#) -#install(TARGETS key_ctl DESTINATION lib/${PROJECT_NAME}) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "ETHERCAT_DRIVER_BUILDING_LIBRARY") -# prevent pluginlib from using boost -target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(hardware_interface ethercat_driver_plugin.xml) - -# INSTALL -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib -) -install( - DIRECTORY include/ - DESTINATION include -) -install( - DIRECTORY config description launch - DESTINATION share/${PROJECT_NAME} -) -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) -endif() - -## EXPORTS -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - ethercat_interface -) -ament_package() diff --git a/ecrt_dev/config/controllers.yaml b/ecrt_dev/config/controllers.yaml deleted file mode 100644 index 44ee106..0000000 --- a/ecrt_dev/config/controllers.yaml +++ /dev/null @@ -1,158 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 250 # Hz - rt_thread_core: 0 - rt_thread_priority: 90 - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - gpio_command_controller: - type: gpio_controllers/GpioCommandController - -trajectory_controller: - ros__parameters: - joints: - - left_arm_joint_1 - - left_arm_joint_2 - - left_arm_joint_3 - - left_arm_joint_4 - - left_arm_joint_5 - - left_arm_joint_6 - - right_arm_joint_1 - - right_arm_joint_2 - - right_arm_joint_3 - - right_arm_joint_4 - - right_arm_joint_5 - - right_arm_joint_6 - command_interfaces: - - position - state_interfaces: - - position - - velocity - 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 - stopped_velocity_tolerance: 0.02 -gpio_command_controller: - ros__parameters: - gpios: - - left_arm_joint_1 - - left_arm_joint_2 - - left_arm_joint_3 - - left_arm_joint_4 - - left_arm_joint_5 - - left_arm_joint_6 - - right_arm_joint_1 - - right_arm_joint_2 - - right_arm_joint_3 - - right_arm_joint_4 - - right_arm_joint_5 - - right_arm_joint_6 - command_interfaces: - left_arm_joint_1: - interfaces: - - fault - - enable - left_arm_joint_2: - interfaces: - - fault - - enable - left_arm_joint_3: - interfaces: - - fault - - enable - left_arm_joint_4: - interfaces: - - fault - - enable - left_arm_joint_5: - interfaces: - - fault - - enable - left_arm_joint_6: - interfaces: - - fault - - enable - right_arm_joint_1: - interfaces: - - fault - - enable - right_arm_joint_2: - interfaces: - - fault - - enable - right_arm_joint_3: - interfaces: - - fault - - enable - right_arm_joint_4: - interfaces: - - fault - - enable - right_arm_joint_5: - interfaces: - - fault - - enable - right_arm_joint_6: - interfaces: - - fault - - enable - state_interfaces: - left_arm_joint_1: - interfaces: - - fault - - enable - left_arm_joint_2: - interfaces: - - fault - - enable - left_arm_joint_3: - interfaces: - - fault - - enable - left_arm_joint_4: - interfaces: - - fault - - enable - left_arm_joint_5: - interfaces: - - fault - - enable - left_arm_joint_6: - interfaces: - - fault - - enable - right_arm_joint_1: - interfaces: - - fault - - enable - right_arm_joint_2: - interfaces: - - fault - - enable - right_arm_joint_3: - interfaces: - - fault - - enable - right_arm_joint_4: - interfaces: - - fault - - enable - right_arm_joint_5: - interfaces: - - fault - - enable - right_arm_joint_6: - interfaces: - - fault - - enable diff --git a/ecrt_dev/config/zeroErr_config.yaml b/ecrt_dev/config/zeroErr_config.yaml deleted file mode 100644 index c8b0f36..0000000 --- a/ecrt_dev/config/zeroErr_config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -# Configuration file for Maxon EPOS3 drive -vendor_id: 0x5a65726f -product_id: 0x00029252 -assign-activate: 0x0300 #DC synch register -auto_fault_reset: true -auto_enable_set: true -auto_state_transitions: true -sdo: # sdo data to be transferred at drive startup - - {index: 0x6060, sub_index: 0, type: uint16, value: 8} # Set 0x1C33:01h = 0x02 DC设置 - #- {index: 0x6040, sub_index: 0, type: uint16, value: 0x80} # 清除错误 - #- {index: 0x6040, sub_index: 0, type: uint16, value: 0} - -rpdo: #PxPDO = receive PDO Mapping - - index: 0x1600 - channels: - - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position - # - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0} #target velocity - # - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0} #target torque - # - {index: 0x6072, sub_index: 0, type: uint16, default: 500} # Max torque - - {index: 0x6040, sub_index: 0, type: uint16, default: 0} # Control word - - {index: 0x6060, sub_index: 0, type: uint8, default: 8} # Control operation - - {index: 0xf0ff, sub_index: 0, type: uint8} # Dummy byte - -tpdo: #TxPDO = transmit PDO Mapping - - index: 0x1a00 - channels: - - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value - # - {index: 0x60F4, sub_index: 0, type: int32} #Position Following error actual value - - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 } # Velocity actual value - # - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 } # Torque actual value - - {index: 0x6041, sub_index: 0, type: uint16} # State word - # - {index: 0x603f, sub_index: 0, type: uint16} # Error code - - {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation} # Mode of operation display - - {index: 0xf0ff, sub_index: 0, type: uint8} # dummy byte \ No newline at end of file diff --git a/ecrt_dev/description/motor_drive.config.xacro b/ecrt_dev/description/motor_drive.config.xacro deleted file mode 100644 index f4e475e..0000000 --- a/ecrt_dev/description/motor_drive.config.xacro +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/ecrt_dev/description/motor_drive.ros2_control.xacro b/ecrt_dev/description/motor_drive.ros2_control.xacro deleted file mode 100644 index 2718313..0000000 --- a/ecrt_dev/description/motor_drive.ros2_control.xacro +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ${ec_plugin} - ${alias} - ${ec_position} - ${mode_of_operation} - ${slave_config_path} - - - - - - - - - ethercat_driver/EthercatDriver - ${master_id} - ${control_frequency} - - - - - - - - - - - - - - - - - - - diff --git a/ecrt_dev/ethercat_driver_plugin.xml b/ecrt_dev/ethercat_driver_plugin.xml deleted file mode 100644 index 48a105b..0000000 --- a/ecrt_dev/ethercat_driver_plugin.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - EtherCAT Driver for ros2_control. - - - diff --git a/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp b/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp deleted file mode 100644 index 3c95dd2..0000000 --- a/ecrt_dev/include/ethercat_driver/ethercat_driver.hpp +++ /dev/null @@ -1,169 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_ -#define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "ethercat_driver/visibility_control.h" -#include -#include -#include -#include -//#include "ethercat_interface/ec_slave.hpp" -//#include "ethercat_interface/ec_master.hpp" -#include "zer_config.hpp" -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -#define CYCLIC_POSITION 8 //CSP 周期同步位置模式 -#define CYCLIC_VELOCITY 9 //CSP 周期同步速度模式 -#define CYCLIC_TORQUE 10 //CSP 周期同步扭矩模式 -#define PVT_MODE 5 //PVT模式 -namespace ethercat_driver -{ - -class EthercatDriver : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(EthercatDriver) - - ETHERCAT_DRIVER_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - - ETHERCAT_DRIVER_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; - - ETHERCAT_DRIVER_PUBLIC - std::vector export_state_interfaces() override; - - ETHERCAT_DRIVER_PUBLIC - std::vector export_command_interfaces() override; - - ETHERCAT_DRIVER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; - - ETHERCAT_DRIVER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - ETHERCAT_DRIVER_PUBLIC - hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override; - - ETHERCAT_DRIVER_PUBLIC - hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; - void readData(); - void writeData(); - void check_master_state(); - void check_domain1_state(); - bool check_slave_config_states(); - void set_motor_enable(int id,bool enable){ - if(id>0&&id0&&id, NUM_SLAVES> motor_enable_arr; - std::vector> getEcModuleParam( - std::string & urdf, std::string component_name, std::string component_type); - - ///std::vector> ec_modules_; - std::vector> ec_module_parameters_; - - std::vector> hw_joint_commands_; - std::vector hw_cmd_position_; - std::vector> hw_sensor_commands_; - std::vector> hw_gpio_commands_; - std::vector> hw_joint_states_; - std::vector> hw_sensor_states_; - std::vector> hw_gpio_states_; - - - //pluginlib::ClassLoader ec_loader_{"ethercat_interface", "ethercat_interface::EcSlave"}; - struct timespec wakeupTime,time; - int control_frequency_; - ///ethercat_interface::EcMaster master_; - ec_master_t *master = NULL; - ec_domain_t *domain1 = NULL; - ec_slave_config_t *sc[NUM_SLAVES] = {0}; - uint8_t *domain1_pd = NULL; - 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_; - std::atomic activated_{false}; - #define FREQUENCY 250 - #define CSP_MAX_VEL_COUNTS_PER_S 65536 - #define CSP_POS_DEADBAND 10 - #define CSP_DEADBAND1 100//100 //CSP允许的误差(计数),原来是10 - #define CSP_DEADBAND2 1000 //300 - #define CSP_SPEED1 20//30 - #define CSP_SPEED2 100 - #define CLOCK_TO_USE CLOCK_MONOTONIC - #define NSEC_PER_SEC (1000000000L) - #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms - #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec) - #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) - const struct timespec cycletime = {0, PERIOD_NS}; - uint16_t clear_cmd[NUM_SLAVES]; - uint16_t command[NUM_SLAVES]; //状态字掩码 - 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}; - double dst_locs[NUM_SLAVES]={0}; - int32_t step_pos[NUM_SLAVES]={0}; - int32_t cur_pos[NUM_SLAVES]={0}; - double enable_arr[NUM_SLAVES]={0}; - int8_t mode_cmd=8; - int inited = 0; //初始化 - unsigned int counter = 0; - unsigned int sync_ref_counter = 0; - bool has_clear_all=false; - enum ErrState{ - ERR_NONE, - ERR_ACK, - ERR_ACK_WAIT, - ERR_CLEAR, - ERR_CLEAR_WAIT, - ERR_FIN, - ERR_FIN_WAIT - }; - ErrState errStaArr[NUM_SLAVES]={ERR_NONE}; -}; -} // namespace ethercat_driver - -#endif // ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_ diff --git a/ecrt_dev/include/ethercat_driver/visibility_control.h b/ecrt_dev/include/ethercat_driver/visibility_control.h deleted file mode 100644 index 845be48..0000000 --- a/ecrt_dev/include/ethercat_driver/visibility_control.h +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_ -#define ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define ETHERCAT_DRIVER_EXPORT __attribute__ ((dllexport)) - #define ETHERCAT_DRIVER_IMPORT __attribute__ ((dllimport)) - #else - #define ETHERCAT_DRIVER_EXPORT __declspec(dllexport) - #define ETHERCAT_DRIVER_IMPORT __declspec(dllimport) - #endif - #ifdef ETHERCAT_DRIVER_BUILDING_LIBRARY - #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_EXPORT - #else - #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_IMPORT - #endif - #define ETHERCAT_DRIVER_PUBLIC_TYPE ETHERCAT_DRIVER_PUBLIC - #define ETHERCAT_DRIVER_LOCAL -#else - #define ETHERCAT_DRIVER_EXPORT __attribute__ ((visibility("default"))) - #define ETHERCAT_DRIVER_IMPORT - #if __GNUC__ >= 4 - #define ETHERCAT_DRIVER_PUBLIC __attribute__ ((visibility("default"))) - #define ETHERCAT_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define ETHERCAT_DRIVER_PUBLIC - #define ETHERCAT_DRIVER_LOCAL - #endif - #define ETHERCAT_DRIVER_PUBLIC_TYPE -#endif - -#endif // ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/ecrt_dev/include/ethercat_driver/zer_config.hpp b/ecrt_dev/include/ethercat_driver/zer_config.hpp deleted file mode 100644 index db357d9..0000000 --- a/ecrt_dev/include/ethercat_driver/zer_config.hpp +++ /dev/null @@ -1,115 +0,0 @@ -#include "ecrt.h" -#include -#define NUM_SLAVES 12 -#define ZER_VID_PID 0x5a65726f,0x00029252 -#define YY_VID_PID 0x00001097,0x00002406 -typedef struct { - unsigned int ctrl_word; // 0x6040:00 - unsigned int target_position; // 0x607A:00 - unsigned int target_velocity; // 0x60FF:00 - unsigned int target_torque; // 0x6071:00 - unsigned int max_torque; // 0x6072:00 - unsigned int operation_mode; // 0x6060:00 - unsigned int reserved1; // 0xf0ff:00 - unsigned int status_word; // 0x6041:00 - unsigned int position_actual_value; // 0x6064:00 - unsigned int velocity_actual_value; // 0x606C:00 - unsigned int torque_actual_value; // 0x6077:00 - unsigned int error_code; // 0x603F:00 - unsigned int modes_of_operation_display; // 0x6061:00 - unsigned int reserved2; // 0xf0ff:00 -} 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=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}, \ - {alias,position, ZER_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\ - {alias,position, ZER_VID_PID, 0x60FF, 0, &zer_offsets[index].target_velocity,NULL},\ - {alias,position, ZER_VID_PID, 0x6071, 0, &zer_offsets[index].target_torque,NULL},\ - {alias,position, ZER_VID_PID, 0x6072, 0, &zer_offsets[index].max_torque,NULL},\ - {alias,position, ZER_VID_PID, 0x6060, 0, &zer_offsets[index].operation_mode,NULL},\ - {alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved1,NULL},\ - {alias,position, ZER_VID_PID, 0x6041, 0, &zer_offsets[index].status_word,NULL},\ - {alias,position, ZER_VID_PID, 0x6064, 0, &zer_offsets[index].position_actual_value,NULL},\ - {alias,position, ZER_VID_PID, 0x606C, 0, &zer_offsets[index].velocity_actual_value,NULL},\ - {alias,position, ZER_VID_PID, 0x6077, 0, &zer_offsets[index].torque_actual_value,NULL},\ - {alias,position, ZER_VID_PID, 0x603F, 0, &zer_offsets[index].error_code,NULL},\ - {alias,position, ZER_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\ - {alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL}, -// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 ------------------- -#if 1 -const static ec_pdo_entry_reg_t zer_domain1_regs[] = { - PDO_ENTRY(0,1,0) - PDO_ENTRY(0,2,1) - PDO_ENTRY(0,3,2) - PDO_ENTRY(0,4,3) - PDO_ENTRY(0,5,4) - PDO_ENTRY(0,6,5) - - PDO_ENTRY(0,7,6) - PDO_ENTRY(0,8,7) - PDO_ENTRY(0,9,8) - PDO_ENTRY(0,10,9) - PDO_ENTRY(0,11,10) - PDO_ENTRY(0,12,11) - {} // 结束标记 -}; -#else -const static ec_pdo_entry_reg_t zer_domain1_regs[] = { - YY_PDO_ENTRY(0,1,0) - PDO_ENTRY(0,2,1) - PDO_ENTRY(0,3,2) - PDO_ENTRY(0,4,3) - PDO_ENTRY(0,5,4) - PDO_ENTRY(0,6,5) - PDO_ENTRY(0,7,6) - - PDO_ENTRY(0,8,7) - PDO_ENTRY(0,9,8) - PDO_ENTRY(0,10,9) - PDO_ENTRY(0,11,10) - PDO_ENTRY(0,12,11) - PDO_ENTRY(0,13,12) - {} // 结束标记 -}; -#endif - -static ec_pdo_entry_info_t zer_device_pdo_entries[] = { - /*RxPdo 0x1600*/ - {0x6040, 0x00, 16}, /* control word */ - {0x607a, 0x00, 32}, /* target position */ - {0x60ff, 0x00, 32}, /* target velocity */ - {0x6071, 0x00, 16}, /* Target Torque */ - {0x6072, 0x00, 16}, /* Max Torque */ - {0x6060, 0x00, 8}, /* modes of operation */ - {0xf0ff, 0x00, 8}, - /*TxPdo 0x1A00*/ - {0x6041, 0x00, 16}, /* status word */ - {0x6064, 0x00, 32}, /* position actual value */ - {0x606c, 0x00, 32}, /* velocity actual value */ - {0x6077, 0x00, 16}, /* torque actual value */ - {0x603f, 0x00, 16}, /* error code */ - {0x6061, 0x00, 8}, /* modes of operation display */ - {0xf0ff, 0x00, 8}, -}; - -static ec_pdo_info_t zer_device_pdos[] = { - //RxPdo - {0x1600, 7, zer_device_pdo_entries + 0 }, - //TxPdo - {0x1A00, 7, zer_device_pdo_entries + 7 } -}; - -static ec_sync_info_t zer_device_syncs[] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE}, - {3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE}, - {0xff} -}; \ No newline at end of file diff --git a/ecrt_dev/launch/motor_drive.launch.py b/ecrt_dev/launch/motor_drive.launch.py deleted file mode 100644 index 4603024..0000000 --- a/ecrt_dev/launch/motor_drive.launch.py +++ /dev/null @@ -1,95 +0,0 @@ - - -from launch import LaunchDescription -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution -from launch.actions import DeclareLaunchArgument,TimerAction - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - 'description_file', - default_value='motor_drive.config.xacro', - description='URDF/XACRO description file with the axis.', - ) - ) - - description_file = LaunchConfiguration('description_file') - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ecrt_driver"), - "description", - description_file, - ] - ), - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ecrt_driver"), - "config", - "controllers.yaml", - ] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, robot_controllers, - { - "lock_memory": True, - "thread_priority": 99, - "cpu_affinity": 6 - } - ], - output="both", - ) - - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - parameters=[robot_description], - output='screen' - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "-c", "/controller_manager"], - ) - - gpio_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["gpio_command_controller", "-c", "/controller_manager"], - ) - - trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - 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]) - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_node, - ]#position_controller_spawner, - - return LaunchDescription( declared_arguments + nodes) diff --git a/ecrt_dev/package.xml b/ecrt_dev/package.xml deleted file mode 100644 index dbe411e..0000000 --- a/ecrt_dev/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - ecrt_driver - 1.2.0 - EtherCAT Driver for `ros2_control` - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - hardware_interface - pluginlib - rclcpp -rclcpp_action - rclcpp_lifecycle - ethercat_interface - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ecrt_dev/src/ethercat_driver.cpp b/ecrt_dev/src/ethercat_driver.cpp deleted file mode 100644 index f3a7dd2..0000000 --- a/ecrt_dev/src/ethercat_driver.cpp +++ /dev/null @@ -1,790 +0,0 @@ -#include // 用于 mlockall -#include // 用于线程优先级设置 -#include // 用于调度策略 -#include // 用于 strerror -#include // 用于 getpid -#include -#include "ethercat_driver/ethercat_driver.hpp" -#include -#include -#include -#include"ecrt.h" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" -#include -namespace ethercat_driver -{ -CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo & info) -{ - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; - } - - cpu_set_t cpuset; - CPU_ZERO(&cpuset); - int cpu_core=7; - 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 = 90; - - if (pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m) != 0) { - RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), - "Failed to set real-time priority: %s", strerror(errno)); - return CallbackReturn::ERROR; - } - - if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) { - RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), - "Failed to lock memory: %s", strerror(errno)); - } - const std::lock_guard lock(ec_mutex_); - activated_.store(false,std::memory_order_relaxed); - for(int i=0;i::quiet_NaN()); - } - hw_sensor_states_.resize(info_.sensors.size()); - for (uint s = 0; s < info_.sensors.size(); s++) { - hw_sensor_states_[s].resize( - info_.sensors[s].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_gpio_states_.resize(info_.gpios.size()); - for (uint g = 0; g < info_.gpios.size(); g++) { - hw_gpio_states_[g].resize( - info_.gpios[g].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_cmd_position_.resize(info_.joints.size()); -#if 1 - hw_joint_commands_.resize(info_.joints.size()); - for (uint j = 0; j < info_.joints.size(); j++) { - hw_joint_commands_[j].resize( - info_.joints[j].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } -#endif - hw_sensor_commands_.resize(info_.sensors.size()); - for (uint s = 0; s < info_.sensors.size(); s++) { - hw_sensor_commands_[s].resize( - info_.sensors[s].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } - hw_gpio_commands_.resize(info_.gpios.size()); - for (uint g = 0; g < info_.gpios.size(); g++) { - hw_gpio_commands_[g].resize( - info_.gpios[g].command_interfaces.size(), - std::numeric_limits::quiet_NaN()); - } -RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info_.joints.size(),info_.joints[0].state_interfaces.size(),info_.joints[1].state_interfaces.size()); - for (uint j = 0; j < info_.joints.size(); j++) { - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints"); - // check all joints for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.joints[j].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.joints[j].command_interfaces[k].name] = std::to_string(k); - } - } - } - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios.size:%ld",info_.gpios.size()); - for (uint g = 0; g < info_.gpios.size(); g++) { - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios"); - // check all gpios for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.gpios[g].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.gpios[g].command_interfaces[k].name] = std::to_string(k); - } - } - } - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors.size:%ld",info_.joints.size()); - for (uint s = 0; s < info_.sensors.size(); s++) { - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors"); - // check all sensors for EC modules and load into ec_modules_ - auto module_params = getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor"); - ec_module_parameters_.insert( - ec_module_parameters_.end(), module_params.begin(), module_params.end()); - for (auto i = 0ul; i < module_params.size(); i++) { - for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) { - module_params[i]["state_interface/" + - info_.sensors[s].state_interfaces[k].name] = std::to_string(k); - } - for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); k++) { - module_params[i]["command_interface/" + - info_.sensors[s].command_interfaces[k].name] = std::to_string(k); - } - - } - } - // - master = ecrt_request_master(0); - if (!master) { - perror("ecrt_request_master"); - //g_started.store(false); return false; - } - domain1 = ecrt_master_create_domain(master); - if (!domain1){ - perror("ecrt_master_create_domain"); - //g_started.store(false); return false; - } - const uint32_t shift_step = PERIOD_NS / 12; - for (int i = 0; i < NUM_SLAVES; i++) { - std::cout << "Configuring slave " << i+1 << "..." << std::endl; - { - sc[i] = ecrt_master_slave_config(master, 0, i+1, ZER_VID_PID); - if (!sc[i]) - { - std::cout << "Failed slave cfg at pos " << i << std::endl; - } - if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs)) - { - std::cout << "Failed PDO config " << i << std::endl; - } - } - ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 20000, 0, 0); - //ecrt_slave_config_dc(sc[i], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0); - } - ///ecrt_master_set_send_interval(master, PERIOD_NS); - ecrt_domain_reg_pdo_entry_list(domain1,zer_domain1_regs); - if (ecrt_master_activate(master)) { - perror("ecrt_master_activate"); - return CallbackReturn::ERROR; - } - domain1_pd = ecrt_domain_data(domain1); - if (!domain1_pd) - { - fprintf(stderr,"domain1_pd null\n"); - return CallbackReturn::ERROR; - } - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", info_.joints.size()); - return CallbackReturn::SUCCESS; -} - -CallbackReturn EthercatDriver::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(rclcpp::get_logger("hehe"),"on configure......"); - - return CallbackReturn::SUCCESS; -} - -std::vector -EthercatDriver::export_state_interfaces() -{ - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_state_interfaces:%ld,%ld,%ld",info_.joints.size(),info_.sensors.size(),info_.gpios.size()); - std::vector state_interfaces; - // export joint state interface - for (uint j = 0; j < info_.joints.size(); j++) { - for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) { - ////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str()); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[j].name, - info_.joints[j].state_interfaces[i].name, - &hw_joint_states_[j][i])); - } - } - // export sensor state interface - for (uint s = 0; s < info_.sensors.size(); s++) { - for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.sensors[s].name, - info_.sensors[s].state_interfaces[i].name, - &hw_sensor_states_[s][i])); - } - } - // export gpio state interface - for (uint g = 0; g < info_.gpios.size(); g++) { - for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.gpios[g].name, - info_.gpios[g].state_interfaces[i].name, - &hw_gpio_states_[g][i])); - } - } - return state_interfaces; -} - -std::vector -EthercatDriver::export_command_interfaces() -{ -RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%ld",info_.joints.size()); - std::vector command_interfaces; - // export joint command interface - ///std::vector test; - #if 0 - for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[i].name, - hardware_interface::HW_IF_POSITION, - &hw_cmd_position_[i])); - } - #endif - for (uint j = 0; j < info_.joints.size(); j++) { - ////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"size:%d\n",info_.joints[j].command_interfaces.size()); - for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) { - ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str()); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[j].name, - info_.joints[j].command_interfaces[i].name, - &hw_joint_commands_[j][i])); - } - } - // export sensor command interface - for (uint s = 0; s < info_.sensors.size(); s++) { - for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.sensors[s].name, - info_.sensors[s].command_interfaces[i].name, - &hw_sensor_commands_[s][i])); - } - } - // export gpio command interface - for (uint g = 0; g < info_.gpios.size(); g++) { - for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) { - ///printf("gpios:%s,%s\n",info_.gpios[g].name,info_.gpios[g].command_interfaces[i].name); - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.gpios[g].name, - info_.gpios[g].command_interfaces[i].name, - &hw_gpio_commands_[g][i])); - } - } - return command_interfaces; -} - -CallbackReturn EthercatDriver::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - const std::lock_guard lock(ec_mutex_); - if (activated_.load(std::memory_order_relaxed)) { - RCLCPP_FATAL(rclcpp::get_logger("EthercatDriver"), "Double on_activate()"); - return CallbackReturn::ERROR; - } - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "on activate ...please wait..."); - if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) { - control_frequency_ = 100; - } else { - control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]); - } - control_frequency_=FREQUENCY; - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"control_frequency_:%d\n",control_frequency_); - if (control_frequency_ < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!"); - return CallbackReturn::ERROR; - } - clock_gettime(CLOCK_TO_USE, &wakeupTime); - for(int i=0;i lock(ec_mutex_); - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "SSSSSSSSSSSSSSSSSSSSSSSS..."); - for(int i=0;i 立即“冻结” - int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value); - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold); - // 可选：扭矩模式也清零（最好维持急停状态，待修改） - 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_.store(false,std::memory_order_relaxed); - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "System successfully stopped!"); - return CallbackReturn::SUCCESS; -} - - - -hardware_interface::return_type EthercatDriver::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - // try to lock so we can avoid blocking the read/write loop on the lock. - ////const std::unique_lock lock(ec_mutex_, std::try_to_lock); - //printf("read data...\n"); - //if (lock.owns_lock() && activated_) { - if (activated_.load(std::memory_order_relaxed)){ - auto start=std::chrono::high_resolution_clock::now(); - readData(); - auto end=std::chrono::high_resolution_clock::now(); - auto diff=end-start; - if(diff.count()>100000) - RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"rdiff:%ld",diff.count()); - } - return hardware_interface::return_type::OK; -} -void EthercatDriver::check_domain1_state(void) -{ - ec_domain_state_t ds; - - ecrt_domain_state(domain1, &ds); - - if (ds.working_counter != domain1_state.working_counter) - //printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包 - if (ds.wc_state != domain1_state.wc_state) - //printf("Domain1: State %u.\n", ds.wc_state); - - domain1_state = ds; -} -void EthercatDriver::check_master_state(void) -{ - ec_master_state_t ms; - - ecrt_master_state(master, &ms); - - if (ms.slaves_responding != master_state.slaves_responding) - //printf("%u slave(s).\n", ms.slaves_responding); - if (ms.al_states != master_state.al_states) - //printf("AL states: 0x%02X.\n", ms.al_states); - if (ms.link_up != master_state.link_up) - //printf("Link is %s.\n", ms.link_up ? "up" : "down"); - - master_state = ms; -} -bool EthercatDriver::check_slave_config_states(void) -{ - ec_slave_config_state_t s; - bool all_op=true; - for (int i = 0; i < NUM_SLAVES; ++i) { - errStaArr[i]=ERR_NONE; - if (!sc[i]) continue; - ecrt_slave_config_state(sc[i], &s); - if ((s.al_state != sc_state[i].al_state)||(s.online != sc_state[i].online)||(s.operational != sc_state[i].operational)){ - 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){ - op_states[i]=0; - all_op=false; - }else{ - op_states[i]=1; - } - } - return all_op; -} -struct timespec EthercatDriver::timespec_add(struct timespec time1, struct timespec time2) -{ - struct timespec result; - - if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { - result.tv_sec = time1.tv_sec + time2.tv_sec + 1; - result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; - } else { - result.tv_sec = time1.tv_sec + time2.tv_sec; - result.tv_nsec = time1.tv_nsec + time2.tv_nsec; - } - - return result; -} -void EthercatDriver::readData(){ - - static int print_cnt=0; - print_cnt+=1; - #if 0 - if(print_cnt%1000==0){ - for(int i=0;i0){ - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],first code 0x%04x",i,err); - clear_cmd[i]=1;// - has_clear_all=false; - }else{ - clear_cmd[i]=0; - } - } - } - }else{ - all_motor_op=check_slave_config_states(); //检查从站状态 - return; - } - } - if (!inited) { - for (int i = 0; i < NUM_SLAVES; ++i) { - command[i] = 0x004F; - status[i] = 0x000F; - last_status[i] = status[i]; - } - 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 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); - cur_pos[i]=pos; - double up_pos=cur_pos[i]-pos_offsets[i]; - //double angle=std::round(up_pos*count_to_rad * 1000.0) / 1000.0; - if(i==2||i==8) - up_pos=-up_pos; - if(op_states[i]==1){ - hw_joint_states_[i][0]=up_pos*count_to_rad;//angle; - hw_joint_states_[i][1]=vel*count_to_speed;//std::round(vel*count_to_speed*1000.0)/1000.0; - double amp=tv*rated_currents[i]; - double effort=0.014*0.6*amp; - hw_joint_states_[i][2]=effort;//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][4]=enable_arr[i]; - //hw_joint_states_[i][2]=err; - - status[i] = sw; //侦测子站状态字变化 - if (status[i] != last_status[i]) { - last_status[i] = status[i]; - } - } -} -void EthercatDriver::writeData(){ - static int print_cnt=0; - print_cnt+=1; - for (int i = 0; i < NUM_SLAVES; ++i) - { -#if 1 - if ((status[i]&0x006f)==0x0008){ - uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code); - if(err>0){ - double fault=hw_joint_commands_[i][0]; - if(print_cnt%1000==1){ - ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err); - } - if(fault==1.0||clear_cmd[i]==1){ - if(print_cnt%1000==0) - RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%.1f/%d,fault code 0x%04x",i,fault,clear_cmd[i],err); - if(errStaArr[i]==ERR_NONE){ - errStaArr[i]=ERR_ACK; - } - } - } - } - //clear fault - if(errStaArr[i]==ERR_ACK){ - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0000); - errStaArr[i]=ERR_ACK_WAIT; - ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK %d",i,err,(sw&0x006f)==0x0008); - }else if(errStaArr[i]==ERR_ACK_WAIT){ - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK_WAIT %d",i,err,(sw&0x006f)==0x0008); - if ((status[i]&0x006f)==0x0008){ - - } - errStaArr[i]=ERR_CLEAR; - }else if(errStaArr[i]==ERR_CLEAR){ - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080); - errStaArr[i]=ERR_CLEAR_WAIT; - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%d,ERR_CLEAR",i,clear_cmd[i]); - }else if(errStaArr[i]==ERR_CLEAR_WAIT){ - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_CLEAR_WAIT %d",i,err,(sw&0x006f)==0x0008); - errStaArr[i]=ERR_FIN; - }else if(errStaArr[i]==ERR_FIN){ - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN %d",i,err,(sw&0x006f)==0x0008); - errStaArr[i]=ERR_FIN_WAIT; - }else if(errStaArr[i]==ERR_FIN_WAIT){ - //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN_WAIT %d",i,err,(sw&0x006f)==0x0008); - if((status[i]&0x006f)==0x0008){ - errStaArr[i]=ERR_ACK; - }else{ - errStaArr[i]=ERR_NONE; - ///continue; - } - } - #endif - if(errStaArr[i]!=ERR_NONE||op_states[i]!=1) - continue; - enable_arr[i]=hw_joint_commands_[i][1]; - //if(print_cnt%500==0) - // RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"enable:%d,%.1f,%.2f",i,enable,hw_joint_commands_[i][2]); - if(enable_arr[i]!=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; - continue; - } - 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; - step_pos[i]=cur_pos[i]; - } 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); - step_pos[i]=cur_pos[i]; - } else if ((status[i] & command[i]) == 0x0023) { - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled - command[i] = 0x006F; - step_pos[i]=cur_pos[i]; - } else if ((status[i] & command[i]) == 0x0027) { - 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); - break; - case CYCLIC_POSITION: { // 8 - // ---- 绝对式目标生成，不再用 pv 做增量基准 ---- - // 每轴记住“上次下发的命令位置” - static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值 - static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化 - // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题 - static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器 - - // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击 - if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) { - csp_cmd_pos[i] = cur_pos[i]; // 先对齐 - vmax_acc[i] = 0; - csp_inited[i] = 1; - } - double down_pos=hw_joint_commands_[i][2]*rad_to_count; - if(i==2||i==8){ - down_pos=-down_pos; - } - double target_pos=down_pos+pos_offsets[i]; - if(print_cnt%1000==1){ - //if(i==8) - // RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.3f,target_pos:%.3f",hw_joint_commands_[i][2],target_pos); - } - dst_locs[i]=target_pos; - #if 0 - int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY; - 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 { - // 限速迈步：命令轨迹只按时间往 goal 逼近 - 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; - } - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]); - #else - const int32_t err_cmd=target_pos-cur_pos[i]; - if (abs(err_cmd)0) step_pos[i]+=(err_cmd>CSP_SPEED1) ? CSP_SPEED1:err_cmd; - else step_pos[i]+=(err_cmd<-CSP_SPEED1) ? -CSP_SPEED1:err_cmd; - // reach_target[i]=0; - //if(err_cmd>0) step_pos[i]+=CSP_SPEED1; - //else step_pos[i]-=CSP_SPEED1; - } else { - //if(err_cmd>0) step_pos[i]+=(err_cmd>CSP_SPEED2) ? CSP_SPEED2:err_cmd; - //else step_pos[i]+=(err_cmd<-CSP_SPEED2) ? -CSP_SPEED2:err_cmd; - if(err_cmd>0) step_pos[i]+=CSP_SPEED2; - else step_pos[i]-=CSP_SPEED2; - } - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]); - #endif - // 可选：跟随误差防护（避免 following error 过大时继续“加命令”） - // const int32_t follow_err = csp_cmd_pos[i] - pv; - // const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数 - // if (std::abs(follow_err) > FE_LIMIT) { - // // 例如：冻结或减半 max_step，给驱动时间追上 - // } - - break; - } - case CYCLIC_TORQUE: // 10 - ///////EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd); - break; - default: - // 未知模式：默认速度模式安全置 0 - //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0); - break; - - } - } else { - // run_enable=false：清零目标 - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); - // CSV 安全：速度清零 - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0); - // CSP 安全：目标位置=当前位置 -> 立即“冻结” - - int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value); - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold); - // 可选：扭矩模式也清零（最好维持急停状态，待修改） - EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0); - EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态 - } - } - } - //Final - if (sync_ref_counter) { - sync_ref_counter--; - } else { - sync_ref_counter = 1; // sync every 2 cycle - clock_gettime(CLOCK_TO_USE, &time); - ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟 - ecrt_master_sync_slave_clocks(master); - } - - ecrt_domain_queue(domain1); //排队域数据 - ecrt_master_send(master); //发送ETHERCAT帧 -} -hardware_interface::return_type EthercatDriver::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) -{ - // try to lock so we can avoid blocking the read/write loop on the lock. - /// const std::unique_lock lock(ec_mutex_, std::try_to_lock); - if (activated_.load(std::memory_order_relaxed)){ - auto start=std::chrono::high_resolution_clock::now(); - writeData(); - auto end=std::chrono::high_resolution_clock::now(); - auto diff=end-start; - if(diff.count()>100000) - RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"wdiff:%ld",diff.count()); - } - return hardware_interface::return_type::OK; -} - -std::vector> EthercatDriver::getEcModuleParam( - std::string & urdf, - std::string component_name, - std::string component_type) -{ - // Check if everything OK with URDF string - if (urdf.empty()) { - throw std::runtime_error("empty URDF passed to robot"); - } - tinyxml2::XMLDocument doc; - if (!doc.Parse(urdf.c_str()) && doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); - } - if (doc.Error()) { - throw std::runtime_error("invalid URDF passed in to robot parser"); - } - - tinyxml2::XMLElement * robot_it = doc.RootElement(); - if (std::string("robot").compare(robot_it->Name())) { - throw std::runtime_error("the robot tag is not root element in URDF"); - } - - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement("ros2_control"); - if (!ros2_control_it) { - throw std::runtime_error("no ros2_control tag"); - } - - std::vector> module_params; - std::unordered_map module_param; - - while (ros2_control_it) { - const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(component_type.c_str()); - while (ros2_control_child_it) { - if (!component_name.compare(ros2_control_child_it->Attribute("name"))) { - const auto * ec_module_it = ros2_control_child_it->FirstChildElement("ec_module"); - while (ec_module_it) { - module_param.clear(); - module_param["name"] = ec_module_it->Attribute("name"); - const auto * plugin_it = ec_module_it->FirstChildElement("plugin"); - if (NULL != plugin_it) { - module_param["plugin"] = plugin_it->GetText(); - } - const auto * param_it = ec_module_it->FirstChildElement("param"); - while (param_it) { - module_param[param_it->Attribute("name")] = param_it->GetText(); - param_it = param_it->NextSiblingElement("param"); - } - module_params.push_back(module_param); - ec_module_it = ec_module_it->NextSiblingElement("ec_module"); - } - } - ros2_control_child_it = ros2_control_child_it->NextSiblingElement(component_type.c_str()); - } - ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control"); - } - - return module_params; -} - -} // namespace ethercat_driver - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ethercat_driver::EthercatDriver, hardware_interface::SystemInterface) diff --git a/ecrt_dev/src/test_motor.cpp b/ecrt_dev/src/test_motor.cpp deleted file mode 100644 index 5209b4b..0000000 --- a/ecrt_dev/src/test_motor.cpp +++ /dev/null @@ -1,544 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include -#include // 添加这行来支持文件流操作 -#include -#include -#include -#include -#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" -#include - -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: - TestMotor(); - ~TestMotor(); - - int motor_dst(std::string name,double dst);//add by hehe - //void ctrl_motor(int id,double pos,int reset,int enable); - void motor_fault(int id,double fault); - void motor_enable(int id,double enable); - void motor_pos(int id,double pos); - void motor_loop(int motor_id,int cnt); - void motor_action(int id,double angle); - void all_motor(); - void ControlLoop(); - bool activateController(const std::string& controller_name); - void setJointValueTarget(double angle); - void setJointValueTarget(const std::vector angles); - void pubTraj(const std::vector angles1,const std::vector angles2); - void pubTraj(const double angle); - bool motor_drv_on=false; - bool all_motor_op=false; - bool is_reach=false; - std::unordered_map curMap_; - std::unordered_map dstMap_; - private: - rclcpp::Publisher::SharedPtr cmdPub_; - rclcpp::Publisher::SharedPtr gpioPub_; - rclcpp::Subscription::SharedPtr jointStatesSub_; - rclcpp_action::Client::SharedPtr client_; - rclcpp::Client::SharedPtr switch_client; - // 文件流相关 - std::ofstream data_file_; // 用于写入数据的文件流 - std::string data_file_path_; // 数据文件路径 - - rclcpp::TimerBase::SharedPtr controlTimer_; - rclcpp::Time lastTime_; // 移至类成员 - struct termios original_termios_; - - - //add by hehe - - //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); - }; - -// 机器人控制器构造函数 -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"); - switch_client = create_client("/controller_manager/switch_controller"); - // 创建发布者 - 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(5000),std::bind(&TestMotor::ControlLoop, this)); // 绑定到新的控制函数); - curMap_.clear(); - dstMap_.clear(); - //posMsg_.interface_groups.resize(12); - //posMsg_.interface_values.resize(12); - ///std::cout << "start enblex ..." << std::endl; - - std::cout << "TestMotor Node is created finished!" << std::endl; -} - -TestMotor::~TestMotor() -{ - std::cout << "Robot controller stopped." << std::endl; -} -///std::vector angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 40, 0}; -void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) -{ - if (!msg) { // 检查消息是否有效 - std::cout << "get null joint states!" << std::endl; - return; - } - if(msg->name.size()>0) - motor_drv_on=true; - //printf("motor_drv_on:%d\n",motor_drv_on); - all_motor_op=true; - for(int i=0;iname.size();i++) - { - curMap_[msg->name[i]] = msg->position[i]; - ///std::cout<<"cur:"<name[i]<<":"<name[i]]<name[i]]==9999.0) - all_motor_op=false; - } - if(all_motor_op){ - if(is_reach){ - ////pubTraj(angles); - } - } -} -bool TestMotor::activateController(const std::string& controller_name) { - ///std::cout<<"激活控制器"<(); - 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; -} -void TestMotor::pubTraj(const std::vector angles1,const std::vector angles2){ - trajectory_msgs::msg::JointTrajectory traj_msg; - ///traj_msg.header.stamp = this->now(); - traj_msg.joint_names = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6", - "right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"}; - trajectory_msgs::msg::JointTrajectoryPoint point; - ///point.positions = {1.0, 0.5, -0.8, 0.0, 0.7, 0.0}; // 目标角度（弧度） - ///point.velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 到达时速度为0（可选） - /// point.time_from_start = rclcpp::Duration::from_seconds(2.0); // 2秒内到达 - double delta=0.0002; - bool skip_pub=true; - for(int i=0;i<6;i++){ - std::string jname = "left_arm_joint_" + std::to_string(i+1); - dstMap_[jname]=angles1.at(i)*M_PI/180.0; - double rad=curMap_[jname]; - rad=dstMap_[jname]; - // double diff=dstMap_[jname]-curMap_[jname]; - // if(diff<-0.005){ - // rad= curMap_[jname]-delta; - // skip_pub=false; - // } - // else if(diff>0.005){ - // rad= curMap_[jname]+delta; - // skip_pub=false; - // } - point.positions.push_back(rad); - printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],rad); - } - for(int i=0;i<6;i++){ - std::string jname = "right_arm_joint_" + std::to_string(i+1); - dstMap_[jname]=angles2.at(i)*M_PI/180.0; - double rad=curMap_[jname]; - rad=dstMap_[jname]; - point.positions.push_back(rad); - printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],rad); - } - traj_msg.points.push_back(point); - //if(!skip_pub) - cmdPub_->publish(traj_msg); -} -void TestMotor::pubTraj(const double angle){ - trajectory_msgs::msg::JointTrajectory traj_msg; - ///traj_msg.header.stamp = this->now(); - traj_msg.joint_names = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6", - "right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"}; - trajectory_msgs::msg::JointTrajectoryPoint point; - ///point.positions = {1.0, 0.5, -0.8, 0.0, 0.7, 0.0}; // 目标角度（弧度） - ///point.velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; // 到达时速度为0（可选） - /// point.time_from_start = rclcpp::Duration::from_seconds(2.0); // 2秒内到达 - double delta=0.0002; - bool skip_pub=true; - for(int i=0;i<6;i++){ - std::string jname = "left_arm_joint_" + std::to_string(i+1); - //dstMap_[jname]=angles.at(i)*M_PI/180.0; - //double rad=curMap_[jname]; - - // double diff=dstMap_[jname]-curMap_[jname]; - // if(diff<-0.005){ - // rad= curMap_[jname]-delta; - // skip_pub=false; - // } - // else if(diff>0.005){ - // rad= curMap_[jname]+delta; - // skip_pub=false; - // } - point.positions.push_back(angle); - printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],angle); - } - for(int i=0;i<6;i++){ - std::string jname = "right_arm_joint_" + std::to_string(i+1); - point.positions.push_back(angle); - printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],angle); - } - traj_msg.points.push_back(point); - //if(!skip_pub) - cmdPub_->publish(traj_msg); -} -// 状态机主循环 -void TestMotor::ControlLoop() { - //Gpio_publish_joint_trajectory(); - ////all_motor(); -} -void TestMotor::motor_pos(int id,double delta) -{ - control_msgs::msg::DynamicInterfaceGroupValues posMsg_; - std::string target_motor="joint_" + std::to_string(id); - for(int i=0;i<12;i++){ - std::string jname = "joint_" + std::to_string(i+1); - posMsg_.interface_groups.push_back(jname); - control_msgs::msg::InterfaceValue tempValue; - //position - tempValue.interface_names = {"position"}; - double diff=dstMap_[jname]-curMap_[jname]; - if(diff<-0.03) - tempValue.values = {curMap_[jname]-delta}; - else if(diff>0.03) - tempValue.values = {curMap_[jname]+delta}; - - std::cout<"<publish(posMsg_); - usleep(10000); -} -void TestMotor::motor_fault(int id,double fault) -{ - control_msgs::msg::DynamicInterfaceGroupValues posMsg_; - std::string target_motor="joint_" + std::to_string(id); - std::cout<<"fault:"; - 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; - //reset - tempValue.interface_names = {"fault"}; - if(id==0){ - tempValue.values = {fault}; - }else{ - if(tempInterfaceGroup!=target_motor) - tempValue.values = {0}; - else - tempValue.values = {fault}; - } - - 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<6;i++){ - std::string tempInterfaceGroup = "left_arm_joint_" + std::to_string(i+1); - posMsg_.interface_groups.push_back(tempInterfaceGroup); - control_msgs::msg::InterfaceValue tempValue; - //enable - tempValue.interface_names = {"enable"}; - if(ipublish(posMsg_); - usleep(1000000); -} -void TestMotor::setJointValueTarget(std::vector angles){ - return; - 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); - // double rad=angles.at(i)*M_PI/180.0; - // point.positions.push_back(rad); - // printf("D %s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]); - // } - double delta=0.01; - for(int i=0;i<12;i++){ - std::string jname = "joint_" + std::to_string(i+1); - dstMap_[jname]=angles.at(i)*M_PI/180.0; - double rad=0.0; - double diff=dstMap_[jname]-curMap_[jname]; - if(diff<-0.03) - rad= curMap_[jname]-delta; - else if(diff>0.03) - rad= curMap_[jname]+delta; - point.positions.push_back(rad); - printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],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); - client_->async_send_goal(goal,send_goal_option); - }else{ - printf("wait action server error\n"); - } -} -void TestMotor::setJointValueTarget(double angle){ - return; - 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 %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_action(int id,double delta) -{ - return; - //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); - } - - motor_loop(2,loop_cnt); - motor_loop(3,loop_cnt); - motor_loop(4,loop_cnt); - motor_loop(5,loop_cnt); - motor_loop(6,loop_cnt); - motor_loop(7,loop_cnt); - motor_loop(8,loop_cnt); - motor_loop(9,loop_cnt); - 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); - auto node=std::make_shared(); - auto spin_thread=std::thread([=](){ - rclcpp::spin(node); - }); - std::cout<<"等待电机驱动启动...\n"; - while(rclcpp::ok()&&!node->motor_drv_on){ - sleep(1); - } - std::cout<<"电机驱动启动完毕!\n等待电机进入OP...\n"; - //std::cout<<""<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::vector angles1={0, 0, 0, 0, 0, 0}; - std::vector angles2={90,0, 40, 0, 0, 0}; - node->is_reach=true; - static int loop_cnt=0; - while(rclcpp::ok()){ - std::cout<<"开始执行角度1.05...\n"; - loop_cnt+=1; - if(loop_cnt%2==0){ - angles1={-40, -40, -40,-40,-40,-40}; - angles2={-40,-40,-40, -40,-40,-40}; - }else{ - angles1={40, 40, 40,40,40,40}; - angles2={40,40,40, 40,40,40}; - } - //angles2={0,0,0, 40,0,0}; - //angles1={-40,-40,-40, 0,-40, 0}; - //angles2={40,40,40, 0,40,0}; - // for(int i=0;i<12;i++){ - // std::string jname = "joint_" + std::to_string(i+1); - // node->dstMap_[jname]=angles.at(i)*M_PI/180.0; - // printf("dst: %s %.2f/%.2f\n",jname.c_str(),node->curMap_[jname],node->dstMap_[jname]); - // } - ///node->dstMap_[jname]={-0.678,-0.678,-0.678,0,-0.678,0,0.678,0.678,0.678,0,0.678,0}; - node->pubTraj(angles1,angles2); - sleep(10); - std::cout<<"开始执行复位...\n"; - // for(int i=0;i<12;i++){ - // std::string jname = "joint_" + std::to_string(i+1); - // node->dstMap_[jname]=angles.at(i)*M_PI/180.0; - // printf("dst: %s %.2f/%.2f\n",jname.c_str(),node->curMap_[jname],node->dstMap_[jname]); - // } - angles1={0, 0, 0, 0, 0, 0}; - angles2={0, 0, 0,0, 0, 0}; - node->pubTraj(angles1,angles2); - sleep(10); - } - - // std::cout<<"复位完毕!\n禁能机械臂电机...\n"; - // node->motor_enable(13,0); - // std::cout<<"禁能机械臂电机完毕!\n"; - } - ///usleep(10000000); - ///node->all_motor(); - rclcpp::shutdown(); - spin_thread.join(); - return 0; -} diff --git a/ecrt_dev_dual/src/ethercat_driver.cpp b/ecrt_dev_dual/src/ethercat_driver.cpp index 1ac3fa9..dcdf3b6 100644 --- a/ecrt_dev_dual/src/ethercat_driver.cpp +++ b/ecrt_dev_dual/src/ethercat_driver.cpp @@ -634,6 +634,7 @@ void EthercatDriver::writeData(){ if(print_cnt%1000==1){ ////RCLCPP_INFO(rclcpp::get_logger("hehe"),"[%d] rad:%.1f,target_pos:%.1f,cur pos:%d",i,dst_rads[i],target_pos,cur_pos[i]); } + EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, target_pos);// #if 0 int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY; const int32_t goal=target_pos; @@ -663,7 +664,7 @@ void EthercatDriver::writeData(){ //if(err_cmd>0) step_pos[i]+=CSP_SPEED2; //else step_pos[i]-=CSP_SPEED2; } - EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]); + ////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]); #endif // 可选：跟随误差防护（避免 following error 过大时继续“加命令”） // const int32_t follow_err = csp_cmd_pos[i] - pv;