ROS2 CANopen 使用教程

系列文章目录

 

---

前言

 

---

 

一、快速入门

1.1 安装

        将 ros2_canopen 克隆到 ROS2 工作区的源文件夹中，安装依赖项并使用 colcon 构建，就大功告成了。

git clone https://github.com/ros-industrial/ros2_canopen.git
cd ..
rosdep install --from-paths src/ros2_canopen --ignore-src -r -y
colcon build

1.2 设置 CAN 控制器

        选项 1：虚拟 CANController

sudo modprobe vcan
sudo ip link add dev vcan0 type vcan
sudo ip link set vcan0 txqueuelen 1000
sudo ip link set up vcan0

        选项 2：Peak CANController

sudo modprobe peak_usb
sudo ip link set can0 up type can bitrate 1000000
sudo ip link set can0 txqueuelen 1000
sudo ip link set up can0

        比特率取决于总线和设备的能力。

        选项 3：candleLight USB-CAN 适配器

sudo modprobe gs_usb
sudo ip link set can0 up type can bitrate 500000
sudo ip link set can0 txqueuelen 1000
sudo ip link set up can0

        比特率取决于总线和设备的性能。

        选项 4：将这些步骤适用于其他 socketcan 设备

1.3 运行示例

        为了试用该库，我们在 canopen_tests 目录中提供了一些示例。如果您已启动 vcan0 接口，则可以运行这些示例。

1.3.1 服务接口

ros2 launch canopen_tests cia402_setup.launch.py

1.3.2 托管服务接口

ros2 launch canopen_tests cia402_lifecycle_setup.launch.py

1.3.3 ROS2 控制

        Proxy 设置

ros2 launch canopen_tests canopen_system.launch.py

        CiA402 设置

ros2 launch canopen_tests cia402_system.launch.py

        机器人设置

ros2 launch canopen_tests robot_control_setup.launch.py

二、用户指南

2.1 操作

        ros2_canopen 堆栈有三种不同的使用方式：

• - 标准节点容器
• - 托管节点容器
• - 标准节点的 ros2_control 系统接口

2.1.1 简单节点容器

        标准节点容器模式将主节点和所有从属驱动节点捆绑在一个称为设备容器的专用容器中。所有节点都是简单的 ROS 2 节点，并提供发布、订阅和服务接口。一旦设备容器启动，所有节点都会被调出并准备就绪。
        目的：简单节点容器适用于用户需要简单易用的启动界面，且不需要 ros2_control 提供任何实时控制功能的应用。

2.1.2 托管节点容器

        托管节点容器的属性与标准节点容器相同。不同的是，所有节点都是生命周期节点，并且有一个名为生命周期管理器的特殊节点。用户可以使用生命周期管理器来控制容器中所有节点的生命周期。
        目的：受管节点容器适用于用户希望拥有比杀死和重启容器更多运行时恢复选项的应用程序。

2.1.3 ROS 2 控制系统接口

        ros2_control 接口目前建立在简单节点容器之上。除了标准节点容器之外，ros2_control 系统接口还提供了一个硬件接口，可用于控制总线上的设备。目前，我们提供了三种不同的系统接口：

• - canopen_ros2_control/CANopenSystem
• - canopen_ros2_control/CIA402System
• - canopen_ros2_control/RobotSystem

        目的：ROS 2 控制系统接口适用于需要低延迟的控制应用程序。

2.2 配置包

        配置包包含 CANopen 协议栈的一个或多个配置。配置包需要包含每个设备的 EDS/DCF 文件、总线配置以及不同配置的启动文件。

        因此，配置包的结构应如下所示：

{package_name}
├── config
│   ├── {bus_config_name_1}
│   |   ├── bus.yml
│   |   ├── {device1}.eds
│   |   ├── {device...}.eds
│   |   └── {slave_n}.eds
│   └── {bus_config_name_2}
│       ├── bus.yml
│       ├── {device1}.eds
│       ├── {device...}.eds
│       └── {slave_n}.eds
├── launch
│   ├── {bus_config_name_1}.launch.py
|   └── {bus_config_name_1}.launch.py
├── CMakeLists.txt
└── package.xml

2.3 总线配置文件

        ros2_canopen 堆栈依赖于一个 YAML 配置文件，该文件用于配置总线拓扑和指定每个设备的配置。该文件详细说明了哪些设备连接到总线、哪些 EDS/DCF 文件适用于这些设备、哪些 EDS/DCF 文件的参数应被覆盖以及哪些驱动程序应被用于控制这些设备。

2.3.1 结构

        YAML 配置文件包含以下部分：

options: # General options especially dcf_path
  [configuration item]: [value]

master: # The configuration of the master
  [configuration item]: [value]
  [...]

