ros2_control diff_drive_controller（一）+https://developer.aliyun.com/article/1585404
三、DiffBot（差速机器人）
DiffBot 或 "差分移动机器人 "是一种具有差分驱动的简单移动底座。该机器人基本上是一个根据差分驱动运动学原理移动的盒子。
在示例_2 中，硬件接口插件只有一个接口。
- 与机器人控制盒的通信使用专有的应用程序接口。
- 所有关节的数据均一次性交换。
DiffBot URDF 文件可在 description/urdf 文件夹中找到。
注意事项
以下命令既适用于本地安装本软件源及其附属程序，也适用于在 docker 容器中运行它们。有关 docker 使用的更多信息，请参阅使用 Docker。
3.1 教程步骤
- 要检查 DiffBot 说明是否正常工作，请使用以下启动命令
ros2 launch ros2_control_demo_example_2 view_robot.launch.py
警告
在终端中获得以下输出是正常的： 警告： 传给 canTransform 参数 target_frame 的无效帧 ID "odom"--帧不存在。出现这种情况是因为 joint_state_publisher_gui 节点需要一些时间才能启动。
-
- 要启动 DiffBot 示例，请打开终端，将 ROS2- 工作空间作为源代码，然后用以下命令执行启动文件
ros2 launch ros2_control_demo_example_2 diffbot.launch.py
- 启动文件加载并启动机器人硬件和控制器，并打开 RViz。在启动终端，您将看到硬件执行器的大量输出，显示其内部状态。过度打印只是为了演示。一般来说，在实现硬件接口时应尽量避免向终端打印。
如果能在 RViz 中看到一个橙色方框，那么一切都已正常启动。不过，为了确保万无一失，在移动 DiffBot 之前，我们还是要先检查一下控制系统。 - 打开另一个终端，执行
ros2 control list_hardware_interfaces
- 您应该获得
command interfaces left_wheel_joint/velocity [available] [claimed] right_wheel_joint/velocity [available] [claimed] state interfaces left_wheel_joint/position left_wheel_joint/velocity right_wheel_joint/position right_wheel_joint/velocity
- 命令接口上的[声称]标记意味着控制器可以对 DiffBot 发出命令。
此外，我们还可以看到命令接口的类型是速度，这是差分驱动机器人的典型特征。 - 检查控制器是否运行
ros2 control list_controllers
- 您应该获得
diffbot_base_controller[diff_drive_controller/DiffDriveController] active joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
- 如果一切正常，现在可以使用 ROS 2 CLI 界面向 Diff Drive Controller 发送命令：
ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel geometry_msgs/msg/TwistStamped " twist: linear: x: 0.7 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 1.0"
- 现在你应该能在 RViz 中看到一个橙色方框在盘旋。此外，在启动文件的终端中也会看到状态的变化。
[DiffBotSystemHardware]: Got command 43.33333 for 'left_wheel_joint'! [DiffBotSystemHardware]: Got command 50.00000 for 'right_wheel_joint'!
- 让我们反观一下 ros2_control 硬件组件。调用
ros2 control list_hardware_components
- 应该给你
Hardware Component 1 name: DiffBot type: system plugin name: ros2_control_demo_example_2/DiffBotSystemHardware state: id=3 label=active command interfaces left_wheel_joint/velocity [available] [claimed] right_wheel_joint/velocity [available] [claimed]
- 这表明自定义硬件接口插件已加载并运行。如果您使用的是真实机器人，但没有运行模拟器，通常使用 mock_components/GenericSystem 硬件组件比编写自定义组件更快。停止启动文件，然后使用附加参数重新启动
ros2 launch ros2_control_demo_example_2 diffbot.launch.py use_mock_hardware:=True
- 呼叫
ros2 control list_hardware_components
- 现在应该给你
Hardware Component 1 name: DiffBot type: system plugin name: mock_components/GenericSystem state: id=3 label=active command interfaces left_wheel_joint/velocity [available] [claimed] right_wheel_joint/velocity [available] [claimed]
- 你会发现加载了一个不同的插件。在 diffbot.ros2_control.xacro 中，可以找到加载该插件和参数 calculate_dynamics 的说明。
<hardware> <plugin>mock_components/GenericSystem</plugin> <param name="calculate_dynamics">true</param> </hardware>
- 这样就能将速度指令整合到位置状态界面，并通过 ros2 topic echo /joint_states 进行检查： 如果机器人在移动，位置值会随时间增加。现在，您可以使用上述命令对设置进行测试，其工作原理应与自定义硬件组件插件相同。
有关 mock_components 的更多信息，请参阅 ros2_control 文档。
3.2 本演示使用的文件
- 启动文件： diffbot.launch.py
- 控制器 yaml： diffbot_controllers.yaml
- URDF 文件： diffbot.urdf.xacro
- 描述： diffbot_description.urdf.xacro
- ros2_control 标签： diffbot.ros2_control.xacro
- RViz 配置：diffbot.rviz
- 硬件接口插件： diffbot_system.cpp
3.3 本演示中的控制器
- Joint State Broadcaster (ros2_controllers repository): doc
- Diff Drive Controller (ros2_controllers repository): doc
**Describe the problems** Hi, I'm a student and I'm trying to use diff_drive_controller with ros2_canopen, I followed the tutorials about ros2_canopen and ros2_control, I referenced the config files the sample program from github. Problem: When I run the program, ros2 diff_drive_controller and ros2_canoepn work fine respectively. When I call ```bash ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel geometry_msgs/msg/TwistStamped " twist: linear: x: 0.7 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 1.0" ``` the robot in rviz turned normally, but wheels that ros2_canopen drives are not turning. Some of the files are not configured correctly, but I don't know how to configure them correctly. the terminal output is ```bash [ros2_control_node-3] [WARN] [1720421078.777601620] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/position' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777606030] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/velocity' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777608982] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/position' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777611514] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/velocity' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777615605] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/position' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777618048] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/velocity' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777620589] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/position' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777622945] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/velocity' command interface already in available list. This can happen due to multiple calls to 'configure' ``` Here are my config files. diffbot.urdf.xacro ```xml <?xml version="1.0"?> <robot xmlns:xacro="http://wiki.ros.org/xacro" name="test_robot"> <xacro:include filename="$(find canopen_tests)/urdf/robot_controller/diffbot_description.urdf.xacro"/> <xacro:include filename="$(find canopen_tests)/urdf/robot_controller/diffbot.ros2_control.xacro"/> <!-- Import Rviz colors --> <xacro:include filename="$(find ros2_control_demo_description)/diffbot/urdf/diffbot.materials.xacro" /> <xacro:diffbot prefix=""> </xacro:diffbot> <xacro:diffbot_ros2_control name="diff_robot" prefix="" bus_config="$(find canopen_tests)/config/robot_control/bus.yml" master_config="$(find canopen_tests)/config/robot_control/master.dcf" can_interface_name="vcan0" master_bin="" /> </robot> ``` diffbot.ros2_control.xacro ```xml <?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <xacro:macro name="diffbot_ros2_control" params=" name prefix bus_config master_config can_interface_name master_bin "> <ros2_control name="${name}" type="system"> <hardware> <plugin>canopen_ros2_control/RobotSystem</plugin> <param name="bus_config">${bus_config}</param> <param name="master_config">${master_config}</param> <param name="can_interface_name">${can_interface_name}</param> <param name="master_bin">"${master_bin}"</param> </hardware> <joint name="left_wheel_joint"> <param name="device_name">left_wheel_joint</param> <command_interface name="velocity"/> <!-- <command_interface name="position"/> --> <state_interface name="position"/> <state_interface name="velocity"/> </joint> <joint name="right_wheel_joint"> <param name="device_name">right_wheel_joint</param> <command_interface name="velocity"/> <!-- <command_interface name="position"/> --> <state_interface name="position"/> <state_interface name="velocity"/> </joint> </ros2_control> </xacro:macro> </robot> ``` diffbot_controllers.yaml ```yaml controller_manager: ros__parameters: update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster robot_controller: type: canopen_ros2_controllers/Cia402RobotController diffbot_base_controller: type: diff_drive_controller/DiffDriveController robot_controller: ros__parameters: joints: - left_wheel_joint - right_wheel_joint command_interfaces: - velocity state_interfaces: - position - velocity operation_mode: 1 command_poll_freq: 5 diffbot_base_controller: ros__parameters: left_wheel_names: ["left_wheel_joint"] right_wheel_names: ["right_wheel_joint"] wheel_separation: 0.10 #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal wheel_radius: 0.015 wheel_separation_multiplier: 1.0 left_wheel_radius_multiplier: 1.0 right_wheel_radius_multiplier: 1.0 publish_rate: 50.0 odom_frame_id: odom base_frame_id: base_link pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] open_loop: true enable_odom_tf: true cmd_vel_timeout: 0.5 #publish_limited_velocity: true #velocity_rolling_window_size: 10 # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* linear.x.has_velocity_limits: true linear.x.has_acceleration_limits: true linear.x.has_jerk_limits: false linear.x.max_velocity: 1.0 linear.x.min_velocity: -1.0 linear.x.max_acceleration: 1.0 linear.x.max_jerk: 0.0 linear.x.min_jerk: 0.0 angular.z.has_velocity_limits: true angular.z.has_acceleration_limits: true angular.z.has_jerk_limits: false angular.z.max_velocity: 1.0 angular.z.min_velocity: -1.0 angular.z.max_acceleration: 1.0 angular.z.min_acceleration: -1.0 angular.z.max_jerk: 0.0 angular.z.min_jerk: 0.0 ``` bus.yml ```yml options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 left_wheel_joint: node_id: 2 dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 position_mode: 1 velocity_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 #- {index: 0x60FF, sub_index: 0} # target velocity right_wheel_joint: node_id: 3 dcf: "cia402_slave.eds" driver: "ros2_canopen::Cia402Driver" package: "canopen_402_driver" period: 10 position_mode: 1 velocity_mode: 1 revision_number: 0 switching_state: 2 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 - {index: 0x60FF, sub_index: 0} # target velocity ``` The full output is: ```bash ros2 launch canopen_tests diffbot.launch.py [INFO] [launch]: All log files can be found below /home/kuanli/.ros/log/2024-07-08-14-44-38-141319-kuanli-7021 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [cia402_slave_node-1]: process started with pid [7053] [INFO] [cia402_slave_node-2]: process started with pid [7055] [INFO] [ros2_control_node-3]: process started with pid [7057] [INFO] [spawner-4]: process started with pid [7059] [INFO] [spawner-5]: process started with pid [7061] [INFO] [robot_state_publisher-6]: process started with pid [7063] [INFO] [rviz2-7]: process started with pid [7065] [robot_state_publisher-6] [WARN] [1720421078.515981679] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. [robot_state_publisher-6] [INFO] [1720421078.516036992] [robot_state_publisher]: got segment base_link [robot_state_publisher-6] [INFO] [1720421078.516079619] [robot_state_publisher]: got segment caster_frontal_wheel [robot_state_publisher-6] [INFO] [1720421078.516082595] [robot_state_publisher]: got segment caster_rear_wheel [robot_state_publisher-6] [INFO] [1720421078.516084581] [robot_state_publisher]: got segment left_wheel [robot_state_publisher-6] [INFO] [1720421078.516086471] [robot_state_publisher]: got segment right_wheel [ros2_control_node-3] [WARN] [1720421078.516945401] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead. [ros2_control_node-3] [INFO] [1720421078.517125003] [resource_manager]: Loading hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.523860573] [resource_manager]: Initialize hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.523997283] [diff_robot_interface]: Registering hardware interface 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.524022573] [diff_robot_interface]: 'diff_robot' has bus config: '/home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/bus.yml' [ros2_control_node-3] [INFO] [1720421078.524024979] [diff_robot_interface]: 'diff_robot' has master config: '/home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/master.dcf' [ros2_control_node-3] [INFO] [1720421078.524029216] [diff_robot_interface]: 'diff_robot' has master bin: '' [ros2_control_node-3] [INFO] [1720421078.524030892] [diff_robot_interface]: 'diff_robot' has can interface: 'vcan0' [ros2_control_node-3] [ERROR] [1720421078.525094754] [left_wheel_joint]: Node id for 'left_wheel_joint' is '2' [ros2_control_node-3] [INFO] [1720421078.525130440] [left_wheel_joint]: Registered position_mode '1' for 'left_wheel_joint' [ros2_control_node-3] [INFO] [1720421078.525162260] [left_wheel_joint]: Registered velocity_mode '1' for 'left_wheel_joint' [ros2_control_node-3] [ERROR] [1720421078.525570611] [right_wheel_joint]: Node id for 'right_wheel_joint' is '3' [ros2_control_node-3] [INFO] [1720421078.525577301] [right_wheel_joint]: Registered position_mode '1' for 'right_wheel_joint' [ros2_control_node-3] [INFO] [1720421078.525581722] [right_wheel_joint]: Registered velocity_mode '1' for 'right_wheel_joint' [ros2_control_node-3] [INFO] [1720421078.525651179] [resource_manager]: Successful initialization of hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.525771950] [resource_manager]: 'configure' hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.529888527] [device_container]: Starting Device Container with: [ros2_control_node-3] [INFO] [1720421078.529925769] [device_container]: master_config /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/master.dcf [ros2_control_node-3] [INFO] [1720421078.529935527] [device_container]: bus_config /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/bus.yml [ros2_control_node-3] [INFO] [1720421078.530795810] [device_container]: Loading Master Configuration. [ros2_control_node-3] [INFO] [1720421078.531368077] [device_container]: Load Library: /home/kuanli/Documents/ros2_ws/install/canopen_master_driver/lib/libmaster_driver.so [ros2_control_node-3] [INFO] [1720421078.533783549] [device_container]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver> [ros2_control_node-3] [INFO] [1720421078.533816542] [device_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver> [ros2_control_node-3] [INFO] [1720421078.538432749] [master]: NodeCanopenBasicMaster [ros2_control_node-3] [INFO] [1720421078.538519270] [device_container]: Load master component. [ros2_control_node-3] [INFO] [1720421078.538620631] [device_container]: Added /master to executor [ros2_control_node-3] NMT: entering reset application state [ros2_control_node-3] NMT: entering reset communication state [ros2_control_node-3] NMT: running as master [ros2_control_node-3] NMT: entering pre-operational state [ros2_control_node-3] NMT: entering operational state [ros2_control_node-3] [INFO] [1720421078.551353316] [device_container]: Loading Driver Configuration. [ros2_control_node-3] [INFO] [1720421078.551432188] [device_container]: Found device left_wheel_joint with driver ros2_canopen::Cia402Driver [ros2_control_node-3] [INFO] [1720421078.551678700] [device_container]: Load Library: /home/kuanli/Documents/ros2_ws/install/canopen_402_driver/lib/libcia402_driver.so [ros2_control_node-3] [INFO] [1720421078.552289006] [device_container]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver> [ros2_control_node-3] [INFO] [1720421078.552298780] [device_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver> [ros2_control_node-3] [INFO] [1720421078.555252795] [device_container]: Load driver component. [ros2_control_node-3] [INFO] [1720421078.555376734] [device_container]: Added /left_wheel_joint to executor [ros2_control_node-3] [ERROR] [1720421078.563945016] [left_wheel_joint]: Could not polling from config, setting to true. [ros2_control_node-3] [INFO] [1720421078.564131312] [left_wheel_joint]: scale_pos_to_dev_ 1000.000000 [ros2_control_node-3] scale_pos_from_dev_ 0.001000 [ros2_control_node-3] scale_vel_to_dev_ 1000.000000 [ros2_control_node-3] scale_vel_from_dev_ 0.001000 [ros2_control_node-3] [ros2_control_node-3] [INFO] [1720421078.565538417] [left_wheel_joint]: eds file /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/cia402_slave.eds [ros2_control_node-3] [INFO] [1720421078.565557010] [left_wheel_joint]: bin file /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/left_wheel_joint.bin [ros2_control_node-3] 2006: warning: DefaultValue underflow [ros2_control_node-3] 2006: warning: ParameterValue underflow [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6060 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=607a subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6060 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=607a subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=60ff subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6061 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6064 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=606c subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6064 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=606c subindex=0 [ros2_control_node-3] [WARN] [1720421078.570551288] [left_wheel_joint]: Wait for device to boot. [cia402_slave_node-1] [INFO] [1720421078.723263601] [slave_node_2]: Reaching inactive state. [cia402_slave_node-2] [INFO] [1720421078.724071424] [slave_node_1]: Reaching inactive state. [INFO] [launch.user]: node 'basic_slave_node' reached the 'inactive' state, 'activating'. [INFO] [launch.user]: node 'basic_slave_node' reached the 'inactive' state, 'activating'. [cia402_slave_node-1] [INFO] [1720421078.734231849] [slave_node_2]: Reaching active state. [cia402_slave_node-2] [INFO] [1720421078.735171494] [slave_node_1]: Reaching active state. [cia402_slave_node-1] 2006: warning: DefaultValue underflow [cia402_slave_node-2] 2006: warning: DefaultValue underflow [cia402_slave_node-2] 2006: warning: ParameterValue underflow [cia402_slave_node-2] NMT: entering reset application state [cia402_slave_node-2] NMT: entering reset communication state [cia402_slave_node-2] NMT: running as slave [cia402_slave_node-1] 2006: warning: ParameterValue underflow [cia402_slave_node-2] NMT: entering pre-operational state [cia402_slave_node-2] NMT: entering operational state [cia402_slave_node-2] [INFO] [1720421078.746887553] [slave_node_1]: Created cia402 slave for node_id 2. [cia402_slave_node-1] NMT: entering reset application state [cia402_slave_node-1] NMT: entering reset communication state [cia402_slave_node-1] NMT: running as slave [cia402_slave_node-1] NMT: entering pre-operational state [cia402_slave_node-1] NMT: entering operational state [ros2_control_node-3] [INFO] [1720421078.748926127] [left_wheel_joint]: Driver booted and ready. [ros2_control_node-3] [INFO] [1720421078.749213120] [left_wheel_joint]: Starting with polling mode. [cia402_slave_node-1] [INFO] [1720421078.749425738] [slave_node_2]: Created cia402 slave for node_id 3. [ros2_control_node-3] [INFO] [1720421078.749735208] [device_container]: Found device right_wheel_joint with driver ros2_canopen::Cia402Driver [ros2_control_node-3] [INFO] [1720421078.750099665] [device_container]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver> [ros2_control_node-3] [INFO] [1720421078.750106124] [device_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver> [ros2_control_node-3] [INFO] [1720421078.759850855] [canopen_402_driver]: Fault reset [cia402_slave_node-2] [INFO] [1720421078.760230937] [cia402_slave]: Switch_On_Disabled [ros2_control_node-3] [INFO] [1720421078.762806326] [device_container]: Load driver component. [ros2_control_node-3] [INFO] [1720421078.763242229] [device_container]: Added /right_wheel_joint to executor [ros2_control_node-3] [ERROR] [1720421078.772435805] [right_wheel_joint]: Could not polling from config, setting to true. [ros2_control_node-3] [INFO] [1720421078.772597169] [right_wheel_joint]: scale_pos_to_dev_ 1000.000000 [ros2_control_node-3] scale_pos_from_dev_ 0.001000 [ros2_control_node-3] scale_vel_to_dev_ 1000.000000 [ros2_control_node-3] scale_vel_from_dev_ 0.001000 [ros2_control_node-3] [ros2_control_node-3] [INFO] [1720421078.773842007] [right_wheel_joint]: eds file /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/cia402_slave.eds [ros2_control_node-3] [INFO] [1720421078.773858938] [right_wheel_joint]: bin file /home/kuanli/Documents/ros2_ws/install/canopen_tests/share/canopen_tests/config/robot_control/right_wheel_joint.bin [ros2_control_node-3] 2006: warning: DefaultValue underflow [ros2_control_node-3] 2006: warning: ParameterValue underflow [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6060 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=607a subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=60ff subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=607a subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=6040 subindex=0 [ros2_control_node-3] Found rpdo mapped object: index=60ff subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6061 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6064 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=606c subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6064 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=6041 subindex=0 [ros2_control_node-3] Found tpdo mapped object: index=606c subindex=0 [ros2_control_node-3] [INFO] [1720421078.777203370] [right_wheel_joint]: Driver booted and ready. [ros2_control_node-3] [INFO] [1720421078.777287134] [right_wheel_joint]: Starting with polling mode. [ros2_control_node-3] [INFO] [1720421078.777506877] [device_container]: Initialisation successful. [ros2_control_node-3] [INFO] [1720421078.777587930] [resource_manager]: Successful 'configure' of hardware 'diff_robot' [ros2_control_node-3] [WARN] [1720421078.777601620] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/position' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777606030] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/velocity' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777608982] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/position' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777611514] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/velocity' state interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777615605] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/position' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777618048] [resource_manager]: (hardware 'diff_robot'): 'left_wheel_joint/velocity' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777620589] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/position' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [WARN] [1720421078.777622945] [resource_manager]: (hardware 'diff_robot'): 'right_wheel_joint/velocity' command interface already in available list. This can happen due to multiple calls to 'configure' [ros2_control_node-3] [INFO] [1720421078.777625780] [resource_manager]: 'activate' hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421078.778997673] [canopen_402_driver]: Init: Read State [ros2_control_node-3] [INFO] [1720421078.779026762] [canopen_402_driver]: Init: Enable [ros2_control_node-3] [INFO] [1720421078.779560895] [canopen_402_driver]: Fault reset [cia402_slave_node-2] [INFO] [1720421078.779644285] [cia402_slave]: Received Shutdown. [cia402_slave_node-2] [INFO] [1720421078.779657992] [cia402_slave]: Ready_To_Switch_On [ros2_control_node-3] [INFO] [1720421078.787483012] [canopen_402_driver]: Fault reset [cia402_slave_node-1] [INFO] [1720421078.787592309] [cia402_slave]: Switch_On_Disabled [cia402_slave_node-2] [INFO] [1720421078.809597351] [cia402_slave]: Received Switch On. [cia402_slave_node-2] [INFO] [1720421078.809621062] [cia402_slave]: Switched_On [cia402_slave_node-2] [INFO] [1720421078.839661748] [cia402_slave]: Received Enable Operation. [cia402_slave_node-2] [INFO] [1720421078.839676134] [cia402_slave]: Operation_Enable [ros2_control_node-3] [INFO] [1720421078.859793683] [canopen_402_driver]: Init: Switch to homing [ros2_control_node-3] [INFO] [1720421078.879658221] [canopen_402_driver]: Init: Execute homing [ros2_control_node-3] [INFO] [1720421078.880225219] [canopen_402_driver]: Init: Switch no mode [ros2_control_node-3] [INFO] [1720421078.881336093] [canopen_402_driver]: Init: Read State [ros2_control_node-3] [INFO] [1720421078.881355630] [canopen_402_driver]: Init: Enable [ros2_control_node-3] [INFO] [1720421078.881416542] [canopen_402_driver]: Init: Switch to homing [ros2_control_node-3] [INFO] [1720421078.881502445] [canopen_402_driver]: Init: Execute homing [ros2_control_node-3] [INFO] [1720421078.881568367] [canopen_402_driver]: Init: Switch no mode [ros2_control_node-3] [INFO] [1720421078.882088325] [canopen_402_driver]: Init: Read State [ros2_control_node-3] [INFO] [1720421078.882104114] [canopen_402_driver]: Init: Enable [ros2_control_node-3] [INFO] [1720421078.887439499] [canopen_402_driver]: Fault reset [cia402_slave_node-1] [INFO] [1720421078.887522517] [cia402_slave]: Received Shutdown. [cia402_slave_node-1] [INFO] [1720421078.887535851] [cia402_slave]: Ready_To_Switch_On [ros2_control_node-3] [INFO] [1720421078.889560728] [canopen_402_driver]: Fault reset [cia402_slave_node-2] [INFO] [1720421078.889673401] [cia402_slave]: Received Disable Voltage. [cia402_slave_node-2] [INFO] [1720421078.889690702] [cia402_slave]: Switch_On_Disabled [cia402_slave_node-1] [INFO] [1720421078.917609643] [cia402_slave]: Received Switch On. [cia402_slave_node-1] [INFO] [1720421078.917629041] [cia402_slave]: Switched_On [cia402_slave_node-1] [INFO] [1720421078.947553738] [cia402_slave]: Received Enable Operation. [cia402_slave_node-1] [INFO] [1720421078.947570893] [cia402_slave]: Operation_Enable [ros2_control_node-3] [INFO] [1720421078.967711891] [canopen_402_driver]: Init: Switch to homing [cia402_slave_node-1] [INFO] [1720421078.977586979] [cia402_slave]: Received Disable Voltage. [cia402_slave_node-1] [INFO] [1720421078.977606985] [cia402_slave]: Switch_On_Disabled [cia402_slave_node-1] [INFO] [1720421079.027645060] [cia402_slave]: Received Shutdown. [cia402_slave_node-1] [INFO] [1720421079.027670667] [cia402_slave]: Ready_To_Switch_On [rviz2-7] [INFO] [1720421079.041031410] [rviz2]: Stereo is NOT SUPPORTED [rviz2-7] [INFO] [1720421079.041136329] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [cia402_slave_node-1] [INFO] [1720421079.057551074] [cia402_slave]: Received Switch On. [cia402_slave_node-1] [INFO] [1720421079.057565374] [cia402_slave]: Switched_On [cia402_slave_node-1] [INFO] [1720421079.087509286] [cia402_slave]: Received Enable Operation. [cia402_slave_node-1] [INFO] [1720421079.087524085] [cia402_slave]: Operation_Enable [ros2_control_node-3] [INFO] [1720421079.107559454] [canopen_402_driver]: Init: Execute homing [ros2_control_node-3] [INFO] [1720421079.107797343] [canopen_402_driver]: Init: Switch no mode [ros2_control_node-3] [INFO] [1720421079.108554086] [canopen_402_driver]: Init: Read State [ros2_control_node-3] [INFO] [1720421079.108571094] [canopen_402_driver]: Init: Enable [ros2_control_node-3] [INFO] [1720421079.108624482] [canopen_402_driver]: Init: Switch to homing [ros2_control_node-3] [INFO] [1720421079.117504450] [canopen_402_driver]: Fault reset [cia402_slave_node-1] [INFO] [1720421079.117678377] [cia402_slave]: Received Disable Voltage. [cia402_slave_node-1] [INFO] [1720421079.117690610] [cia402_slave]: Switch_On_Disabled [rviz2-7] [INFO] [1720421079.121077594] [rviz2]: Stereo is NOT SUPPORTED [cia402_slave_node-1] [INFO] [1720421079.167777331] [cia402_slave]: Received Shutdown. [cia402_slave_node-1] [INFO] [1720421079.167791409] [cia402_slave]: Ready_To_Switch_On [cia402_slave_node-1] [INFO] [1720421079.197510895] [cia402_slave]: Received Switch On. [cia402_slave_node-1] [INFO] [1720421079.197524812] [cia402_slave]: Switched_On [cia402_slave_node-1] [INFO] [1720421079.227702994] [cia402_slave]: Received Enable Operation. [cia402_slave_node-1] [INFO] [1720421079.227723871] [cia402_slave]: Operation_Enable [ros2_control_node-3] [INFO] [1720421079.247622060] [canopen_402_driver]: Init: Execute homing [ros2_control_node-3] [INFO] [1720421079.247938692] [canopen_402_driver]: Init: Switch no mode [ros2_control_node-3] [INFO] [1720421079.247995935] [resource_manager]: Successful 'activate' of hardware 'diff_robot' [ros2_control_node-3] [INFO] [1720421079.251798686] [controller_manager]: update rate is 100 Hz [ros2_control_node-3] [INFO] [1720421079.251877772] [controller_manager]: RT kernel is recommended for better performance [ros2_control_node-3] [INFO] [1720421079.353826009] [controller_manager]: Loading controller 'joint_state_broadcaster' [spawner-4] [INFO] [1720421079.362552094] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-3] [INFO] [1720421079.363306236] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-3] [INFO] [1720421079.363369393] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [spawner-4] [INFO] [1720421079.392615212] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [INFO] [spawner-4]: process has finished cleanly [pid 7059] [ros2_control_node-3] [INFO] [1720421079.598626332] [controller_manager]: Loading controller 'diffbot_base_controller' [spawner-5] [INFO] [1720421079.612869785] [spawner_diffbot_base_controller]: Loaded diffbot_base_controller [ros2_control_node-3] [INFO] [1720421079.613690004] [controller_manager]: Configuring controller 'diffbot_base_controller' [ros2_control_node-3] [INFO] [1720421079.632086407] [left_wheel_joint]: Switching to 'left_wheel_joint/velocity' command mode with CIA402 operation mode '1' [cia402_slave_node-2] [INFO] [1720421079.639705436] [cia402_slave]: Received Shutdown. [cia402_slave_node-2] [INFO] [1720421079.639721550] [cia402_slave]: Ready_To_Switch_On [cia402_slave_node-2] [INFO] [1720421079.669576794] [cia402_slave]: Received Switch On. [cia402_slave_node-2] [INFO] [1720421079.669591158] [cia402_slave]: Switched_On [cia402_slave_node-2] [INFO] [1720421079.699584510] [cia402_slave]: Received Enable Operation. [cia402_slave_node-2] [INFO] [1720421079.699598646] [cia402_slave]: Operation_Enable [cia402_slave_node-2] [INFO] [1720421079.729752321] [cia402_slave]: run_profiled_position_mode [cia402_slave_node-2] [INFO] [1720421079.729784218] [cia402_slave]: Profile_Speed 1.000000, Profile Acceleration: 2.000000 [ros2_control_node-3] [INFO] [1720421079.739699013] [right_wheel_joint]: Switching to 'right_wheel_joint/velocity' command mode with CIA402 operation mode '1' [cia402_slave_node-1] [INFO] [1720421079.747707511] [cia402_slave]: Received Disable Voltage. [cia402_slave_node-1] [INFO] [1720421079.747719421] [cia402_slave]: Switch_On_Disabled [cia402_slave_node-1] [INFO] [1720421079.797586817] [cia402_slave]: Received Shutdown. [cia402_slave_node-1] [INFO] [1720421079.797602523] [cia402_slave]: Ready_To_Switch_On [cia402_slave_node-1] [INFO] [1720421079.827537657] [cia402_slave]: Received Switch On. [cia402_slave_node-1] [INFO] [1720421079.827553510] [cia402_slave]: Switched_On [cia402_slave_node-1] [INFO] [1720421079.857609405] [cia402_slave]: Received Enable Operation. [cia402_slave_node-1] [INFO] [1720421079.857626791] [cia402_slave]: Operation_Enable [cia402_slave_node-1] [INFO] [1720421079.857785674] [cia402_slave]: run_profiled_position_mode [cia402_slave_node-1] [INFO] [1720421079.857805683] [cia402_slave]: Profile_Speed 1.000000, Profile Acceleration: 2.000000 [spawner-5] [INFO] [1720421079.878384262] [spawner_diffbot_base_controller]: Configured and activated diffbot_base_controller [INFO] [spawner-5]: process has finished cleanly [pid 7061] ``` The topics are: ```bash ros2 topic list /clicked_point /diagnostics /diffbot_base_controller/cmd_vel /diffbot_base_controller/odom /diffbot_base_controller/transition_event /dynamic_joint_states /goal_pose /initialpose /joint_state_broadcaster/transition_event /joint_states /left_wheel_joint/joint_states /left_wheel_joint/nmt_state /left_wheel_joint/rpdo /left_wheel_joint/tpdo /parameter_events /right_wheel_joint/joint_states /right_wheel_joint/nmt_state /right_wheel_joint/rpdo /right_wheel_joint/tpdo /robot_description /rosout /slave_node_1/transition_event /slave_node_2/transition_event /tf /tf_static ``` The services are: ```bash ros2 service list /controller_manager/configure_controller /controller_manager/describe_parameters /controller_manager/get_parameter_types /controller_manager/get_parameters /controller_manager/get_type_description /controller_manager/list_controller_types /controller_manager/list_controllers /controller_manager/list_hardware_components /controller_manager/list_hardware_interfaces /controller_manager/list_parameters /controller_manager/load_controller /controller_manager/reload_controller_libraries /controller_manager/set_hardware_component_state /controller_manager/set_parameters /controller_manager/set_parameters_atomically /controller_manager/switch_controller /controller_manager/unload_controller /device_container/describe_parameters /device_container/get_parameter_types /device_container/get_parameters /device_container/get_type_description /device_container/init_driver /device_container/list_parameters /device_container/set_parameters /device_container/set_parameters_atomically /diffbot_base_controller/describe_parameters /diffbot_base_controller/get_parameter_types /diffbot_base_controller/get_parameters /diffbot_base_controller/get_type_description /diffbot_base_controller/list_parameters /diffbot_base_controller/set_parameters /diffbot_base_controller/set_parameters_atomically /joint_state_broadcaster/describe_parameters /joint_state_broadcaster/get_parameter_types /joint_state_broadcaster/get_parameters /joint_state_broadcaster/get_type_description /joint_state_broadcaster/list_parameters /joint_state_broadcaster/set_parameters /joint_state_broadcaster/set_parameters_atomically /launch_ros_7021/describe_parameters /launch_ros_7021/get_parameter_types /launch_ros_7021/get_parameters /launch_ros_7021/get_type_description /launch_ros_7021/list_parameters /launch_ros_7021/set_parameters /launch_ros_7021/set_parameters_atomically /left_wheel_joint/cyclic_position_mode /left_wheel_joint/cyclic_velocity_mode /left_wheel_joint/describe_parameters /left_wheel_joint/get_parameter_types /left_wheel_joint/get_parameters /left_wheel_joint/get_type_description /left_wheel_joint/halt /left_wheel_joint/init /left_wheel_joint/interpolated_position_mode /left_wheel_joint/list_parameters /left_wheel_joint/nmt_reset_node /left_wheel_joint/nmt_start_node /left_wheel_joint/position_mode /left_wheel_joint/recover /left_wheel_joint/sdo_read /left_wheel_joint/sdo_write /left_wheel_joint/set_parameters /left_wheel_joint/set_parameters_atomically /left_wheel_joint/target /left_wheel_joint/torque_mode /left_wheel_joint/velocity_mode /master/describe_parameters /master/get_parameter_types /master/get_parameters /master/get_type_description /master/list_parameters /master/sdo_read /master/sdo_write /master/set_parameters /master/set_parameters_atomically /right_wheel_joint/cyclic_position_mode /right_wheel_joint/cyclic_velocity_mode /right_wheel_joint/describe_parameters /right_wheel_joint/get_parameter_types /right_wheel_joint/get_parameters /right_wheel_joint/get_type_description /right_wheel_joint/halt /right_wheel_joint/init /right_wheel_joint/interpolated_position_mode /right_wheel_joint/list_parameters /right_wheel_joint/nmt_reset_node /right_wheel_joint/nmt_start_node /right_wheel_joint/position_mode /right_wheel_joint/recover /right_wheel_joint/sdo_read /right_wheel_joint/sdo_write /right_wheel_joint/set_parameters /right_wheel_joint/set_parameters_atomically /right_wheel_joint/target /right_wheel_joint/torque_mode /right_wheel_joint/velocity_mode /robot_state_publisher/describe_parameters /robot_state_publisher/get_parameter_types /robot_state_publisher/get_parameters /robot_state_publisher/get_type_description /robot_state_publisher/list_parameters /robot_state_publisher/set_parameters /robot_state_publisher/set_parameters_atomically /rviz2/describe_parameters /rviz2/get_parameter_types /rviz2/get_parameters /rviz2/get_type_description /rviz2/list_parameters /rviz2/set_parameters /rviz2/set_parameters_atomically /slave_node_1/change_state /slave_node_1/describe_parameters /slave_node_1/get_available_states /slave_node_1/get_available_transitions /slave_node_1/get_parameter_types /slave_node_1/get_parameters /slave_node_1/get_state /slave_node_1/get_transition_graph /slave_node_1/get_type_description /slave_node_1/list_parameters /slave_node_1/set_parameters /slave_node_1/set_parameters_atomically /slave_node_2/change_state /slave_node_2/describe_parameters /slave_node_2/get_available_states /slave_node_2/get_available_transitions /slave_node_2/get_parameter_types /slave_node_2/get_parameters /slave_node_2/get_state /slave_node_2/get_transition_graph /slave_node_2/get_type_description /slave_node_2/list_parameters /slave_node_2/set_parameters /slave_node_2/set_parameters_atomically /transform_listener_impl_5e41e8bf69c0/get_type_description ``` **Environment :** - OS: Ubuntu 22.04 - Version iron I tried many times but I can not config it correctly. Could you please help me?
