search

ros-controls / ros2_controllers

Generic robotic controllers to accompany ros2_control
https://control.ros.org
Apache License 2.0
347 stars 312 forks source link

Some problems with config files when I use ros2_canopen in combination with diff_drive_controller #1197

Closed 15906185391 closed 1 month ago

15906185391 commented 2 months ago

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 

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 

[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 list_hardware_components 
Hardware Component 1
    name: diff_robot
    type: system
    plugin name: canopen_ros2_control/RobotSystem
    state: id=3 label=active
    command interfaces
        left_wheel_joint/position [available] [unclaimed]
        left_wheel_joint/velocity [available] [claimed]
        left_wheel_joint/position [available] [unclaimed]
        left_wheel_joint/velocity [available] [claimed]
        right_wheel_joint/position [available] [unclaimed]
        right_wheel_joint/velocity [available] [claimed]
        right_wheel_joint/position [available] [unclaimed]
        right_wheel_joint/velocity [available] [claimed]

Here are my config files.

diffbot.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,RegisterEventHandler
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    declared_arguments = []
    declared_arguments.append(
        DeclareLaunchArgument(
            "gui",
            default_value="true",
            description="Start RViz2 automatically with this launch file.",
        )
    )

    declared_arguments.append(
        DeclareLaunchArgument(
            "use_mock_hardware",
            default_value="false",
            description="Start robot with mock hardware mirroring command to its states.",
        )
    )

    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [
                    FindPackageShare("canopen_tests"),
                    "urdf/robot_controller",
                    "diffbot.urdf.xacro",
                ]
            ),
        ]
    )
    robot_description = {"robot_description": robot_description_content}
    robot_control_config = PathJoinSubstitution(
        [FindPackageShare("canopen_tests"), "config/robot_control", "diffbot_controllers.yaml"]
    )

    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare("canopen_tests"), "rviz", "diffbot.rviz"]
    )

    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"],
    )

    robot_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diffbot_base_controller", "--controller-manager", "/controller_manager"],
    )

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
        remappings=[
            ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
        ],
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
    )

    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 = [
        slave_node_2,
        slave_node_1,
        control_node,
        joint_state_broadcaster_spawner,
        robot_controller_spawner,
        robot_state_publisher_node,
        rviz_node,
    ]

    return LaunchDescription(nodes_to_start)

diffbot.urdf.xacro

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

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

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:

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:

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:

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 :

I tried many times but I can not config it correctly. Could you please help me?

christophfroehlich commented 2 months ago

I don't know ros2_canopen, but what I see now:

15906185391 commented 2 months ago

I don't know ros2_canopen, but what I see now:

  • a remapping on the wrong place (remapping of cmd_vel for robot_state_publisher, but there is no such a topic there).
  • you configure two controllers for the same interfaces? What is robot_controller for? maybe ros2_canopen does not work with diff_drive controller if it needs special interfaces.

Thanks for the reply!

I followed the tutorial: https://ros-industrial.github.io/ros2_canopen/manual/rolling/user-guide/how-to-create-a-robot-system.html

Maybe I configure two controllers for the same interfaces. I commented out robot_controller and still have the same problem. ros2_canopen is configured with state_interface and command_interface, and diff_controller should have been configured once as well, and I'm trying to suppress one of the controllers for state and command interface configurations.

christophfroehlich commented 1 month ago

Sorry, I have no experience with ros2_canopen. Maybe you should ask there, and link this issue here for reference.

15906185391 commented 1 month ago

Sorry, I have no experience with ros2_canopen. Maybe you should ask there, and link this issue here for reference.

Thanks for your reply! This is a ros2_canopen problem, and I have solved this issue. Thank you!