defaults: # Defaults that apply to all slave nodes
  [configuration item]: [value]
  []

nodes: # Configurations for all slave nodes
  - [device_name]:
      [configuration item]: [value]
      [...]

2.3.2 选项部分

        选项部分包含一般选项。目前只有以下选项。

选项配置

配置项	描述
dcf_path	运行时生成的 .bin 文件所在的目录。您可以将其设置为"@BUS_CONFIG_PATH@"，这样 cmake 就会自动生成该文件。

2.2.4 Master 部分

        Master 部分有许多配置选项。这些选项并非 ros2_canopen 独有，而是来自 lely 核心库。下面是可能的配置项列表。 

Master Configuration

配置项目	描述
node_id（节点 ID）	节点 ID（默认值：255）
driver（驱动程序）	The fully qualified class name of the master to use. （要使用的主控程序的全限定类名。）
package（软件包）	The ros2 package name in which the master class can be found. （主站类所在的 ros2 软件包名称。）
baudrate（波特率）	The baudrate in kbit/s (default: 1000) （以 kbit/s 为单位的波特率（默认值：1000））
vendor_id	The vendor-ID (default: 0x00000000) （供应商标识（默认值：0x00000000））
product_code	The product code (default: 0x00000000) （产品编码（默认值：0x00000000））
revision_number	The revision number (default: 0x00000000). （版本号（默认值：0x00000000）。）
serial_number	The serial number (default: 0x00000000). （序列号（默认值：0x00000000）。）
heartbeat_multiplier （心跳倍增器）	The multiplication factor used to obtain the slave heartbeat consumer time from the master heartbeat producer time (default: see options section). （用于从主心跳生产者时间获取从心跳消费者时间的乘法系数（默认值：参见选项部分）。）
heartbeat_consumer （心跳消费者）	Specifies whether the master should monitor the heartbeats of the slaves (default: true). （指定主站是否应监控从站的心跳（默认值：true）。）
heartbeat_producer （心跳生产者）	The heartbeat producer time in ms (default: 0). （以毫秒为单位的心跳产生器时间（默认值：0）。）
emcy_inhibit_time	The EMCY inhibit time in multiples of 100 μs (default: 0, see object 1015). （以 100 μs 的倍数表示的 EMCY 抑制时间（默认值：0，参见对象 1015）。）
sync_period （同步周期）	The SYNC interval in μs (default: 0). （同步时间间隔，单位为 μs（默认值：0）。）
sync_window （同步窗口）	The SYNC window length in μs (default: 0, see object 1007). （同步窗口长度，单位为 μs（默认值：0，参见对象 1007）。）
sync_overflow	The SYNC counter overflow value (default: 0, see object 1019). （SYNC 计数溢出值（默认值：0，参见对象 1019）。）
error_behavior	A dictionary of error behaviors for different classes or errors (default: {1: 0x00}, see object 1029). （不同类别或错误的错误行为字典（默认值：{1: 0x00}，请参见对象 1029）。）
nmt_inhibit_time	The NMT inhibit time in multiples of 100 μs (default: 0, see object 102A). （以 100 μs 的倍数表示的 NMT 抑制时间（默认值：0，参见对象 102A）。）
start	Specifies whether the master shall switch into the NMT operational state by itself (default: true, see bit 2 in object 1F80). （指定主站是否应自行切换到 NMT 运行状态（默认值：true，参见对象 1F80 中的第 2 位）。）
start_nodes	Specifies whether the master shall start the slaves (default: true, see bit 3 in object 1F80). （指定主站是否应启动从站（默认值：true，参见对象 1F80 中的第 3 位）。）
start_all_nodes	Specifies whether the master shall start all nodes simultaneously (default: false, see bit 1 in object 1F80). （指定主站是否同时启动所有节点（默认值：false，参见对象 1F80 中的第 1 位）。）
reset_all_nodes	Specifies whether all slaves shall be reset in case of an error event on a mandatory slave (default: false, see bit 4 in object 1F80). （指定在强制从节点发生错误事件时是否重置所有从节点（默认值：false，参见对象 1F80 中的第 4 位）。）
stop_all_nodes	Specifies whether all slaves shall be stopped in case of an error event on a mandatory slave (default: false, see bit 6 in object 1F80). （指定在强制从属设备发生错误事件时是否停止所有从属设备（默认值：false，参见对象 1F80 中的第 6 位）。）
boot_time	The timeout for booting mandatory slaves in ms (default: 0, see object 1F89). （启动强制从属设备的超时时间，以毫秒为单位（默认值：0，参见对象 1F89）。）
boot_timeout	The timeout for booting all slaves in ms (default: 2000ms). （启动所有从属设备的超时时间，以毫秒为单位（默认值：2000ms）。）

