search

ros-industrial / ros2_canopen

CANopen driver framework for ROS2
https://ros-industrial.github.io/ros2_canopen/manual/rolling/
143 stars 62 forks source link

Kinco device stuck at "Wait for devie to boot" when using real CAN Bus #271

Closed glaciercoder closed 6 months ago

glaciercoder commented 6 months ago

Describe the bug I'm using Kinco driver with ros2 canopen robot controller, when I launch the launch file, the process stuck in "Wait for device to boot" and I see no can messages using candump. I've made sure my can interface works fine.

Bus config:

options:
  dcf_path: "@BUS_CONFIG_PATH@"

master:
  node_id: 255
  driver: "ros2_canopen::MasterDriver"
  package: "canopen_master_driver"
  baud_rate: 500
  heartbeat_consumer: false
  heartbeat_producer: 2000
  start_nodes: true
  sync_period: 5000000 

defaults:
  dcf: "kinco.eds"
  driver: "ros2_canopen::Cia402Driver"
  package: "canopen_402_driver"
  period: 20
  velocity_mode: 1
  revision_number: 2
  polling: false
  heartbeat_producer: 1000
  sdo:
    - {index: 0x6060, sub_index: 0, value: 3} # Working mode
  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
    2:
      enabled: true
      cob_id: "auto"
      transmission: 0x01
      mapping:
        - {index: 0x606C, sub_index: 0} # position actual value
  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: 0x60FF, sub_index: 0} # target velocity

nodes:
    wheel0_driving:
      node_id: 0x11
    wheel1_driving:
      node_id: 0x12
    wheel2_driving:
      node_id: 0x13
    wheel3_driving:
      node_id: 0x14
    wheel4_driving:
      node_id: 0x15
    wheel5_driving:
      node_id: 0x16

controller config:

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_velocity_controller:
      type: forward_command_controller/ForwardCommandController

forward_position_controller:
  ros__parameters:
    joints:
      - wheel0_steering
      - wheel1_steering
      - wheel2_steering
      - wheel3_steering
      - wheel4_steering
      - wheel5_steering
    interface_name: position 

forward_velocity_controller:
  ros__parameters:
    joints:
      - wheel0_driving
      - wheel1_driving
      - wheel2_driving
      - wheel3_driving
      - wheel4_driving
      - wheel5_driving
    interface_name: velocity

Launch file

def generate_launch_description():
    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [
                    FindPackageShare("vos_description"),
                    "src",
                    "description",
                    "vos_bringup.urdf.xacro",
                ]
            ),
        ]
    )

    robot_control_config = PathJoinSubstitution(
        [FindPackageShare("kinco_interface"), "config", "forward_controller.yaml"]
    )

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[{"robot_description": robot_description_content}, 
                    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"],
    )

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

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[{"robot_description": robot_description_content}],
    )

    nodes_to_start = [
        robot_state_publisher_node,
        control_node,
        joint_state_broadcaster_spawner,
        forward_position_controller_spawner,
        forward_velocity_controller_spawner,
    ]

    return LaunchDescription(nodes_to_start)

When it stuck at "Wait for device ", I see no messages such as "[INFO] [launch.user]: node 'basic_slave_node' reached the 'inactive' state, 'activating'. [basic_slave_node-1] [INFO] [1710791762.023111597] [can0_com_node]: Reaching inactive state. [basic_slave_node-1] [INFO] [1710791762.024751476] [can0_com_node]: Reaching active state. [basic_slave_node-1] [INFO] [1710791762.026134468] [can0_com_node]: Created slave for node_id 6." mentioned by https://github.com/ros-industrial/ros2_canopen/issues/269#issue-2195244670. However, when I call 

ros2 service call /cia402_device_2/init std_srvs/srv/Trigger

It said "The device is no activated" and returned False.

Setup:

Additional context I'm on an urgent project, great appreciations if someone could help me.

lvjinxin-l commented 6 months ago

I have the same question, what should I do, hope someone can reply

glaciercoder commented 6 months ago

And nothing happened when I recieved 711#00(https://github.com/ros-industrial/ros2_canopen/issues/225#issuecomment-1807414594)

