ros-industrial / ros2_canopen

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

How to use Profile Velocity Mode? #297

Closed millejade closed 2 months ago

millejade commented 4 months ago

Bug I'm trying to use the velocity mode but I'm not sure if I'm doing it right. Position Mode is working well. If I'm doing it right, where I should focus to fix this issue on my end?

Logs This is the full logs on the ros2 launch terminal

[INFO] [launch]: All log files can be found below /home/guest/.ros/log/HP
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [device_container_node-1]: process started with pid [15338]
[device_container_node-1] [INFO] [1722411521.819445745] [device_container_node]: Starting Device Container with:
[device_container_node-1] [INFO] [1722411521.819547889] [device_container_node]:     master_config /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/master.dcf
[device_container_node-1] [INFO] [1722411521.819563386] [device_container_node]:     bus_config /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/bus.yml
[device_container_node-1] [INFO] [1722411521.819572328] [device_container_node]:     can_interface_name can0
[device_container_node-1] [INFO] [1722411521.819841673] [device_container_node]: Loading Master Configuration.
[device_container_node-1] [INFO] [1722411521.820127245] [device_container_node]: Load Library: /opt/ros/humble/lib/libmaster_driver.so
[device_container_node-1] [INFO] [1722411521.821512643] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1722411521.821534499] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1722411521.824830349] [master]: NodeCanopenBasicMaster
[device_container_node-1] [INFO] [1722411521.824860395] [device_container_node]: Load master component.
[device_container_node-1] [INFO] [1722411521.824878301] [device_container_node]: Added /master to executor
[device_container_node-1] [WARN] [1722411521.825953703] [master]: No timeout parameter found in config file. Using default value of 100ms.
[device_container_node-1] [INFO] [1722411521.825989996] [master]: Master boot timeout set to 2000ms.
[device_container_node-1] [INFO] [1722411521.837081959] [device_container_node]: Loading Driver Configuration.
[device_container_node-1] [INFO] [1722411521.837125702] [device_container_node]: Found device dev with driver ros2_canopen::Cia402Driver
[device_container_node-1] [INFO] [1722411521.837283055] [device_container_node]: Load Library: /opt/ros/humble/lib/libcia402_driver.so
[device_container_node-1] [INFO] [1722411521.840257181] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1722411521.840309733] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1722411521.844046563] [device_container_node]: Load driver component.
[device_container_node-1] [INFO] [1722411521.844085816] [device_container_node]: Added /dev to executor
[device_container_node-1] [INFO] [1722411521.851427740] [dev]: Non transmit timeout100ms
[device_container_node-1] [WARN] [1722411521.851477959] [dev]: Could not read period from config, setting to 10ms
[device_container_node-1] [WARN] [1722411521.851506346] [dev]: Could not read enable diagnostics from config, setting to false.
[device_container_node-1] [INFO] [1722411521.851579480] [dev]: scale_pos_to_dev_ 1000.000000
[device_container_node-1] scale_pos_from_dev_ 0.001000
[device_container_node-1] scale_vel_to_dev_ 1000.000000
[device_container_node-1] scale_vel_from_dev_ 0.001000
[device_container_node-1] 
[device_container_node-1] [INFO] [1722411521.852798576] [tmcm_1241]: eds file /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/dev.dcf
[device_container_node-1] [INFO] [1722411521.852825448] [tmcm_1241]: bin file /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/dev.bin
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[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=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=607a subindex=0
[device_container_node-1] Found rpdo mapped object: index=6040 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=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6061 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6064 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=606c subindex=0
[device_container_node-1] [INFO] [1722411521.855047584] [dev]: Driver booted and ready.
[device_container_node-1] [INFO] [1722411521.855142838] [dev]: Starting with polling mode.
[device_container_node-1] [INFO] [1722411521.865395528] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1722411842.878762614] [canopen_402_driver]: Init: Read State
[device_container_node-1] [INFO] [1722411842.878791460] [canopen_402_driver]: Init: Enable
[device_container_node-1] [INFO] [1722411842.885309188] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1722411842.998478755] [canopen_402_driver]: Init: Switch to homing
[device_container_node-1] [INFO] [1722411843.005535258] [canopen_402_driver]: Init: Execute homing
[device_container_node-1] [INFO] [1722411843.008151811] [canopen_402_driver]: Init: Switch no mode
[device_container_node-1] terminate called after throwing an instance of 'lely::canopen::SdoError'
[device_container_node-1]   what():  TpdoWrite:01:60FF:00: Object cannot be mapped to the PDO (06040041): Object cannot be mapped to the PDO
[ERROR] [device_container_node-1]: process has died [pid 15338, exit code -6, cmd '/opt/ros/humble/lib/canopen_core/device_container_node --ros-args --log-level info --ros-args -r __node:=device_container_node -r __ns:=/ --params-file /tmp/launch_params__gpcxucj --params-file /tmp/launch_params_66_hzx58 --params-file /tmp/launch_params_qg_jfm1l --params-file /tmp/launch_params_d3xqlx98'].

