CANopen for noobs - How to begin?

[grafoteka]

Hello, I am trying to use ros_canopen with 1 IXXAT USB-to-CAN controller and 6 Maxon MCD Epos.

The system is working correctly in Windows and in a normal cpp file in Ubuntu. But I am having a lot of troubles with the ROS integration.

First I tried the epos_hardware package (http://wiki.ros.org/epos_hardware) but it only works for me with a serial adapter and the frequency for control was only 3 Hz. That is why I had to change to CAN configuration.

I am following this steps: https://answers.ros.org/question/250174/how-to-control-maxon-motor-by-using-ros_canopen/

1. Socketcan is working
   
2. The URDF is OK
3. The DCF file has been generated with Maxon EPOS Studio. But I have a question here. Do I have to generate the file with the CAN network or just for a single unit? I suppose that should be with the CAN network.
4. Prepare driver config.

bus:
  device: can0 # socketcan network
  loopback: false # socket should loop back messages
  driver_plugin: can::SocketCANInterface
  master_allocator: canopen::SimpleMaster::Allocator
sync:
  interval_ms: 10 # set to 0 to disable sync
  update_ms: <interval_ms> #update interval of control loop, must be set explecitly if sync is disabled
  overflow: 0 # overflow sync counter at value or do not set it (0, default)
  #heartbeat: # simple heartbeat producer, optional!
  #rate: 20 # heartbeat rate
  msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started

# struct syntax
nodes:
  node1:
    id: 1
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_1.dcf" # path to EDS/DCF file

  node2:
    id: 2
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_2.dcf" # path to EDS/DCF file

  node3:
    id: 3
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_3.dcf" # path to EDS/DCF file

  node4:
    id: 4
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_4.dcf" # path to EDS/DCF file

  node5:
    id: 5
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_5.dcf" # path to EDS/DCF file

  node6:
    id: 6
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_6.dcf" # path to EDS/DCF file

defaults: # optional, all defaults can be overwritten per node
  # canopen_chain_node settings ..
  motor_allocator: canopen::Motor402::Allocator # select allocator for motor layer
  #motor_layer: # settings passed to motor layer (plugin-specific)
  switching_state: 5 # (Operation_Enable), state for mode switching
  pos_to_device: "rint(rad2deg(pos)*1000)" # rad -> mdeg
  pos_from_device: "deg2rad(obj6064)/1000" # actual position [mdeg] -> rad
  vel_to_device: "rint(rad2deg(vel)*1000)" # rad/s -> mdeg/s
  vel_from_device: "deg2rad(obj606C)/1000" # actual velocity [mdeg/s] -> rad/s
  eff_to_device: "rint(eff)" # just round to integer
  #eff_from_device: "0" # unset

5. list of controllers

hexapodo:
  # Publish all joint states -----------------------------------
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 50

  # Velocity Controllers ---------------------------------------
    pata1_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_1_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3
    pata2_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_2_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3
    pata3_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_3_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3
    pata4_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_4_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3
    pata5_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_5_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3
    pata6_velocity_controller:
        type: effort_controllers/JointVelocityController
        joint: pata_6_to_base_link_joint
        pid: {p: 3.0, i: 0.0, d: 0.0}
        required_drive_mode: 3

6. Prepare a launchfile for canopen_motor_node

<launch>

    <!-- Load the URDF into ROS parameter server -->
    <param name="robot_description"
         command="$(find xacro)/xacro.py '$(find hexapodo_description)/robots/hexapodo.robot.xacro' --inorder" />
    <!--param name="robot_description" textfile="$(find hexapodo_description)/robots/hexapodo.robot.xacro"/-->

    <node name="canopen_chain" pkg="canopen_chain_node" type="canopen_chain_node" output="screen" clear_params="true">
        <rosparam command="load" file="$(find canopen_chain_node)/config/canopen.yaml" />
    </node>

<!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find hexapodo_control)/config/hexapodo_control.yaml" command="load"/>

    <!-- load the controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
      output="screen" ns="/hexapodo" args="joint_state_controller
                                       pata1_velocity_controller
                                       pata2_velocity_controller
                                       pata3_velocity_controller
                                       pata4_velocity_controller
                                       pata5_velocity_controller
                                       pata6_velocity_controller"/>
    <!-- Combine joint values -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

    <!-- Show in Rviz   -->
    <node name="rviz" pkg="rviz" type="rviz"/>