2.3.3 设备部分

        通过设备配置可以配置所连接 CANopen 设备的特性。

注意事项

        需要注意的是，在选择操作（简单节点或托管节点）时，应仅选择生命周期驱动程序或仅选择简单驱动程序。

        混合使用将不起作用！

配置项	描述
driver	The fully qualified class name of the driver to use. （要使用的驱动程序的全限定类名。）
package	The ros2 package name in which the driver class can be found. （可以找到驱动程序类的 ros2 软件包名称。）
enable_lazy_load	A flag that states whether the driver is loaded on start-up. （表示是否在启动时加载驱动程序的标志。）
dcf	The filename of the EDS/DCF describing the slave (mandatory). （描述从属设备的 EDS/DCF 文件名（必须填写）。）
dcf_path	The directory in which the generated .bin file will be available at runtime (default: see options section). （运行时生成的 .bin 文件所在的目录（默认值：参见选项部分）。）
node_id	The node-ID (default: 255, can be omitted if specified in the DCF). （节点 ID（默认值：255，如果在 DCF 中指定，则可省略）。）
revision_number	The revision number (default: 0x00000000, can be omitted if specified in the DCF). （修订版本号（默认值：0x00000000，如果在 DCF 中指定，则可省略）。）
serial_number	The serial number (default: 0x00000000, can be omitted if specified in the DCF). （序列号（默认值：0x00000000，可在 DCF 中省略）。）
heartbeat_multiplier	The multiplication factor used to obtain master heartbeat consumer time from the slave heartbeat producer time (default: see options section). （用于从心跳生产者时间中获取主心跳消费者时间的乘法系数（默认值：参见选项部分）。）
heartbeat_consumer	Specifies whether the slave should monitor the heartbeat of the master (default: false). （指定从属设备是否应监控主设备的心跳（默认值：false）。）
heartbeat_producer	The heartbeat producer time in ms (default: 0). （以毫秒为单位的心跳生产者时间（默认值：0）。）
error_behavior	A dictionary of error behaviors for different classes or errors (default: {}, see object 1029). （针对不同类别或错误的错误行为字典（默认值：{}，参见对象 1029）。）
rpdo	The Receive-PDO configuration (see below). （接收-PDO 配置（见下文）。）
tpdo	The Transmit-PDO configuration (see below). （发送-PDO 配置（见下文）。）
boot	Specifies whether the slave will be configured and booted by the master (default: true, see bit 2 in object 1F81). （指定从站是否由主站配置和启动（默认值：true，参见对象 1F81 中的第 2 位）。）
mandatory	Specifies whether the slave is mandatory (default: false, see bit 3 in object 1F81). （指定从属设备是否必须使用（默认值：false，参见对象 1F81 中的第 3 位）。）
reset_communication	Specifies whether the NMT reset communication command may be sent to the slave (default: true, see bit 4 in object 1F81). （指定是否可以向从属设备发送 NMT 复位通信命令（默认值：true，参见对象 1F81 中的第 4 位）。）
software_file	The name of the file containing the firmware (default: “”, see object 1F58). （含有固件的文件名（默认值：""，参见对象 1F58）。）
software_version	The expected software version (default: 0x00000000, see object 1F55).（预期软件版本（默认值：0x00000000，参见对象 1F55）。）
configuration_file	The name of the file containing the configuration (default: “<dcf_path>/<name>.bin” (where <name> is the section name), see object 1F22).（包含配置的文件名（默认值："<dcf_path>/<name>.bin"（其中 <name> 为部分名称），请参见对象 1F22）。）
restore_configuration	The sub-index of object 1011 to be used when restoring the configuration (default: 0x00).（恢复配置时使用的对象 1011 的子索引（默认值：0x00）。）
sdo_timeout_ms	The timeout to use for SDO reads/writes to this device. (default: 20ms) （用于 SDO 读/写该设备的超时。(默认值：20ms））
sdo	Additional SDO requests to be sent during configuration (see below). （配置期间要发送的附加 SDO 请求（见下文）。）

2.3.4 更多参考资料

        dcfgen 文档详细介绍了如何使用 dcfgen 工具生成 DCF： https://opensource.lely.com/canopen/docs/dcf-tools/

2.3.5 变量

        @bus_config_path@： 如果遵循配置包结构，则自动定义配置路径。

2.4 配置包 CMake

        为了构建配置包并从总线配置文件和 eds/dcf 文件生成必要的运行时工件，ly_core_libraries 包包含一个额外的 CMAKE 宏。

cogen_dcf(target)

        目标：配置的名称（例如，config/{bus_config_name_1} 就是 bus_config_name_1）

cogen_dcf(bus_config)

2.5 主驱动程序

        主驱动程序负责创建必要的 can 接口，并建立一个 canopen 事件循环，供驱动程序挂接。此外，该节点还提供通过 nmt 和 sdo 与节点通信的服务。

Master Drivers

Type	Package	Name
lifecycle	canopen_master_driver	ros2_canopen::LifecycleMasterDriver
simple	canopen_master_driver	ros2_canopen::MasterDriver

2.5.1 Services

Services	Type	Description
~/read_sdo	COReadID	Reads an SDO object specified by Index, Subindex and Datatype of the device with the specified nodeid. （读取一个 SDO 对象，该对象由指定节点 ID 的设备的 Index、Subindex 和 Datatype 指定。）
~/write_sdo	COWriteID	Writes Data to an SDO object specified by Index, Subindex and Datatype on the device with the specified nodeid. （向指定节点 ID 的设备上由 Index、Subindex 和 Datatype 指定的 SDO 对象写入数据。）
~/set_heartbeat	COHeartbeatID	Sets the heartbeat of the device with the specified nodeid to the heartbeat value (ms) （将指定节点 ID 的设备心跳设置为心跳值（毫秒））
~/set_nmt	CONmtID	Sends the NMT command to the device with the specified nodeid （向指定节点 ID 的设备发送 NMT 命令）

2.6 Proxy Driver（代理驱动程序）

代理驱动程序通过 ROS2 服务和报文转发特定设备的 CANopen 功能。

Proxy Drivers

Type	Package	Name
lifecycle	canopen_proxy_driver	ros2_canopen::LifecycleProxyDriver
simple	canopen_proxy_driver	ros2_canopen::ProxyDriver

2.6.1 Services

Services	Type	Description
~/nmt_reset_node	Trigger	Resets CANopen Device the Proxy Device Node manages. （重置代理设备节点管理的 CANopen 设备。）
~/sdo_read	CORead	Reads an SDO object from the specified index, subindex and datatype of the remote device. （从远程设备的指定索引、子索引和数据类型中读取 SDO 对象。）
~/sdo_write	COWrite	Writes data to an SDO object on the specified index, subindex and datatype of the remote device. （根据远程设备的指定索引、子索引和数据类型向 SDO 对象写入数据。）

2.6.2 Publishers

Topic	Type	Description
~/nmt_state	String	Publishes NMT state on change （更改时发布 NMT 状态）
~/rpdo	COData	Publishes received PDO objects on reception （在接收时发布收到的 PDO 对象）

2.6.3 Subscribers

Topic	Type	Description
~/tpdo	COData	Writes received data to remote device if the specified object is RPDO mapped on remote device. （如果指定对象是远程设备上的 RPDO 映射，则将接收到的数据写入远程设备。）

 2.7 Cia402 驱动器

        Cia402 驱动器为运动控制器实现了 CIA402 配置文件，可设置驱动器状态、运行模式并向运动控制器发送目标值。

CIA402 Drivers

Type	Package	Name
lifecycle	canopen_402_driver	ros2_canopen::LifecycleCia402Driver
simple	canopen_402_driver	ros2_canopen::Cia402Driver

2.7.1 Services

Services	Type	Description
~/nmt_reset_node	Trigger	Resets CANopen Device the Proxy Device Node manages. （重置代理设备节点管理的 CANopen 设备。）
~/sdo_read	CORead	Reads an SDO object from the specified index, subindex and datatype of the remote device. （从远程设备的指定索引、子索引和数据类型中读取 SDO 对象。）
~/sdo_write	COWrite	Writes data to an SDO object on the specified index, subindex and datatype of the remote device. （根据远程设备的指定索引、子索引和数据类型向 SDO 对象写入数据。）
~/init	Trigger	Initialises motion controller including referencing （初始化运动控制器，包括参照）
~/recover	Trigger	Recovers motion controller （恢复运动控制器）
~/halt	Trigger	Stops motion controller （停止运动控制器）
~/position_mode	Trigger	Switches to profiled position mode （切换到轮廓定位模式）
~/velocity_mode	Trigger	Switches to profiled velocity mode （切换到速度曲线模式）
~/torque_mode	Trigger	Switches to profiled torque mode （切换到曲线扭矩模式）
~/cyclic_position_mode	Trigger	Switches to cyclic position mode （切换到循环位置模式）
~/cyclic_velocity_mode	Trigger	Switches to cyclic velocity mode （切换到循环速度模式）
~/interpolated_position_mode	Trigger	Switches to interpolated position mode, only linear mode with fixed time is supported （切换到内插位置模式，仅支持固定时间的线性模式）
~/target	CODouble	Sets the target value. Only accepted when an operation mode is set. （设置目标值。仅在设置操作模式时接受。）

2.7.2 Publishers

Publishers	Type	Description
~/joint_states	sensor_msgs/msg/JointState	Joint states of the drive （驱动器的关节状态）
~/nmt_state	String	Publishes NMT state on change
~/rpdo	COData	Publishes received PDO objects on reception

2.7.3 Subscribers

Topic	Type	Description
~/target	COTargetDouble	Sets target value.
~/tpdo	COData	Writes received data to remote device if the specified object is RPDO mapped on remote device.

2.7.4 总线配置参数

        可用于此驱动程序 bus.yml 的附加参数。

Parameter	Type	Description
polling	bool	Enables polling of the drive status. Default: true. If false, period will be used to run a ros2 timer as update loop. If true, the update loop will be triggered by the sync signal and directly executed in the canopen realtime loop. This requires all data processed in the update loop to be PDO, otherwise the loop will get stuck. This can speed reduce processor load significantly though. （启用驱动器状态轮询。默认值：true。如果为假，将使用周期运行 ros2 定时器作为更新循环。如果为 true，更新循环将由同步信号触发，并直接在 canopen 实时循环中执行。这就要求更新循环中处理的所有数据都必须是 PDO，否则循环就会卡住。不过这可以大大降低处理器负载。）
period	Milliseconds	Refresh period for 402 state machine. Should be similar to sync period of master. （402 状态机的刷新周期。应类似于主站的同步周期。）
switching_state	see below	The state to switch the operation mode in. （切换运行模式的状态。）
scale_pos_to_dev	double	Scaling factor to convert from SI units to device units for position. （缩放因子，用于将位置单位从国际单位制转换为设备单位。）
scale_vel_to_dev	double	Scaling factor to convert from SI units to device units for velocity. （缩放因子，用于将速度单位从 SI 单位转换为设备单位。）
scale_pos_from_dev	double	Scaling factor to convert from device units to SI units for position. （缩放因子，用于将设备单位转换为位置的 SI 单位。）
scale_vel_from_dev	double	Scaling factor to convert from device units to SI units for velocity. （缩放因子，用于将速度单位从设备单位转换为 SI 单位。）
position_mode	int	The drives operation mode to use for the position interface （位置接口使用的驱动器运行模式）
velocity_mode	int	The drives operation mode to use for the velocity interface （速度接口使用的驱动器运行模式）
torque_mode	int	The drives operation mode to use for the torque interface （扭矩接口使用的驱动器运行模式）

2.8 如何创建配置包

        为了在机器人上使用ros2_canopen协议栈，您需要创建一个配置包，其中包含总线配置和启动脚本。下文将详细介绍创建配置包的步骤。

2.8.1 创建配置包

        创建配置包时，首先要做出一些决定。您需要选择软件包名称，决定需要哪些总线配置（通常是每个 CAN 接口一个），以及拥有哪些从站。

• package_name：软件包名称
• bus_config_name：总线配置的名称（可以有多个）。

        您可以使用 ros2 pkg create 命令创建软件包。

ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name}
cd {package_name}
rm -rf src
rm -rf include
mkdir -p launch
mkdir -p config

         现在你的软件包目录应该是这样的。

