search

ros-industrial / ros_canopen

CANopen driver framework for ROS (http://wiki.ros.org/ros_canopen)
GNU Lesser General Public License v3.0
345 stars 275 forks source link

CANopen for noobs - How to begin? #283

Open grafoteka opened 6 years ago

grafoteka commented 6 years ago

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 image
  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 
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 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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

Old one image New one image

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:

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 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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:

mathias-luedtke commented 6 years ago

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

grafoteka commented 6 years ago

@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.

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 commented 6 years ago

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

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

ciniminis1 commented 3 years ago

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

Unfortunately, I do get the same message.

mathias-luedtke commented 3 years ago

EMCY received: 81#0000000000000000

This just tells you that all errors have been reset.

https://github.com/ros-industrial/ros_canopen/pull/397 change the log level, but it hasn't been released yet (should do this!).

zouzhe1 commented 2 years ago

@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