</launch>

When I use the launcher it seems to be correct, but when I execute:
rosservice call /driver/init
I get the next error, from the rosservice:

jorge@jorge-ubuntu:~$ rosservice call /driver/init 
success: False
message: "could not start node '4'"

And from the launcher:

[ INFO] [1527761900.313002947]: Initializing XXX
[ INFO] [1527761900.313342980]: Current state: 1 device error: system:0 internal_error: 0 (OK)
[ INFO] [1527761900.313549363]: Current state: 2 device error: system:0 internal_error: 0 (OK)
[ INFO] [1527761908.533445322]: Current state: 0 device error: system:0 internal_error: 0 (OK)
[ INFO] [1527761908.533558974]: Current state: 0 device error: system:0 internal_error: 0 (OK)

Can someone help me to control the motors?

Thank you very much
Kinds regards,
Jorge

[grafoteka]

I have get a little step forward,
Something is wrong with the node 4. So I have decided to don't launch it by the moment.
Now, with the new launcher I can get the motors enabled.

[ INFO] [1528096380.811105310]: Initializing XXX
[ INFO] [1528096383.022834373]: Current state: 1 device error: system:0 internal_error: 0 (OK)
[ INFO] [1528096383.023066861]: Current state: 2 device error: system:0 internal_error: 0 (OK)
EMCY: 81#0000000000000000
EMCY: 82#0000000000000000
EMCY: 83#0000000000000000
EMCY: 85#0000000000000000
EMCY: 86#0000000000000000

Also, I can read the /diagnostics topic

---
header: 
  seq: 225
  stamp: 
    secs: 1528096488
    nsecs:  44686352
  frame_id: ''
status: 
  - 
    level: 0
    name: "canopen_chain: chain"
    message: ''
    hardware_id: "none"
    values: []
  - 
    level: 0
    name: "canopen_chain: pata1_velocity_controller"
    message: ''
    hardware_id: "none"
    values: []
  - 
    level: 0
    name: "canopen_chain: pata2_velocity_controller"
    message: ''
    hardware_id: "none"
    values: []
  - 
    level: 0
    name: "canopen_chain: pata3_velocity_controller"
    message: ''
    hardware_id: "none"
    values: []
  - 
    level: 0
    name: "canopen_chain: pata5_velocity_controller"
    message: ''
    hardware_id: "none"
    values: []
  - 
    level: 0
    name: "canopen_chain: pata6_velocity_controller"
    message: ''
    hardware_id: "none"
    values: []
---

But I don't know how to give an order to any motor or read it position.

Also, I would like to know how to assign the hardware_id.

Many thanks,
Jorge

[mathias-luedtke]

Do I have to generate the file with the CAN network or just for a single unit? I suppose that should be with the CAN network.

You can use individual files. For more generic cases, one file for all with dcf_overlay might be easier to maintain.

But I don't know how to give an order to any motor or read it position.

Your launch file starts canopen_chain_node.
For access of motor-related objects, you could use canopen_motor_node.

Also, I would like to know how to assign the hardware_id.

You can set hardware_id for the whole chain in the ~hardware_id param.

[grafoteka]

Thank you for your reply @ipa-mdl
I try to run canopen_motor_chain_node but I have the next error:

jorge@jorge-ubuntu:~$ rosrun canopen_motor_node canopen_motor_node 
[ERROR] [1528102275.161342267]: Device not set