{package_name}
├── config
├── launch
├── CMakeLists.txt
└── package.xml

2.8.2 创建总线配置

1. 总线配置决策决定需要多少总线配置。在 config 文件夹中为每个总线配置添加一个子文件夹。

   mkdir -p {bus_config_name}

2. 为总线配置添加设备信息
   为总线配置创建文件夹后，将总线配置中设备的 .eds 文件添加到相应文件夹中。

   现在，您的软件包目录应该是这样的。

   {package_name}
   ├── config
   │   ├── {bus_config_name_1}
   │   |   ├── {device1}.eds
   │   |   ├── {device...}.eds
   │   |   └── {slave_n}.eds
   │   └── {bus_config_name_2}
   │       ├── {device1}.eds
   │       ├── {device...}.eds
   │       └── {slave_n}.eds
   ├── CMakeLists.txt
   └── package.xml

3. 创建总线配置规范
   为指定总线配置，ros2_canopen 使用名为 bus.yml 的 YAML 文件。请在相应的总线配置文件夹中创建该文件。

   touch bus.yml

   {package_name}
   ├── config
   │   ├── {bus_config_name_1}
   │   |   ├── bus.yml
   │   |   ├── {device1}.eds
   │   |   ├── {device...}.eds
   │   |   └── {slave_n}.eds
   │   └── {bus_config_name_2}
   │       ├── bus.yml
   │       ├── {device1}.eds
   │       ├── {device...}.eds
   │       └── {slave_n}.eds
   ├── CMakeLists.txt
   └── package.xml

