Support: ros_canopen, experimental ROS2 branches

You can ask questions about the experimental ROS2 support for ros_canopen found at https://github.com/samiamlabs/ros_canopen in this topic.

First of all, thank you for this forum !
I’m still stuck trying to activate the canopen_chain node. If I use your yaml file but just change the path to the .dcf file, I think it should work fine.
The .dcf file is the same I used in my ROS1 project so I don’t think the mistake is in there.
What could be the problem ?
After closing with crtl-c I do get the “CAN init Finished” message
`
code: https://github.com/samiamlabs/ros_canopen/blob/dashing-experimental/canopen_chain_node/src/canopen_chain_component.cpp#L577

The only possible problem that I can see is the difference between both lines of code in the .yaml file:

defaults:
      eds_pkg: 'canopen_chain_node' # optional package  name for relative path
      eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

canopen_nodes: ['motor1']
      motor1:
      id: 2 
      eds_pkg: 'canopen_chain_node' # optional package  name for relative path
      eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

What should I use or is there no difference ?

The indentation level of motor1 looks wrong in this post.

It should be:

canopen_nodes: ['motor1']

motor1:
  id: 2 
  eds_pkg: 'canopen_chain_node' # optional package  name for relative path
  eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

Did the same .dcf file work with ros_canopen for ROS1?

Are you getting the following error before it gets stuck? Could not initialize main CAN Layer! Is the CAN network available? Is device power on?
I added some more information to that error print on the eloquent-experimental branch that could be helpful.

I also just finished a new implementation for Profiled Velocity Control with CiA402 motors on that branch that could possibly work out of the box if you are using such a motor :slight_smile:

I think the branch works on dashing, but have only tested it on eloquent yet.

Yes indeed, this was a typo in my question.
If I only use the default node in the .yaml file, I don’t get an error message at all.
If I use both the default and motor1 node (same dcf file), I get the Could not initialize main CAN Layer! Is the CAN network available? Is device power on? error message.
How should I write the .yaml file ? The .dcf file worked fine for ROS.
I will try the eloquent branch and give some feedback, thanks :slight_smile:

Ok!

It may be helpful if you copy-paste the entire terminal output from launching here so I can check if all the parameters are picked up properly.

Case1:

defaults:
    eds_pkg: 'canopen_chain_node' # optional package  name for relative path
    eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

[manual_composition-1] [INFO] [canopen_chain]: Creating canopen_chain component
[manual_composition-1] [INFO] [canopen_chain]: Configuring canopen_chain
[manual_composition-1] [INFO] [canopen_chain]: update_ms: 10
[manual_composition-1] [INFO] [canopen_chain]: Configuring bus
[manual_composition-1] [INFO] [canopen_chain]: bus.device: can0
[manual_composition-1] [INFO] [canopen_chain]: bus.driver_plugin: socketcan_inte rface/SocketCANInterface
[manual_composition-1] [INFO] [canopen_chain]: bus.master_allocator: canopen_mas ter/SimpleMasterAllocator
[manual_composition-1] [INFO] [canopen_chain]: bus.loopback: false
[manual_composition-1] [INFO] [canopen_chain]: Configuring sync
[manual_composition-1] [INFO] [canopen_chain]: sync.overflow: 10
[manual_composition-1] [INFO] [canopen_chain]: sync.interval_ms: 10
[manual_composition-1] [INFO] [canopen_chain]: use_sync: 0
[manual_composition-1] [INFO] [canopen_chain]: Configuring heartbeat
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.use_heartbeat: 0
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.msg: 708#05
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.rate: 10.000000
[manual_composition-1] [INFO] [canopen_chain]: Configuring defaults
[manual_composition-1] [INFO] [canopen_chain]: defaults.eds_pkg: canopen_chain_n ode
[manual_composition-1] [INFO] [canopen_chain]: defaults.eds_file: test/config/mo tor1.dcf
[manual_composition-1] [INFO] [canopen_chain]: Configuring nodes
[manual_composition-1] [INFO] [canopen_chain]: Activating canopen_chain

After closing the node with ctrl-C

[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[manual_composition-1] [INFO] [canopen_chain]: CAN init has finished!

Case 2:

defaults:
      eds_pkg: 'canopen_chain_node' # optional package  name for relative path
      eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

canopen_nodes: ['motor1']
motor1:
      id: 2 
      eds_pkg: 'canopen_chain_node' # optional package  name for relative path
      eds_file: 'test/config/motor1.dcf' # path to EDS/DCF file

[manual_composition-1] [INFO] [canopen_chain]: Creating canopen_chain component
[manual_composition-1] [INFO] [canopen_chain]: Configuring canopen_chain
[manual_composition-1] [INFO] [canopen_chain]: update_ms: 10
[manual_composition-1] [INFO] [canopen_chain]: Configuring bus
[manual_composition-1] [INFO] [canopen_chain]: bus.device: can0
[manual_composition-1] [INFO] [canopen_chain]: bus.driver_plugin: socketcan_interface/SocketCANInterface
[manual_composition-1] [INFO] [canopen_chain]: bus.master_allocator: canopen_master/SimpleMasterAllocator
[manual_composition-1] [INFO] [canopen_chain]: bus.loopback: false
[manual_composition-1] [INFO] [canopen_chain]: Configuring sync
[manual_composition-1] [INFO] [canopen_chain]: sync.overflow: 10
[manual_composition-1] [INFO] [canopen_chain]: sync.interval_ms: 10
[manual_composition-1] [INFO] [canopen_chain]: use_sync: 0
[manual_composition-1] [INFO] [canopen_chain]: Configuring heartbeat
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.use_heartbeat: 0
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.msg: 708#05
[manual_composition-1] [INFO] [canopen_chain]: heartbeat.rate: 10.000000
[manual_composition-1] [INFO] [canopen_chain]: Configuring defaults
[manual_composition-1] [INFO] [canopen_chain]: defaults.eds_pkg: canopen_chain_node
[manual_composition-1] [INFO] [canopen_chain]: defaults.eds_file: test/config/motor1.dcf
[manual_composition-1] [INFO] [canopen_chain]: Configuring nodes
[manual_composition-1] [INFO] [canopen_chain]: motor1 id: 2
[manual_composition-1] [INFO] [canopen_chain]: motor1 eds_file: test/config/motor1.dcf
[manual_composition-1] [INFO] [canopen_chain]: motor1 eds_pkg: canopen_chain_node
[manual_composition-1] [INFO] [canopen_chain]: Activating canopen_chain
[manual_composition-1] [ERROR] [canopen_chain]: Could not initialize main CAN Layer! Is the CAN network available? Is device power on?
[ERROR] [launch_ros.actions.lifecycle_node]: Failed to make transition ‘TRANSITION_ACTIVATE’ for LifecycleNode ‘/canopen_chain’
[manual_composition-1] [INFO] [canopen_chain]: Activating canopen_chain
[manual_composition-1] [ERROR] [canopen_chain]: Could not initialize main CAN Layer! Is the CAN network available? Is device power on?
[ERROR] [launch_ros.actions.lifecycle_node]: Failed to make transition ‘TRANSITION_ACTIVATE’ for LifecycleNode ‘/canopen_chain’
[manual_composition-1] [INFO] [canopen_chain]: Activating canopen_chain
[manual_composition-1] [ERROR] [canopen_chain]: Could not initialize main CAN Layer! Is the CAN network available? Is device power on?
[ERROR] [launch_ros.actions.lifecycle_node]: Failed to make transition ‘TRANSITION_ACTIVATE’ for LifecycleNode ‘/canopen_chain’

Looks like there is a typo in the first version: defaults.eds_pkg: canopen_chain_n ode
An extra space character in the package name. Should probably print a warning for that…

The second error could be a lot of different things, from parsing the .dfc file, failing to set an object dictionary entry on the device, wrong baudrate etc…

The error message on the new branch should give you more info about what went wrong during the initialization.

It is only after closing the process that everything seems to work:
I tried to send “0x6” to the controlword and afther closing the process I can see following message:

[manual_composition-1] [INFO] [canopen_chain]: Setting object: 6040 from node: motor1 to: 6

full output:

[manual_composition-1] [INFO] [canopen_chain]: Activating canopen_chain
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[main_pub-2] [ERROR] [main_pub]: Failed to init motor.
[main_pub-2] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[main_pub-2]   what():  Failed to create interrupt guard condition in Executor constructor: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at /tmp/binarydeb/ros-dashing-rcl-0.7.8/src/rcl/guard_condition.c:69
[INFO] [manual_composition-1]: process has finished cleanly [pid 22193]
[ERROR] [main_pub-2]: process has died [pid 22194, exit code -6, cmd '/home/jens/ros_canopen_ws/canopen_node/install/canopen_node/lib/canopen_node/main_pub __node:=main_pub'].
[manual_composition-1] [INFO] [canopen_chain]: CAN init has finished!
[manual_composition-1] [INFO] [canopen_chain]: Setting object: 6040 from node: motor1 to: 6
[manual_composition-1] [INFO] [rclcpp]: signal_handler(signal_value=2)

Is the motor initializing by itself or do I need to send a service to do so ?

ROS1 -> std_srvs/srv/Trigger (/init)

(“CAN init has finished”) is not showing and the /init server is not active.

waiting for service to become available…

How do I need to handle the initialization ?

Edit:
In the code I can find the /recover service using the same std_srvs/srv/Trigger service.


I would expect something simular but that can handle the initialisation (/init)

Edit:
Is this initializing the motor ? (and failing …)

Yes, I moved the “init” function from a service to the on_activate life cycle function.

I really don’t understand why it’s not working. I don’t get any error messages but it doesn’t want to initialize.

I’m able to send PDO and SDO messages with the motor and I get some answers so I assume that the communication is fine.

It is unfortunately the case that ROS Loggs are not always printed to terminal in ROS2. Have you tried looking at the output by using ros2 topic echo /rosout?

Do you mean that the ROS services work for sending PDO and SDO messages or that you can to it with some other tool?

1 Like

Thank you that was really helpful, I do get the “CAN init has finished” :slight_smile:

example 1: changing the velocity acceleration (default = 1000)

jens@jens-desktop:~$ ros2 service call /set_object canopen_msgs/srv/SetObject “{node: motor1, object: 6048sub1, value: 1500}”
waiting for service to become available…
requester: making request: canopen_msgs.srv.SetObject_Request(node=‘motor1’, object=‘6048sub1’, value=‘1500’, cached=False)

response:
canopen_msgs.srv.SetObject_Response(success=True, message=’’)

jens@jens-desktop:~$ ros2 service call /get_object canopen_msgs/srv/GetObject “{node: motor1, object: 6048sub1}”
waiting for service to become available…
requester: making request: canopen_msgs.srv.GetObject_Request(node=‘motor1’, object=‘6048sub1’, cached=False)

This seems to work right ?

But now I want to enable the motor like shown in the canopen state flow.

jens@jens-desktop:~$ ros2 service call /set_object canopen_msgs/srv/SetObject “{node: motor1, object: 6040, value: 6}”
waiting for service to become available…
requester: making request: canopen_msgs.srv.SetObject_Request(node=‘motor1’, object=‘6040’, value=‘6’, cached=False)

response:
canopen_msgs.srv.SetObject_Response(success=True, message=’’)

jens@jens-desktop:~$ ros2 service call /get_object canopen_msgs/srv/GetObject “{node: motor1, object: 6041}”
waiting for service to become available…
requester: making request: canopen_msgs.srv.GetObject_Request(node=‘motor1’, object=‘6041’, cached=False)

response:
canopen_msgs.srv.GetObject_Response(success=True, message=‘Got object without issues!’, value=‘0’)

The statusword stays “0”, so this means that the motor is still not in status “Switch ON disabled” but in “Not Ready to switch on”. And to get to the next step, the motor needs to initialize.

This together with the missing output made me think that the initialization failed.

Ok!

Here is how it looks for me in wiershark when i activate the node: https://youtu.be/SnnrymGVSL8

It is working ! Thank you a lot for your time and effort :slight_smile:
I had a problem with the dcf-file like you thought a few days ago (maybe something has changed with the port to ROS2), but it works with the eds-file.

Great to hear!

I noticed that out-commeted stuff in dcf-files seem to unsupported. I have made very limited changes to the parsing from ROS1, so it is a bit odd that that was the issue.

Are you using the motor profile or with no profile?

I’m still using the chain_node instead of the motor_node. I saw that you added the 402-profile on github, but I don’t know yet how to use it without the motor_node.

I did noticed a small difference in our dcf-files. My defaultValue is set to “0” while yours is left blank. When I tried to get the StatusWord, it always replied with “0”. Maybe this was the mistake and maybe this didn’t matter in ROS1.

[6041]
ParameterName=Status word
Denotation=
ObjectType=0x7
;StorageLocation=RAM
DataType=0x0006
AccessType=ro
HighLimit=65535
LowLimit=0
DefaultValue=
ParameterValue=
PDOMapping=1

[6041]
ParameterName=Statusword
ObjectType=0x7
DataType=0x0006
AccessType=ro
DefaultValue=0
PDOMapping=1
ParameterValue=0

Ok, a bit odd but maybe I or someone else made some type of breaking change in the parser.

A config like:

motor_left:
  id: 2 
  profiles: ['motor']
  eds_pkg: 'canopen_chain_node' # optional package  name for relative path
  eds_file: 'test/config/N5-2-2.eds' # path to EDS/DCF file
  default_operation_mode: 'Profiled Velocity'
  motor_to_angular_velocity_scaling_factor: 1.0

For canopen_chain_node should create services and topics for controlling the motor in Profiled Velocity Mode if the motor is 402 compliant. It is only tested on Nanotec N5 drivers with properly mapped PDOs though…

I suspect that this line can cause issues. I think I have had some trouble with out-commented lines. have started to just add ParameterValue entries to the .eds file manually rather than using a tool to minimize issues myself.