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

del step

Browse Source
This commit is contained in:
Your Name
2026-03-05 17:33:35 +08:00
parent a0bc503e32
commit 8e394594c8
14 changed files with 2 additions and 2167 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
ecrt_dev/CMakeLists.txt
View File

@@ -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()

158
ecrt_dev/config/controllers.yaml
View File

@@ -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

34
ecrt_dev/config/zeroErr_config.yaml
View File

@@ -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

9
ecrt_dev/description/motor_drive.config.xacro
View File

@@ -1,9 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
<link name="world"/>
<xacro:include filename="motor_drive.ros2_control.xacro" />
<xacro:motor_drive/>
</robot>

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

@@ -1,64 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 公共参数定义（一次修改，全局生效） -->
<xacro:property name="master_id" value="0" />
<xacro:property name="control_frequency" value="10" />
<xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
<xacro:property name="alias" value="0" />
<xacro:property name="mode_of_operation" value="8" />
<xacro:property name="slave_config_path" value="config/zeroErr_config.yaml" />
<!-- 子宏：封装单个关节的重复配置 -->
<!-- 参数：joint_name（关节名称）、ec_position（EtherCAT位置索引） -->
<xacro:macro name="single_joint_config" params="joint_name ec_position">
<joint name="${joint_name}">
<!-- 状态接口（位置/速度/力矩） -->
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
<state_interface name="fault"/>
<state_interface name="enable"/>
<!-- 命令接口（位置控制/故障重置/使能） -->
<command_interface name="fault"/>
<command_interface name="enable"/>
<command_interface name="position"/>
<!-- EtherCAT模块配置（复用公共参数） -->
<ec_module name="zeroErr">
<plugin>${ec_plugin}</plugin>
<param name="alias">${alias}</param>
<param name="position">${ec_position}</param>
<param name="mode_of_operation">${mode_of_operation}</param>
<param name="slave_config">${slave_config_path}</param>
</ec_module>
</joint>
</xacro:macro>
<!-- 主宏：电机驱动系统配置 -->
<xacro:macro name="motor_drive">
<ros2_control name="motor_drive" type="system">
<hardware>
<plugin>ethercat_driver/EthercatDriver</plugin>
<param name="master_id">${master_id}</param>
<param name="control_frequency">${control_frequency}</param>
</hardware>
<!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
<xacro:single_joint_config joint_name="left_arm_joint_1" ec_position="1" />
<xacro:single_joint_config joint_name="left_arm_joint_2" ec_position="2" />
<xacro:single_joint_config joint_name="left_arm_joint_3" ec_position="3" />
<xacro:single_joint_config joint_name="left_arm_joint_4" ec_position="4" />
<xacro:single_joint_config joint_name="left_arm_joint_5" ec_position="5" />
<xacro:single_joint_config joint_name="left_arm_joint_6" ec_position="6" />
<xacro:single_joint_config joint_name="right_arm_joint_1" ec_position="7" />
<xacro:single_joint_config joint_name="right_arm_joint_2" ec_position="8" />
<xacro:single_joint_config joint_name="right_arm_joint_3" ec_position="9" />
<xacro:single_joint_config joint_name="right_arm_joint_4" ec_position="10" />
<xacro:single_joint_config joint_name="right_arm_joint_5" ec_position="11" />
<xacro:single_joint_config joint_name="right_arm_joint_6" ec_position="12" />
</ros2_control>
</xacro:macro>
</robot>

9
ecrt_dev/ethercat_driver_plugin.xml
View File

@@ -1,9 +0,0 @@
<library path="ecrt_driver">
<class name="ethercat_driver/EthercatDriver"
type="ethercat_driver::EthercatDriver"
base_class_type="hardware_interface::SystemInterface">
<description>
EtherCAT Driver for ros2_control.
</description>
</class>
</library>

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

@@ -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<thread>
#include <unordered_map>
#include <memory>
#include <string>
#include <vector>
#include <array>
#include <atomic>
#include <pluginlib/class_loader.hpp>
#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 <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"
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<hardware_interface::StateInterface> export_state_interfaces() override;
ETHERCAT_DRIVER_PUBLIC
std::vector<hardware_interface::CommandInterface> 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&&id<NUM_SLAVES+1){
motor_enable_arr[id-1].store(enable);
}
};
bool get_motor_enable(int id){
if(id>0&&id<NUM_SLAVES+1){
return motor_enable_arr[id-1].load();
}
return false;
}
struct timespec timespec_add(struct timespec time1, struct timespec time2);
private:
std::chrono::high_resolution_clock::time_point rec_time;
std::array<std::atomic<bool>, NUM_SLAVES> motor_enable_arr;
std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
std::string & urdf, std::string component_name, std::string component_type);
///std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
std::vector<std::vector<double>> hw_joint_commands_;
std::vector<double> hw_cmd_position_;
std::vector<std::vector<double>> hw_sensor_commands_;
std::vector<std::vector<double>> hw_gpio_commands_;
std::vector<std::vector<double>> hw_joint_states_;
std::vector<std::vector<double>> hw_sensor_states_;
std::vector<std::vector<double>> hw_gpio_states_;
//pluginlib::ClassLoader<ethercat_interface::EcSlave> 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<bool> 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_

49
ecrt_dev/include/ethercat_driver/visibility_control.h
View File

@@ -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_

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