4. 编辑总线配置规范
   您需要根据需要修改每个 bus.yml 文件。首先，您需要定义这些文件和生成文件在运行时的位置。如果使用 colcon 从源代码构建，通常需要定义如下内容。

   options:
     dcf_path: install/{package_name}/share/{package_name}/config/{bus_config_name}

   然后，你需要确定你的主站。

   master:
     node_id: [node id]
     package: [ros2 package where to find the master driver (usually canopen_core)]
     driver: [component type of the driver (ros2_canopen::MasterDriver or ros2_canopen::LifecycleMasterDriver)]

   如果使用 ros2_canopen 的生命周期版本，请确保指定了生命周期主控程序。并根据需要添加其他配置数据。有关可用配置选项的文档，请参阅配置包文档。

   定义好主进程的配置后，就可以添加从进程了。下文介绍了每个从属设备的必填数据。更多配置选项请参阅配置包文档。从站名称是分配给驱动程序的节点名称。

   nodes:
     - [unique slave name]:
       node_id: [node id]
       package: [ros2 package where to find the driver]
       driver: [qualified name of the driver]

   如果使用生命周期版本的 ros2_canopen，请确保使用生命周期从属程序。

2.8.3 创建启动配置

        在软件包目录下创建启动文件夹和启动文件。