Setup:

Additional context So what I did was:

1. Do the roslaunch

This is my Launch Files:

These are the logs after launching it:

[INFO] [launch]: All log files can be found below /home/guest/.ros/log/HP
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [device_container_node-1]: process started with pid [15338]
[device_container_node-1] [INFO] [1722411521.819445745] [device_container_node]: Starting Device Container with:
[device_container_node-1] [INFO] [1722411521.819547889] [device_container_node]:     master_config /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/master.dcf
[device_container_node-1] [INFO] [1722411521.819563386] [device_container_node]:     bus_config /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/bus.yml
[device_container_node-1] [INFO] [1722411521.819572328] [device_container_node]:     can_interface_name can0
[device_container_node-1] [INFO] [1722411521.819841673] [device_container_node]: Loading Master Configuration.
[device_container_node-1] [INFO] [1722411521.820127245] [device_container_node]: Load Library: /opt/ros/humble/lib/libmaster_driver.so
[device_container_node-1] [INFO] [1722411521.821512643] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1722411521.821534499] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::MasterDriver>
[device_container_node-1] [INFO] [1722411521.824830349] [master]: NodeCanopenBasicMaster
[device_container_node-1] [INFO] [1722411521.824860395] [device_container_node]: Load master component.
[device_container_node-1] [INFO] [1722411521.824878301] [device_container_node]: Added /master to executor
[device_container_node-1] [WARN] [1722411521.825953703] [master]: No timeout parameter found in config file. Using default value of 100ms.
[device_container_node-1] [INFO] [1722411521.825989996] [master]: Master boot timeout set to 2000ms.
[device_container_node-1] [INFO] [1722411521.837081959] [device_container_node]: Loading Driver Configuration.
[device_container_node-1] [INFO] [1722411521.837125702] [device_container_node]: Found device dev with driver ros2_canopen::Cia402Driver
[device_container_node-1] [INFO] [1722411521.837283055] [device_container_node]: Load Library: /opt/ros/humble/lib/libcia402_driver.so
[device_container_node-1] [INFO] [1722411521.840257181] [device_container_node]: Found class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1722411521.840309733] [device_container_node]: Instantiate class: rclcpp_components::NodeFactoryTemplate<ros2_canopen::Cia402Driver>
[device_container_node-1] [INFO] [1722411521.844046563] [device_container_node]: Load driver component.
[device_container_node-1] [INFO] [1722411521.844085816] [device_container_node]: Added /dev to executor
[device_container_node-1] [INFO] [1722411521.851427740] [dev]: Non transmit timeout100ms
[device_container_node-1] [WARN] [1722411521.851477959] [dev]: Could not read period from config, setting to 10ms
[device_container_node-1] [WARN] [1722411521.851506346] [dev]: Could not read enable diagnostics from config, setting to false.
[device_container_node-1] [INFO] [1722411521.851579480] [dev]: scale_pos_to_dev_ 1000.000000
[device_container_node-1] scale_pos_from_dev_ 0.001000
[device_container_node-1] scale_vel_to_dev_ 1000.000000
[device_container_node-1] scale_vel_from_dev_ 0.001000
[device_container_node-1] 
[device_container_node-1] [INFO] [1722411521.852798576] [tmcm_1241]: eds file /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/dev.dcf
[device_container_node-1] [INFO] [1722411521.852825448] [tmcm_1241]: bin file /home/guest/Desktop/CANopen-study/ros_ws/install/dev_canopen/share/dev_canopen/config/cia402/dev.bin
[device_container_node-1] Found rpdo mapped object: index=6040 subindex=0
[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=6040 subindex=0
[device_container_node-1] Found rpdo mapped object: index=607a subindex=0
[device_container_node-1] Found rpdo mapped object: index=6040 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=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6061 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6064 subindex=0
[device_container_node-1] Found tpdo mapped object: index=6041 subindex=0
[device_container_node-1] Found tpdo mapped object: index=606c subindex=0
[device_container_node-1] [INFO] [1722411521.855047584] [dev]: Driver booted and ready.
[device_container_node-1] [INFO] [1722411521.855142838] [dev]: Starting with polling mode.
[device_container_node-1] [INFO] [1722411521.865395528] [canopen_402_driver]: Fault reset

2. Call the Service /init

After calling this service, I got this logs from the same terminal:

[device_container_node-1] [INFO] [1722411842.878762614] [canopen_402_driver]: Init: Read State
[device_container_node-1] [INFO] [1722411842.878791460] [canopen_402_driver]: Init: Enable
[device_container_node-1] [INFO] [1722411842.885309188] [canopen_402_driver]: Fault reset
[device_container_node-1] [INFO] [1722411842.998478755] [canopen_402_driver]: Init: Switch to homing
[device_container_node-1] [INFO] [1722411843.005535258] [canopen_402_driver]: Init: Execute homing
[device_container_node-1] [INFO] [1722411843.008151811] [canopen_402_driver]: Init: Switch no mode

3. Call the Service to use Velocity Mode

I then called the service for velocity mode and it returns True, check the logs below:

ros2 service call /tmcm_1241/velocity_mode std_srvs/srv/Trigger
requester: making request: std_srvs.srv.Trigger_Request()

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

4. Call the Service Target

This service call returns True - check the logs below:

ros2 service call /tmcm_1241/target canopen_interfaces/srv/COTargetDouble "{target: 100000}"
requester: making request: canopen_interfaces.srv.COTargetDouble_Request(target=100000.0)

response:
canopen_interfaces.srv.COTargetDouble_Response(success=True)

On the other hand, this is the log from the ros2 launch terminal:

[device_container_node-1] terminate called after throwing an instance of 'lely::canopen::SdoError'
[device_container_node-1]   what():  TpdoWrite:01:60FF:00: Object cannot be mapped to the PDO (06040041): Object cannot be mapped to the PDO
[ERROR] [device_container_node-1]: process has died [pid 15338, exit code -6, cmd '/opt/ros/humble/lib/canopen_core/device_container_node --ros-args --log-level info --ros-args -r __node:=device_container_node -r __ns:=/ --params-file /tmp/launch_params__gpcxucj --params-file /tmp/launch_params_66_hzx58 --params-file /tmp/launch_params_qg_jfm1l --params-file /tmp/launch_params_d3xqlx98'].
millejade commented 4 months ago

For additional info:

This is my bus.yml:

# General options especially dcf_path
options:
  dcf_path: "@BUS_CONFIG_PATH@"

# The configuration of the master
master:
  node_id: 2
  driver: "ros2_canopen::MasterDriver"
  package: "canopen_master_driver"
  sync_period: 20000 # The SYNC interval in μs (default: 0)

# Defaults that apply to all slave nodes
defaults:
  driver: "ros2_canopen::Cia402Driver"
  package: "canopen_402_driver"
  polling: true

# Configurations for all slave nodes
nodes:
  dev:
    dcf: "dev.dcf"
    node_id: 1

And this is my launch file:

dev_canopen.launch.py

def generate_launch_description():
    ld = LaunchDescription()

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

    ld.add_action(device_container)

    return ld
hellantos commented 4 months ago

Hi, I haven't seen this error yet. It seems, that 60ff is not mapped as PDO even though it is indicated as mapped in your dcf.

You can check how to do pdo configuration in bus.yaml here canopen_tests/config/cia402/bus.yml. For Velocity mode at least 606c (actual velocity) as tpdo an 60ff (target velocity) as rpdo are necessary.

millejade commented 4 months ago

Hello,

Thanks for quick response!

That means I'm doing the correct process of sending velocity?

(It was because I've seen in the documentation a subscriber /target but it doesn't really appear when I'm doing ros2 topic list.)

millejade commented 4 months ago

Hello, just an update!

It is indeed an issue on my TPDO4 and RPDO4 - they are disabled in the device so I had to enable it in a DCF/EDS file accordingly.

I enabled it and I do not encounter any error after doing this:

@hellantos, questions:

millejade commented 3 months ago

Hello @hellantos !

Any insights on my previous questions?

Thank you!

hellantos commented 3 months ago

@millejade Can you check if the pdos are sent by using candump or a similar logging tool? If you only enabled in the dcf but not on the device, this will make ros2_canopen believe it is enabled, while the device does not have the pdos enabled.

If you want ros2 canopen to enable the pdos please specify the pdos in bus.yml.

millejade commented 3 months ago

Thanks @hellantos!

Maybe the PDOs are enabled in the DCF but not in the device.

If you want ros2 canopen to enable the pdos please specify the pdos in bus.yml.

Do you mean, if the PDOs were not enabled in the device, I can enable it using bus.yml? (Given that the PDOs are enabled in the DCF)

millejade commented 2 months ago

This has been solved. Thanks @hellantos!

The TPDO and RPDO is actually disabled. Even if I enabled it on the DCF/EDS and enabled it using the device's IDE, it still didn't work but enabling it only using sdo in bus.yml works. For me, I have to rely on the FW manual of the device where index 0x1403 is my RPDO4 and 0x1803 is my TPDO4 - these are the PDOs that I need for Velocity control. You can refer to the FW Manual of the device as I'm not sure if it is different for each device.