I look for this message in the package but I haven't found it. Also I try find any config file, but there isn't.
Maybe this error could be because I have this warning in the chain_node launcher?

[INFO] [1528103262.783379]: Controller Spawner: Waiting for service controller_manager/load_controller
[WARN] [1528103293.022169]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[controller_spawner-3] process has finished cleanly
log file: /home/jorge/.ros/log/bc1ff920-67d6-11e8-85b8-20cf30b6161c/controller_spawner-3*.log

If this is correct, the next step should be to launch correctly the controller_manager with the URDF of robot?

Thank you,
Jorge

[mathias-luedtke]

Do not run it directly, but in your launch file (instead of canopen_motor_node canopen_chain_node!!).
Your config is for canopen_motor_node anyway.

[grafoteka]

Hi @ipa-mdl,
I continue working and now, I don't get the error in my RViz visualization:

Old one

New one

But still stuck with the controller_manager and the canopen_motor_node

I can launch the controller_manager without any errors:

<!-- Load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find hexapodo_control)/config/hexapodo_control.yaml" command="load"/>

    <!-- load the controllers -->
    <node name="controller"
          pkg="controller_manager"
          type="controller_manager"
          respawn="false"
          output="screen"
          ns="hexapodo"
          args="spawn
                joint_state_controller
                pata1_velocity_controller
                pata2_velocity_controller
                pata3_velocity_controller
                pata4_velocity_controller
                pata5_velocity_controller
                pata6_velocity_controller"/>

And this is the controllers yaml file:

hexapodo:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  # Velocity Controllers ---------------------------------------
  pata1_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_1_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

  pata2_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_2_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

  pata3_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_3_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

  pata4_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_4_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

  pata5_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_5_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

  pata6_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: motor_6_to_base_link_joint
    pid: {p: 3.0, i: 0.0, d: 0.0}
    required_drive_mode: 3

But even I have no errors with the spawn of the controller_manager, there isn't any controller.

If I run rosservice list, this is what I get:

jorge@jorge-ubuntu:~$ rosservice list 
/canopen_chain/get_loggers
/canopen_chain/set_logger_level
/driver/get_object
/driver/halt
/driver/init
/driver/recover
/driver/set_object
/driver/shutdown
/joint_state_publisher/get_loggers
/joint_state_publisher/set_logger_level
/robot_state_publisher/get_loggers
/robot_state_publisher/set_logger_level
/rosout/get_loggers
/rosout/set_logger_level
/rviz/get_loggers
/rviz/reload_shaders
/rviz/set_logger_level

Maybe, I should have to implement:
hardware_interface::VelocityJointInterface:
- Velocity
- Profiled Velocity
- Cyclic Synchronous Velocity

As the wiki of canopen_motor_node explains?

Do not run it directly, but in your launch file (instead of canopen_motor_node!).
Your config is for canopen_motor_node anyway.

Do you mean that with

<node name="canopen_chain" pkg="canopen_chain_node" type="canopen_chain_node" output="screen" clear_params="true">
<rosparam command="load" file="$(find canopen_chain_node)/config/canopen.yaml" />

the canopen_motor_node is launched?

[mathias-luedtke]

Do you mean that with [...] the canopen_motor_node is launched?

No, this is not what I meant.
I would suggest to have a look at some existing support packages lik schunk_lwap.

This discussion is out of the scope of this issue tracker.
If you need help in updating the launch files, ROS answers might be a better place to get community support.

[mathias-luedtke]

Do not run it directly, but in your launch file (instead of canopen_motor_node!).

Sry, this line was wrong, it should read "instead of canopen_chain_node".

[grafoteka]

Sorry, I think I was confused with the configuration of the package.

I only need the URDF of the robot for the nodes name:

# struct syntax
nodes:
  motor_1_to_base_link_joint: # Es el joint del URDF
    id: 1
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_1.dcf" # path to EDS/DCF file
  motor_2_to_base_link_joint:
    id: 2
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_2.dcf" # path to EDS/DCF file
  motor_3_to_base_link_joint:
    id: 3
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_3.dcf" # path to EDS/DCF file
  motor_5_to_base_link_joint:
    id: 5
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_5.dcf" # path to EDS/DCF file
  motor_6_to_base_link_joint:
    id: 6
    eds_pkg: canopen_chain_node # optionals package name for relative path
    eds_file: "config/MCDepos_node_6.dcf" # path to EDS/DCF file

modify my launch file to this one:

<launch>
    <arg name="urdf_file" default="$(find xacro)/xacro '$(find hexapodo_description)/robots/hexapodo.robot.xacro' --inorder" />
    <param name="robot_description" command="$(arg urdf_file)" />
    <node ns="hexapodo" name="driver" pkg="canopen_motor_node" type="canopen_motor_node" output="screen" clear_params="true" launch-prefix="">
        <rosparam command="load" file="$(find canopen_chain_node)/config/canopen.yaml" />
    </node>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen">
    </node>
    <!-- Combine joint values -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
    <!-- Show in Rviz   -->
    <node name="rviz" pkg="rviz" type="rviz"/>
</launch>

The launch spawn everything correctly, but when I run:

rosservice call /hexapodo/driver/init

I get the next error:

success: False
message: "/home/jorge/WS/epos_ws/src/ros_canopen/canopen_master/src/pdo.cpp(359): Throw in\
  \ function void canopen::PDOMapper::Buffer::read(const canopen::ObjectDict::Entry&,\
  \ canopen::String&)\nDynamic exception type: boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<std::bad_cast>\
  \ >\nstd::exception::what: std::bad_cast\n[canopen::tag_objectdict_key*] = 6061sub0\n"

I'm reading about this error, it seems that Maxon use a diferent direction like you explain here.

But, in my case, I have the same error even changing the operation mode:

• 6 Homing Mode
• 3 Profile Velocity Mode
• 1 Profile Position Mode
• -1 Position Mode
• -2 Velocity Mode
• -3 Current Mode
• -4 Diagnostic Mode
• -5 MasterEncoder Mode
• -6 Step/Direction Mode

[mathias-luedtke]

Your EDS/DCF might list the wrong type for 6061, please validate it with CANchkEDS.

[grafoteka]

@ipa-mdl, I have to download this program vektor canEds

Once installed, I execute Vector CANeds and run the check file with my DCF file.

I have a lot of errors and warnings, but also, 3 Unkown elements.

So, what I think is that I should modify the element 60C1sub1 to try to delete the error.

But, what do i have to modify? the name or some values?

Many thanks,
Jorge

[grafoteka]

Hi all,
I 've been working with CANeds, and now I only have two erros:

• ToolInfos:
  
• 202E
  

But even CANeds doesn't show any error with 60C1sub1, when I try to init the motors I continue having the error with 6061sub0.

success: False
message: "/home/jorge/WS/epos_ws/src/ros_canopen/canopen_master/src/pdo.cpp(359): Throw in\
  \ function void canopen::PDOMapper::Buffer::read(const canopen::ObjectDict::Entry&,\
  \ canopen::String&)\nDynamic exception type: boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<std::bad_cast>\
  \ >\nstd::exception::what: std::bad_cast\n[canopen::tag_objectdict_key*] = 6061sub0\n"

I'm trying to follow this answer and this one

[martinlucan]

Hello there, does anyone knows what following error message means?
EMCY received: 81#0000000000000000

Unfortunately, I do get the same message.

[mathias-luedtke]

EMCY received: 81#0000000000000000

This just tells you that all errors have been reset.

#397 change the log level, but it hasn't been released yet (should do this!).

[zouzhe1]

@grafoteka hello, i think you dont noobs.
i want to ask some problem

1. yaml and launch file need write by myslef ? where path to put
2. ros_control and ros_controller need ? wiki no say