mkdir launch
touch {...}.launch.py

添加以下代码

def generate_launch_description():
      """Generate launch description with multiple components."""
      path_file = os.path.dirname(__file__)

      ld = launch.LaunchDescription()


      device_container = IncludeLaunchDescription(
          PythonLaunchDescriptionSource(
              [
                  os.path.join(get_package_share_directory("canopen_core"), "launch"),
                  "/canopen.launch.py",
              ]
          ),
          launch_arguments={
              "master_config": os.path.join(
                  get_package_share_directory("{package_name}"),
                  "config",
                  "{bus_config_name}",
                  "master.dcf",
              ),
              "master_bin": os.path.join(
                  get_package_share_directory("{package_name}"),
                  "config",
                  "{bus_config_name}",
                  "master.bin",
              ),
              "bus_config": os.path.join(
                  get_package_share_directory("{package_name}"),
                  "config",
                  "{bus_config_name}",
                  "bus.yml",
              ),
              "can_interface_name": "{can_interface_name i.e. can0}",
          }.items(),

      )

      ld.add_action(device_container)

      return ld

2.8.4 创建 CMAKE 配置

        最后，我们需要调整 CMakeLists.txt 文件，以正确接收所有内容。

cmake_minimum_required(VERSION 3.8)
project({package_name})

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(canopen_core REQUIRED)
find_package(canopen_interfaces REQUIRED)
find_package(canopen_base_driver REQUIRED)
find_package(canopen_proxy_driver REQUIRED)
find_package(lely_core_libraries REQUIRED)


cogen_dcf({bus_config_name})

install(DIRECTORY
  launch/
  DESTINATION share/${PROJECT_NAME}/launch/
)

install(DIRECTORY
  launch_tests/
  DESTINATION share/${PROJECT_NAME}/launch_tests/
)


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

2.9 如何使用 ros2_control 创建 cia301 系统？

        CiA 301 配置文件又称 CANopen 应用层，是 CANopen 协议的基础。它定义了 CANopen 系统中用于设备通信和网络管理的基本原则、通信服务和对象字典。

        CiA 301 配置文件定义了多项通信服务，允许设备交换数据和执行操作。这些服务包括用于在设备间传输对象数据的 SDO（服务数据对象）服务、用于实时数据交换的 PDO（过程数据对象）服务，以及用于网络初始化、设备状态控制和错误处理的 NMT（网络管理）服务。

        使用 ROS2 启动 CiA301 接口包括三个步骤：

• 为总线和 ROS2_control 准备配置
• 准备状态和命令接口
• 最后是准备启动文件。

2.9.1 准备配置

        要使用 CiA301 配置文件的控制系统接口，我们应准备以下配置

• bus_conf

• master_dcf
• master_bin
• can_interface，（默认：vcan0）

        定义总线配置参数

master:
    node_id: 1
    driver: "ros2_canopen::MasterDriver"
    package: "canopen_master_driver"
    baudrate: 250
options:
    dcf_path: "@BUS_CONFIG_PATH@"
joint_1:
    node_id: 0x00
    dcf: "joint.eds"
    driver: "ros2_canopen::ProxyDriver"
    package: "canopen_proxy_driver"
    reset_communication: false
joint_2:
    node_id: 0x01
    dcf: "joint.eds"
    driver: "ros2_canopen::ProxyDriver"
    package: "canopen_proxy_driver"
    reset_communication: false

        定义 ros2_control 参数