@@ -1,115 +0,0 @@
#include "ecrt.h"
#include <math.h>
#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}
};

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

@@ -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)

25
ecrt_dev/package.xml
View File

@@ -1,25 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ecrt_driver</name>
<version>1.2.0</version>
<description>EtherCAT Driver for `ros2_control`</description>
<maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake_ros</buildtool_depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_lifecycle</depend>
<depend>ethercat_interface</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

790
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -1,790 +0,0 @@
#include <sys/mman.h> // 用于 mlockall
#include <pthread.h> // 用于线程优先级设置
#include <sched.h> // 用于调度策略
#include <cstring> // 用于 strerror
#include <unistd.h> // 用于 getpid
#include <sys/resource.h>
#include "ethercat_driver/ethercat_driver.hpp"
#include <tinyxml2.h>
#include <string>
#include <regex>
#include"ecrt.h"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
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, &param) != 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<std::mutex> lock(ec_mutex_);
activated_.store(false,std::memory_order_relaxed);
for(int i=0;i<NUM_SLAVES;i++){
set_motor_enable(i+1,false);
}
hw_joint_states_.resize(info_.joints.size());
for (uint j = 0; j < info_.joints.size(); j++) {
hw_joint_states_[j].resize(
info_.joints[j].state_interfaces.size(),
std::numeric_limits<double>::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<double>::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<double>::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<double>::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<double>::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<double>::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<hardware_interface::StateInterface>
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<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface>
EthercatDriver::export_command_interfaces()
{
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%ld",info_.joints.size());
std::vector<hardware_interface::CommandInterface> command_interfaces;
// export joint command interface
///std::vector<double> 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<std::mutex> 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<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);
}
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;
}
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);
all_motor_op=false;
return CallbackReturn::SUCCESS;
}
CallbackReturn EthercatDriver::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
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);
// 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 状态
}
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<std::mutex> 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;i<info_.joints.size();i++){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[%d]%s:",i,info_.joints[i].name.c_str());
for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"%.2f,",hw_joint_commands_[i][j]);
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"\n");
}
#endif
#if 1
wakeupTime = timespec_add(wakeupTime, cycletime);
////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
#endif
ecrt_master_receive(master);
ecrt_domain_process(domain1);
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
check_master_state();
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);
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)<CSP_DEADBAND1) {
step_pos[i] = target_pos;
reach_target[i]=1;
}else if(abs(err_cmd)<CSP_DEADBAND2){
if(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<std::mutex> 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<std::unordered_map<std::string, std::string>> 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<std::unordered_map<std::string, std::string>> module_params;
std::unordered_map<std::string, std::string> 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)

544
ecrt_dev/src/test_motor.cpp
View File

@@ -1,544 +0,0 @@
#include "rclcpp/rclcpp.hpp"
#include <filesystem>
#include <fstream> // 添加这行来支持文件流操作
#include <time.h>
#include <termios.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#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 <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;
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<double> angles);
void pubTraj(const std::vector<double> angles1,const std::vector<double> angles2);
void pubTraj(const double angle);
bool motor_drv_on=false;
bool all_motor_op=false;
bool is_reach=false;
std::unordered_map<std::string,double> curMap_;
std::unordered_map<std::string,double> dstMap_;
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_; // 数据文件路径
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_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);
// 订阅仿真状态
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/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<double> 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;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(all_motor_op){
if(is_reach){
////pubTraj(angles);
}
}
}
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;
}
void TestMotor::pubTraj(const std::vector<double> angles1,const std::vector<double> 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<<jname<<":"<<curMap_[jname]<<"-->"<<tempValue.values[0]<<std::endl;
posMsg_.interface_values.push_back(tempValue);
}
gpioPub_->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<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
std::cout<<std::endl;
gpioPub_->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(i<id)
tempValue.values = {enable};
else
tempValue.values = {0};
///std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
for(int i=0;i<6;i++){
std::string tempInterfaceGroup = "right_arm_joint_" + std::to_string(i+1);
posMsg_.interface_groups.push_back(tempInterfaceGroup);
control_msgs::msg::InterfaceValue tempValue;
//enable
tempValue.interface_names = {"enable"};
if(i<id)
tempValue.values = {enable};
else
tempValue.values = {0};
///std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
posMsg_.interface_values.push_back(tempValue);
}
///std::cout<<std::endl;
gpioPub_->publish(posMsg_);
usleep(1000000);
}
void TestMotor::setJointValueTarget(std::vector<double> 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<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);
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<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();
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<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_loop(int motor_id,int loop_cnt){
std::cout<<"start test:"<<motor_id<<std::endl;
////motor_fault(0,1);
motor_enable(13,1);
if(loop_cnt%2==0)
//motor_pos(motor_id,3000);
motor_action(motor_id,0.1);
else
//motor_pos(motor_id,-3000);
motor_action(motor_id,-0.1);
///usleep(3*1000000);
}
void TestMotor::all_motor(){
loop_cnt+=1;
static int sw=0;
motor_loop(0,loop_cnt);
#if 0
int mid=loop_cnt%100;
if(mid<13&&mid>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<TestMotor>();
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<<""<<std::endl;
while(rclcpp::ok()&&!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::vector<double> angles1={0, 0, 0, 0, 0, 0};
std::vector<double> 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;
}

3
ecrt_dev_dual/src/ethercat_driver.cpp
View File

@@ -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;