lvjinxin-l commented 6 months ago

I am very much in agreement with you. I smile bitterly. I hope the official can clearly tell me whether I can establish communication with the real device and perform real-time control.

glaciercoder commented 6 months ago

Can you describe the problem you met? @lvjinxin-l

kurtist123 commented 6 months ago

@glaciercoder I was able to get my device booting (https://github.com/ros-industrial/ros2_canopen/issues/269). Here is my bus config:

options: dcf_path: "@BUS_CONFIG_PATH@"

master: node_id: 5 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" baud_rate: 250 start_nodes: false heartbeat_consumer: false heartbeat_producer: 1000 sync_period: 50000

defaults: dcf: "acu_can0.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: false heartbeat_consumer: false heartbeat_producer: 2000 boot: false period: 10 diagnostics: enable: false

nodes: proxy_1: node_id: 1 tpdo: 1: enabled: true cob_id: 0x324 rpdo: 1: enabled: true cob_id: 0x344 transmission: 0xFF event_timer: 100

A couple differences I can see from your code, I'm using the ProxyDriver, not Cia402Driver. I also have the master option start_nodes set to false. My other device starts itself and sends its heartbeat 7xx#00 at boot. After my proxy driver gets that heartbeat message I get passed the "Wait for device to boot" message.

I think I had some issues if I had start_nodes set to true, as I believe it wants to do some other NMT protocol stuff. If your device needs to be booted by the master, could you try setting start_nodes to false, and manually (or have your device) send the 711#00 message just to see if that gets you passed the "Wait for device boot" message just to try an narrow down the issue.

lvjinxin-l commented 6 months ago

Hello, can you control the servo drive and control the motor initialization. Can you please provide the full engineering code, me and he are both stuck at the boot device. @kurtist123

glaciercoder commented 6 months ago

@glaciercoder I was able to get my device booting (#269). Here is my bus config:

options: dcf_path: "@BUS_CONFIG_PATH@" master: node_id: 5 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" baud_rate: 250 start_nodes: false heartbeat_consumer: false heartbeat_producer: 1000 sync_period: 50000 defaults: dcf: "acu_can0.eds" driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: false heartbeat_consumer: false heartbeat_producer: 2000 boot: false period: 10 diagnostics: enable: false nodes: proxy_1: node_id: 1 tpdo: 1: enabled: true cob_id: 0x324 rpdo: 1: enabled: true cob_id: 0x344 transmission: 0xFF event_timer: 100

A couple differences I can see from your code, I'm using the ProxyDriver, not Cia402Driver. I also have the master option start_nodes set to false. My other device starts itself and sends its heartbeat 7xx#00 at boot. After my proxy driver gets that heartbeat message I get passed the "Wait for device to boot" message.

I think I had some issues if I had start_nodes set to true, as I believe it wants to do some other NMT protocol stuff. If your device needs to be booted by the master, could you try setting start_nodes to false, and manually (or have your device) send the 711#00 message just to see if that gets you passed the "Wait for device boot" message just to try an narrow down the issue.

I'm very grateful for your help, I will try proxydriver and change my config, thanks.

glaciercoder commented 6 months ago

Hello, can you control the servo drive and control the motor initialization. Can you please provide the full engineering code, me and he are both stuck at the boot device. @kurtist123

It's not polite to ask for the full code bro. You can try these advice first and submit your problem.

lvjinxin-l commented 6 months ago

yeah，yeah，This is my fault and I will try to fix it. @glaciercoder @kurtist123

lvjinxin-l commented 6 months ago

I followed your method to this point, is this how you communicate with the servo drive and start the motor, I can't test my servo drive until tomorrow. [device_container_node-1] [WARN] [1711504962.207865522] [proxy_1]: Wait for device to boot. [device_container_node-1] [INFO] [1711505008.572547667] [proxy_1]: Driver booted and ready. [device_container_node-1] [INFO] [1711505008.572901877] [proxy_1]: Starting with event
@kurtist123 ros2 service list /device_container_node/change_state /device_container_node/describe_parameters /device_container_node/get_parameter_types /device_container_node/get_parameters /device_container_node/init_driver /device_container_node/list_parameters /device_container_node/set_parameters /device_container_node/set_parameters_atomically /launch_ros_33833/describe_parameters /launch_ros_33833/get_parameter_types /launch_ros_33833/get_parameters /launch_ros_33833/list_parameters /launch_ros_33833/set_parameters /launch_ros_33833/set_parameters_atomically /master/describe_parameters /master/get_parameter_types /master/get_parameters /master/list_parameters /master/sdo_read /master/sdo_write /master/set_parameters /master/set_parameters_atomically /proxy_1/describe_parameters /proxy_1/get_parameter_types /proxy_1/get_parameters /proxy_1/list_parameters /proxy_1/nmt_reset_node /proxy_1/nmt_start_node /proxy_1/sdo_read /proxy_1/sdo_write /proxy_1/set_parameters /proxy_1/set_parameters_atomically But my services are not as many as before, there is no initialization, etc.

kurtist123 commented 6 months ago

dummy_config.zip

I've attached a dummy_config package that has most of my config in it. I've removed my eds file because it has proprietary data in it. You should be able to drop your eds file into the config/can0 directory and add the eds file name in the bus.yml and can0.launch.py files where I've put XXXXXX.eds.

I launch it with ros2 launch dummy_config can0.launch.py canbus:=can0

You will need to change the TPDO and RPDO mappings in the bus.yml to match your eds file.

Once I see the "Driver booted and ready" and "Starting with event" messages, I start seeing my PDOs on the can bus.

A couple things that took me way too long to figure out that may or may not be helpful:

  1. The fake slaves are used for testing on virtual CAN only and simulate your peripheral devices. You do not want to start fake slaves when talking to real devices on a real CAN bus
  2. You need to have a real CAN network set up and a real peripheral device on that CAN network for this to work on a real CAN bus.
  3. Make sure your CAN bus is set up correctly. Below are the commands I use.

sudo ip link set down can0 sudo ip link set can0 type can bitrate 250000 sudo ip link set can0 txqueuelen 1000 sudo ip link set up can0

  1. You have to have the PDOs set up in your eds file and bus.yml. This seems redundant but would not work otherwise
  2. Transmit and receive PDOs are from the point of view of your peripheral device. So the device running your ROS code is actually transmitting the RPDO and receiving the TPDO. In my example bus.yml my ROS device is sending 0x344, its an RPDO because the peripheral device is receiving it.

Your list of services look pretty similar to mine.

kurtist123 commented 6 months ago

I'm also using a forked version of this repo. I made a couple small updates that have not been accepted in PR. They should have absolutely nothing to do with the issue you are seeing but here's my fork just incase. The humble-develop branch has all my updates based on the humble branch.

Unfortunately I'm willing to bet the issue you are seeing has something to do with the Cia402Driver and corresponding protocol. I'm only using the ProxyDriver so I'm not familiar with what all the Cia402Driver does.

lvjinxin-l commented 6 months ago

When I finish running the launch file,and cansend can0 702#00, the terminal responds as follows：[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0 [device_container_node-1] Found rpdo mapped object: index=6060 subindex=0 [device_container_node-1] Found rpdo mapped object: index=607a subindex=0 [device_container_node-1] Found rpdo mapped object: index=60ff subindex=0 [device_container_node-1] Found tpdo mapped object: index=6041 subindex=0 [device_container_node-1] Found tpdo mapped object: index=6061 subindex=0 [device_container_node-1] Found tpdo mapped object: index=6064 subindex=0 [device_container_node-1] Found tpdo mapped object: index=606c subindex=0 [device_container_node-1] [WARN] [1711590016.248517424] [test3]: Wait for device to boot. [device_container_node-1] [INFO] [1711590041.189577478] [test3]: Driver booted and ready. [device_container_node-1] [INFO] [1711590041.191179428] [test3]: Starting with event mode. [device_container_node-1] [INFO] [1711590041.219297034] [canopen_402_driver]: Fault reset I think the end "Fault reset"maybe something wrong. when I launch service : ros2 service call /test3/init std_srvs/srv/Trigger requester: making request: std_srvs.srv.Trigger_Request()

response: std_srvs.srv.Trigger_Response(success=False, message='')

terminal responds as follows [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] sync_sdo_read_typed: id=2 index=0x6502 subindex=0 timed out. [device_container_node-1] [INFO] [1711590103.632577376] [canopen_402_driver]: Init: Read State [device_container_node-1] [INFO] [1711590103.632684677] [canopen_402_driver]: Init: Enable [device_container_node-1] [INFO] [1711590103.668500964] [canopen_402_driver]: Fault reset [device_container_node-1] [INFO] [1711590108.633396486] [canopen_402_driver]: Transition timed out. [device_container_node-1] Could not enable motor please help me my bus.yml: options: dcf_path: "@BUS_CONFIG_PATH@"

master: node_id: 1 driver: "ros2_canopen::MasterDriver" package: "canopen_master_driver" sync_period: 10000 baudrate: 250 start_nodes: false heartbeat_consumer: false heartbeat_producer: 1000 sync_period: 50000

vendor_id: 0x00000300

product_code: 0x4644

defaults: dcf: "Dolphin_V100B0.eds"

driver: "ros2_canopen::Cia402Driver"

package: "canopen_402_driver"

driver: "ros2_canopen::ProxyDriver" package: "canopen_proxy_driver" polling: false heartbeat_consumer: false heartbeat_producer: 2000 period: 10 boot: false period: 10

switching_state: 2

revision_number: 0

sdo:

- {index: 0x6060, sub_index: 0, value: 3}

- {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: 420}
- {index: 0x6083, sub_index: 0, value: 800}
#- {index: 0x6060, sub_index: 0, value: 3} # 3 = profile velocity mode

tpdo: 1: enabled: true

cob_id: "auto"

  cob_id: 0x324
  transmission: 0x01
  mapping:
    - {index: 0x6041, sub_index: 0} # status word
    - {index: 0x6061, sub_index: 0} # mode of operation

rpdo: 1: enabled: true

cob_id: "auto"

  cob_id: 0x344
  transmission: 0xFF
  event_timer: 100
  mapping:
    - {index: 0x6040, sub_index: 0} # control word
    - {index: 0x6060, sub_index: 0} # mode of operation

nodes: proxy_1:
node_id: 2

diagnostics: enable: false

glaciercoder commented 6 months ago

I'm gonna try proxy driver next week, maybe answer you then.

lvjinxin-l commented 6 months ago

How's it going, good brother, is there any progress @glaciercoder

glaciercoder commented 6 months ago

Yeah, I made it!!!!! The bug comes from my master id. The master id is 255 by default in ros2 canopen. However, node is only uses 7 bits in a can frame, which means the id can not exceed 128. I changed my id to 7 and everthing works!!! Thanks for your help! @kurtist123 @lvjinxin-l And have solved the problem to tackle with pdo? I found ros2 topic echo can not echo pdo messages. It said ".../.../COData" is invalid. I don't know whether it is a bug.

leensoft commented 6 months ago

Yeah, I made it!!!!! The bug comes from my master id. The master id is 255 by default in ros2 canopen. However, node is only uses 7 bits in a can frame, which means the id can not exceed 128. I changed my id to 7 and everthing works!!! Thanks for your help! @kurtist123 @lvjinxin-l And have solved the problem to tackle with pdo? I found ros2 topic echo can not echo pdo messages. It said ".../.../COData" is invalid. I don't know whether it is a bug.

Brother, could you please send me the bus.yml file? My email is 896189427@qq.com.

glaciercoder commented 6 months ago

OK guys, I've solved the whole problem including booting and data transmission. The problem I met is caused partly by ros2 canopen and partly by kinco device. Thank you all for your help. The question will be closed.

lvjinxin-l commented 5 months ago

Good brother, please send some running configuration information for me to refer to.My email is 2419439520@qq.com @glaciercoder

leensoft commented 5 months ago

OK guys, I've solved the whole problem including booting and data transmission. The problem I met is caused partly by ros2 canopen and partly by kinco device. Thank you all for your help. The question will be closed.

Brother, could you please send me the bus.yml file?