controller_manager:
    ros__parameters:
        update_rate: 100  # Hz

        joint_state_broadcaster:
            type: joint_state_broadcaster/JointStateBroadcaster

        joint_1_controller:
            type: canopen_ros2_controllers/CanopenProxyController

        joint_2_controller:
            type: canopen_ros2_controllers/CanopenProxyController

    joint_1_controller:
        ros__parameters:
            joint: joint_1


   joint_2_controller:
        ros__parameters:
            joint: joint_2

         master_dcf 的示例见 https://github.com/ros-industrial/ros2_canopen/blob/master/canopen_tests/config/simple/simple.eds

2.9.2 使用 RPDO 访问当前状态

2.9.2.1 定义接头和 CANopen 数据结构

        第一步是定义一个结构，用于保存有关接头和相关 CANopen 数据的信息。该结构是 CANopen 网络的基础，可确保存储所有相关数据并在需要时进行访问。

2.9.2.2 PDO 索引和子索引

        每个过程数据对象（PDO）都有一个索引和一个子索引。索引是每个 PDO 的唯一标识符，将其与系统中的其他 PDO 区分开来。子索引用于访问每个 PDO 中的单个数据字段，因为一个 PDO 可以包含多个数据字段。

2.9.2.3 网络管理（NMT）

        网络管理（NMT）是 CANopen 协议套件中的一项基本服务。它提供基本的设备控制命令，如启动、停止和复位，并管理网络中设备的状态。

        对于 RPDO，使用以下方式定义数据：

"rpdo/index

"rpdo/subindex

"rpdo/type

"rpdo/data

        对于 NMT，我们可以通过以下方式读取状态

"nmt/state

2.9.3 使用 TPDO 发送命令

        为了向 CANopen 网络中的硬件设备发送命令，我们首先需要导出相应的硬件接口。这是一个关键步骤，可使我们有效地控制网络中的每个接头。

2.9.3.1 注册传输过程数据对象（TPDOs）

        与处理状态接口的方法类似，我们必须为每个关节注册传输过程数据对象（TPDOs）。这些 TPDO 与以下命令相关：

• "tpdo/index
• "tpdo/subindex
• "tpdo/type
• "tpdo/data
• "tpdo/owns

2.9.3.2 网络管理 (NMT) 命令

        除此之外，我们还能注册与网络管理 (NMT) 相关的命令，以控制网络内设备的状态。这对设备的顺利运行和控制非常重要。与 NMT 相关的命令包括

• "nmt/reset
• "nmt/reset_fbk
• "nmt/start
• "nmt/start_fbk

        这些 NMT 命令不仅有助于管理设备状态，还能在执行命令后从设备向控制系统提供反馈（用 "fbk "表示）。这种反馈机制对于确保成功执行命令和管理网络的整体健康至关重要。

2.9.4 如何启动节点

        最后，我们为接口准备启动文件。示例见：https://github.com/ros-industrial/ros2_canopen/blob/master/canopen_ros2_control/launch/canopen_system.launch.py

        有关测试，请参阅以下部分。

2.10 如何使用 ros2_control 创建机器人系统

        本指南介绍如何利用 ros2_control 创建一个简单的机器人系统硬件接口。

1. 创建一个新的配置包，名称为 canopen_robot_control_example。

   ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name}
   cd {package_name}
   rm -rf src
   rm -rf include
   mkdir -p launch
   mkdir -p config

2. 新建一个名为 robot_control 的总线配置文件夹。

   mkdir -p config/robot_control

3. 创建一个新的总线配置文件，文件名为 bus.yml。

   touch config/robot_control/bus.yml

4. 在 bus.yml 文件中添加以下内容

   options:
     dcf_path: "@BUS_CONFIG_PATH@"
   
   master:
     node_id: 1
     driver: "ros2_canopen::MasterDriver"
     package: "canopen_master_driver"
     sync_period: 10000
   
   defaults:
     dcf: "cia402_slave.eds"
     driver: "ros2_canopen::Cia402Driver"
     package: "canopen_402_driver"
     period: 10
     position_mode: 1
     revision_number: 0
     sdo:
       - {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms
       - {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s
       - {index: 0x6081, sub_index: 0, value: 1000}
       - {index: 0x6083, sub_index: 0, value: 2000}
       - {index: 0x6060, sub_index: 0, value: 7}
     tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
       1:
         enabled: true
         cob_id: "auto"
         transmission: 0x01
         mapping:
           - {index: 0x6041, sub_index: 0} # status word
           - {index: 0x6061, sub_index: 0} # mode of operation display
       2:
         enabled: true
         cob_id: "auto"
         transmission: 0x01
         mapping:
           - {index: 0x6064, sub_index: 0} # position actual value
           - {index: 0x606c, sub_index: 0} # velocity actual position
     rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
       1:
         enabled: true
         cob_id: "auto"
         mapping:
         - {index: 0x6040, sub_index: 0} # controlword
         - {index: 0x6060, sub_index: 0} # mode of operation
       2:
         enabled: true
         cob_id: "auto"
         mapping:
         - {index: 0x607A, sub_index: 0} # target position
   
   nodes:
     joint_1:
       node_id: 2
     joint_2:
       node_id: 3

5. 将 canopen_tests/config/robot_control 软件包中的 cia402_slave.eds 文件复制到 config/robot_control 文件夹。

6. 创建 ros2_controllers.yaml，并添加以下内容。 

   controller_manager:
     ros__parameters:
       update_rate: 100  # Hz
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
   
       forward_position_controller:
         type: forward_command_controller/ForwardCommandController
   
   forward_position_controller:
     ros__parameters:
       joints:
         - joint1
         - joint2
       interface_name: position

7. 在软件包的启动目录下创建一个名为 robot_control.launch.py 的启动文件，并添加以下内容。

   from launch import LaunchDescription
   from launch.actions import DeclareLaunchArgument
   from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
   from launch_ros.actions import Node
   from launch_ros.substitutions import FindPackageShare
   from launch.actions import IncludeLaunchDescription
   from launch.launch_description_sources import PythonLaunchDescriptionSource
   
   
   def generate_launch_description():
       robot_description_content = Command(
           [
               PathJoinSubstitution([FindExecutable(name="xacro")]),
               " ",
               PathJoinSubstitution(
                   [
                       FindPackageShare("canopen_tests"),
                       "urdf",
                       "robot_controller",
                       "robot_controller.urdf.xacro",
                   ]
               ),
           ]
       )
       robot_description = {"robot_description": robot_description_content}
       robot_control_config = PathJoinSubstitution(
           [FindPackageShare("canopen_tests"), "config/robot_control", "ros2_controllers.yaml"]
       )
   
       control_node = Node(
           package="controller_manager",
           executable="ros2_control_node",
           parameters=[robot_description, robot_control_config],
           output="screen",
       )
   
       joint_state_broadcaster_spawner = Node(
           package="controller_manager",
           executable="spawner",
           arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
       )
   
       forward_position_controller_spawner = Node(
           package="controller_manager",
           executable="spawner",
           arguments=["forward_position_controller", "--controller-manager", "/controller_manager"],
       )
   
       robot_state_publisher_node = Node(
           package="robot_state_publisher",
           executable="robot_state_publisher",
           output="both",
           parameters=[robot_description],
       )
   
       slave_config = PathJoinSubstitution(
           [FindPackageShare("canopen_tests"), "config/robot_control", "cia402_slave.eds"]
       )
   
       slave_launch = PathJoinSubstitution(
           [FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"]
       )
       slave_node_1 = IncludeLaunchDescription(
           PythonLaunchDescriptionSource(slave_launch),
           launch_arguments={
               "node_id": "2",
               "node_name": "slave_node_1",
               "slave_config": slave_config,
           }.items(),
       )
   
       slave_node_2 = IncludeLaunchDescription(
           PythonLaunchDescriptionSource(slave_launch),
           launch_arguments={
               "node_id": "3",
               "node_name": "slave_node_2",
               "slave_config": slave_config,
           }.items(),
       )
   
       nodes_to_start = [
           control_node,
           joint_state_broadcaster_spawner,
           forward_position_controller_spawner,
           robot_state_publisher_node,
           slave_node_1,
           slave_node_2
       ]
   
       return LaunchDescription(nodes_to_start)

8. 创建 urdf 文件夹 将 canopen_tests/urdf/robot_controller 软件包中的所有文件添加到软件包的 urdf 文件夹中。

9. 编辑软件包的 CMakeLists.txt 文件，在 find_package 部分后添加以下几行。

   cogen_dcf(robot_control)
   
   install(DIRECTORY
   launch urdf
   DESTINATION share/${PROJECT_NAME})
   

10. 编译软件包并获取 setup.bash 文件的源代码。

11. 启动启动文件

12. 现在，您可以使用 forward_command_controller 控制器来控制机器人。您还可以在 rviz 中添加一个 tf 或机器人模型，并将固定框架设置为 base_link，从而实现机器人的可视化。您可以使用以下命令移动机器人。

    ros2 topic pub /joint1/forward_position_controller/command std_msgs/msg/Float64 "data: [1.0, 1.0]"

ros2 service call /cia402_device_1/sdo_write canopen_interfaces/srv/COWrite "{index: 0x1600, subindex: 0x01, data: 0x60400010, type: 32}"