search

ICube-Robotics / ethercat_driver_ros2

Hardware Interface for EtherCAT module integration with ros2_control
https://icube-robotics.github.io/ethercat_driver_ros2/
Apache License 2.0
126 stars 32 forks source link

ethercat_driver_ros2 and ros2_control implementation #47

Closed NamanMo97 closed 1 year ago

NamanMo97 commented 1 year ago

I'm trying to implement an integration between ros2_control architecture and ethercat_driver_ros2 in order to control at the beginning a single actuator and run a BLDC motor. I want your instructions please on what to do next in order to use ros2_control to send position commands to the driver.

I'm using ros2_control_demos package to start my integration specifically (rrbot_modular_actuators.launch.py) example_6, and I haven't add any modification to the launch files.

Inside the ros2_control description file rrbot_modular_actuators.ros2_control.xacro I added the Hardware Interfaces and EtherCAT Master and EtherCAT Slave modules as Plugins with GPIO controller interface so my rrbot_modular_actuators.ros2_control.xacro now looks like this:


<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="rrbot_modular_actuators" params="name prefix slowdown:=2.0">

    <ros2_control name="mySystem" type="system">
      <hardware>
        <plugin>ethercat_driver/EthercatDriver</plugin>
        <param name="master_id">0</param>
        <param name="control_frequency">100</param>
      </hardware>

    <gpio name="myGPIO">
      <command_interface name="digital_out_1"/>
      <command_interface name="digital_out_2"/>
      <state_interface name="digital_out_1"/>
      <state_interface name="digital_out_2"/>
      <state_interface name="ana_input.1"/>
      <state_interface name="ana_input.2"/>

      <ec_module name="EL3104">
        <plugin>ethercat_plugins/Beckhoff_EL3104</plugin>
        <param name="alias">0</param>
        <param name="position">1</param>
        <param name="ai.1">ana_input.1</param>
        <param name="ai.4">ana_input.2</param>
      </ec_module>
      <ec_module name="EL2124">
        <plugin>ethercat_plugins/Beckhoff_EL2124</plugin>
        <param name="alias">0</param>
        <param name="position">5</param>
        <param name="do.4">digital_out_2</param>
        <param name="do.6">dig_output.1</param>

      </ec_module>
    </gpio>

    </ros2_control>

    <ros2_control name="RRBotModularJoint1" type="actuator">
      <hardware>
        <plugin>ros2_control_demo_example_6/RRBotModularJoint</plugin>
        <param name="example_param_hw_start_duration_sec">2.0</param>
        <param name="example_param_hw_stop_duration_sec">3.0</param>
        <param name="example_param_hw_slowdown">${slowdown}</param>
      </hardware>
      <joint name="${prefix}joint1">
        <command_interface name="position">
          <param name="min">-100</param>
          <param name="max">100</param>
        </command_interface>
        <state_interface name="position"/>
      </joint>
    </ros2_control>
    <ros2_control name="RRBotModularJoint2" type="actuator">
      <hardware>
        <plugin>ros2_control_demo_example_6/RRBotModularJoint</plugin>
        <param name="example_param_hw_start_duration_sec">2.0</param>
        <param name="example_param_hw_stop_duration_sec">3.0</param>
        <param name="example_param_hw_slowdown">${slowdown}</param>
      </hardware>
      <joint name="${prefix}joint2">
        <command_interface name="position">
          <param name="min">-100</param>
          <param name="max">100</param>
        </command_interface>
        <state_interface name="position"/>
      </joint>
    </ros2_control>
  </xacro:macro>

</robot>

I also added the gpio_command_controller inside rrbot_modular_actuators.yaml in order to send command to gpio resources:

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    forward_position_controller:
      type: forward_command_controller/ForwardCommandController

    gpio_command_controller:
      type: gpio_controllers/GpioCommandController

gpio_command_controller:
  ros__parameters:
    gpios:
      - myGPIO
    interface_name: gbio
    command_interfaces:
      myGPIO:
        - dig_output.1
        - dig_output.2
    state_interfaces:
      myGPIO:
        - ana_input.1

forward_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    interface_name: position

Running the launch file I get:

[robot_state_publisher-2] [INFO] [1679052481.567923792] [kdl_parser]: Link base_link had 1 children.
[robot_state_publisher-2] [INFO] [1679052481.568000545] [kdl_parser]: Link link1 had 1 children.
[robot_state_publisher-2] [INFO] [1679052481.568020270] [kdl_parser]: Link link2 had 0 children.
[robot_state_publisher-2] [INFO] [1679052481.568040984] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1679052481.568106468] [robot_state_publisher]: got segment link1
[robot_state_publisher-2] [INFO] [1679052481.568118603] [robot_state_publisher]: got segment link2
[robot_state_publisher-2] [INFO] [1679052481.568127149] [robot_state_publisher]: got segment world
[ros2_control_node-1] [INFO] [1679052481.593316285] [resource_manager]: Loading hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679052481.595581411] [resource_manager]: Initialize hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679052481.595816370] [EthercatDriver]: gpios

[ros2_control_node-1] [INFO] [1679052481.597600894] [EthercatDriver]: Got 2 modules
[ros2_control_node-1] [INFO] [1679052481.597622571] [resource_manager]: Successful initialization of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679052481.597957013] [resource_manager]: Loading hardware 'RRBotModularJoint1' 
[ros2_control_node-1] [INFO] [1679052481.598631113] [resource_manager]: Initialize hardware 'RRBotModularJoint1' 
[ros2_control_node-1] [INFO] [1679052481.598809211] [resource_manager]: Successful initialization of hardware 'RRBotModularJoint1'
[ros2_control_node-1] [INFO] [1679052481.598911572] [resource_manager]: Loading hardware 'RRBotModularJoint2' 
[ros2_control_node-1] [INFO] [1679052481.598971285] [resource_manager]: Initialize hardware 'RRBotModularJoint2' 
[ros2_control_node-1] [INFO] [1679052481.599001802] [resource_manager]: Successful initialization of hardware 'RRBotModularJoint2'
[ros2_control_node-1] [INFO] [1679052481.599223583] [resource_manager]: 'configure' hardware 'RRBotModularJoint1' 
[ros2_control_node-1] [INFO] [1679052481.599260287] [resource_manager]: Successful 'configure' of hardware 'RRBotModularJoint1'
[ros2_control_node-1] [INFO] [1679052481.599279349] [resource_manager]: 'activate' hardware 'RRBotModularJoint1' 
[ros2_control_node-1] [INFO] [1679052481.599297878] [RRBotModularJoint]: Activating ...please wait...
[ros2_control_node-1] [INFO] [1679052482.599672588] [RRBotModularJoint]: 2.0 seconds left...
[ros2_control_node-1] [INFO] [1679052483.599894492] [RRBotModularJoint]: 1.0 seconds left...
[ros2_control_node-1] [INFO] [1679052483.599980622] [RRBotModularJoint]: Successfully activated!
[ros2_control_node-1] [INFO] [1679052483.600021359] [resource_manager]: Successful 'activate' of hardware 'RRBotModularJoint1'
[ros2_control_node-1] [INFO] [1679052483.600146213] [resource_manager]: 'configure' hardware 'RRBotModularJoint2' 
[ros2_control_node-1] [INFO] [1679052483.600180487] [resource_manager]: Successful 'configure' of hardware 'RRBotModularJoint2'
[ros2_control_node-1] [INFO] [1679052483.600223010] [resource_manager]: 'activate' hardware 'RRBotModularJoint2' 
[ros2_control_node-1] [INFO] [1679052483.600247927] [RRBotModularJoint]: Activating ...please wait...
[spawner-3] [INFO] [1679052483.779430554] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[ros2_control_node-1] [INFO] [1679052484.600523902] [RRBotModularJoint]: 2.0 seconds left...
[ros2_control_node-1] [INFO] [1679052485.600762584] [RRBotModularJoint]: 1.0 seconds left...
[ros2_control_node-1] [INFO] [1679052485.600833169] [RRBotModularJoint]: Successfully activated!
[ros2_control_node-1] [INFO] [1679052485.600869368] [resource_manager]: Successful 'activate' of hardware 'RRBotModularJoint2'
[ros2_control_node-1] [INFO] [1679052485.601090822] [resource_manager]: 'configure' hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679052485.601148087] [resource_manager]: Successful 'configure' of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679052485.601198399] [resource_manager]: 'activate' hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679052485.601225225] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-1] {0, 1, 0x2, 0xc203052, 0x6000, 0x11}
[ros2_control_node-1] {0, 1, 0x2, 0xc203052, 0x6010, 0x11}
[ros2_control_node-1] {0, 1, 0x2, 0xc203052, 0x6020, 0x11}
[ros2_control_node-1] {0, 1, 0x2, 0xc203052, 0x6030, 0x11}
[ros2_control_node-1] {0, 5, 0x2, 0xaf93052, 0x7000, 0x1}
[ros2_control_node-1] [INFO] [1679052485.603528164] [EthercatDriver]: Activated EcMaster!
[spawner-3] [INFO] [1679052485.803220450] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[ros2_control_node-1] [INFO] [1679052486.603911103] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1679052486.604015554] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1679052486.604064336] [resource_manager]: Successful 'activate' of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679052486.615295271] [controller_manager]: update rate is 100 Hz
[ros2_control_node-1] [INFO] [1679052486.615506738] [controller_manager]: RT kernel is recommended for better performance

[ros2_control_node-1] 6 slave(s).
[ros2_control_node-1] Master AL states: 0x02.
[ros2_control_node-1] Link is up.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Domain: WC 1.
[ros2_control_node-1] Domain: State 1.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] Slave: State 0x01.
[ros2_control_node-1] Domain: WC 3.
[ros2_control_node-1] Domain: State 2.
[ros2_control_node-1] Slave: State 0x04.
[ros2_control_node-1] Master AL states: 0x0A.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.

[ros2_control_node-1] [INFO] [1679052486.615568292] [RRBotModularJoint]: Reading...
[ros2_control_node-1] [INFO] [1679052486.615579178] [RRBotModularJoint]: Got state 0.00000 for joint 'joint1'!
[ros2_control_node-1] [INFO] [1679052486.615587202] [RRBotModularJoint]: Joints successfully read!
[ros2_control_node-1] [INFO] [1679052486.615593049] [RRBotModularJoint]: Reading...
[ros2_control_node-1] [INFO] [1679052486.615597918] [RRBotModularJoint]: Got state 0.00000 for joint 'joint2'!
[ros2_control_node-1] [INFO] [1679052486.615603144] [RRBotModularJoint]: Joints successfully read!
[ros2_control_node-1] [INFO] [1679052486.615652516] [RRBotModularJoint]: Writing...please wait...

Checking for slaves operation mode

neuermann@NeuerMann:~/neuer_ws$ ethercat slaves
0  0:0  OP     +  EK1100 EtherCAT-Koppler (2A E-Bus)
1  0:1  OP     +  EL3104 4Ch. Ana. Input +/-10V Diff.
2  0:2  PREOP  +  EL3104 4Ch. Ana. Input +/-10V Diff.
3  0:3  PREOP  +  EL3008 8K. Ana. Eingang  +/-10V
4  0:4  PREOP  +  EL1809 16K. Dig. Eingang 24V, 3ms
5  0:5  OP     +  EL2809 16K. Dig. Ausgang 24V, 0.5A

Checking controllers and hardware interfaces

neuermann@NeuerMann:~/neuer_ws$ ros2 control list_controllers
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
forward_position_controller[forward_command_controller/ForwardCommandController] active    
gpio_command_controller[gpio_controllers/GpioCommandController] active    

neuermann@NeuerMann:~/neuer_ws$ ros2 control list_hardware_interfaces 
command interfaces
    joint1/position [available] [claimed]
    joint2/position [available] [claimed]
    myGPIO/dig_output.1 [available] [claimed]
    myGPIO/dig_output.2 [available] [claimed]
state interfaces
    joint1/position
    joint2/position
    myGPIO/ana_input.1
    myGPIO/ana_input.2
    myGPIO/dig_output.1
    myGPIO/dig_output.2
neuermann@NeuerMann:~/neuer_ws$
mcbed commented 1 year ago

I have some hard time understanding what you plan to do. At this point you have 2 gpio modules set up and you can control the digital outputs with the controller. But joint1and joint2 are demo joints so using the demo hardware.
At some point you need to connect your BLDC motor to some ethercat hardware and configure it using something like 

<joint name="myJoint">
  <command_interface name="xxx"/>
  <state_interface name="xxx"/>
  <ec_module name="ECModule">
    <plugin>ethercat_plugins/ECModule</plugin>
    <param name="alias">xx</param>
    <param name="position">xx</param>
  </ec_module>
</joint>
mcbed commented 1 year ago

@NamanMo97, you can have a look at #51 for an example using a Maxon motor drive.

NamanMo97 commented 1 year ago

@mcbed Hello, Thank you for your time and help, I will check the new package and example. Meanwhile I added an Elmo driver based on the implementation of SimplECAT to ethercat_plugins and set it up and configured. What should I do now in order to send position or velocity commands to the motor and make it work?


<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="rrbot_system_multi_interface" params="name prefix use_mock_hardware:=^|false mock_sensor_commands:=^|false slowdown:=2.0">

    <ros2_control name="mySystem" type="system">
      <hardware>
        <plugin>ethercat_driver/EthercatDriver</plugin>
        <param name="master_id">0</param>
        <param name="control_frequency">100</param>
      </hardware>

     <joint name="myJoint">
        <state_interface name="position"/>
        <state_interface name="velocity"/>
        <command_interface name="position"/>
        <command_interface name="velocity"/>
        <ec_module name="Elmo_GoldWhistle">
          <plugin>ethercat_plugins/Elmo_GoldWhistle</plugin>
          <param name="alias">0</param>
          <param name="position">0</param>
        </ec_module>
      </joint>
    </ros2_control>

  </xacro:macro>

</robot>

neuermann@NeuerMann:~/neuer_ws$ ros2 launch ros2_control_demo_example_3 rrbot_system_multi_interface.launch.py 
[INFO] [launch]: All log files can be found below /home/neuermann/.ros/log/2023-03-28-09-05-53-218628-NeuerMann-5647
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [5650]
[INFO] [robot_state_publisher-2]: process started with pid [5652]
[INFO] [spawner-3]: process started with pid [5654]
[robot_state_publisher-2] [INFO] [1679987153.669931042] [kdl_parser]: Link base_link had 1 children.
[robot_state_publisher-2] [INFO] [1679987153.670010951] [kdl_parser]: Link link1 had 1 children.
[robot_state_publisher-2] [INFO] [1679987153.670618833] [kdl_parser]: Link link2 had 0 children.
[robot_state_publisher-2] [INFO] [1679987153.670647427] [robot_state_publisher]: got segment base_link
[robot_state_publisher-2] [INFO] [1679987153.670708167] [robot_state_publisher]: got segment link1
[robot_state_publisher-2] [INFO] [1679987153.670719923] [robot_state_publisher]: got segment link2
[robot_state_publisher-2] [INFO] [1679987153.670744047] [robot_state_publisher]: got segment world
[ros2_control_node-1] [INFO] [1679987153.698994986] [resource_manager]: Loading hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679987153.711068677] [resource_manager]: Initialize hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679987153.712278209] [EthercatDriver]: joints
[ros2_control_node-1] [INFO] [1679987153.718911094] [EthercatDriver]: Got 1 modules
[ros2_control_node-1] [INFO] [1679987153.718934380] [resource_manager]: Successful initialization of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679987153.719388593] [resource_manager]: 'configure' hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679987153.719413374] [resource_manager]: Successful 'configure' of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679987153.719425791] [resource_manager]: 'activate' hardware 'mySystem' 
[ros2_control_node-1] [INFO] [1679987153.719431149] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x607a, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x60ff, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6071, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6072, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6040, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6060, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x20a0, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x2205, 0x1}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6064, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6077, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6041, 0x0}
[ros2_control_node-1] {0, 0, 0x9a, 0x30924, 0x6061, 0x0}
[ros2_control_node-1] [INFO] [1679987153.720057439] [EthercatDriver]: Activated EcMaster!
[ros2_control_node-1] 1 slave(s).
[ros2_control_node-1] Master AL states: 0x02.
[ros2_control_node-1] Link is up.
[ros2_control_node-1] Slave: State 0x02.
[ros2_control_node-1] Slave: online.
[ros2_control_node-1] STATE: Not Ready to Switch On
[ros2_control_node-1] [INFO] [1679987154.720689780] [EthercatDriver]: updated!
[ros2_control_node-1] [INFO] [1679987154.720781272] [EthercatDriver]: System Successfully started!
[ros2_control_node-1] [INFO] [1679987154.720821178] [resource_manager]: Successful 'activate' of hardware 'mySystem'
[ros2_control_node-1] [INFO] [1679987154.740478906] [controller_manager]: update rate is 100 Hz
[ros2_control_node-1] [INFO] [1679987154.740711754] [controller_manager]: RT kernel is recommended for better performance
[ros2_control_node-1] [INFO] [1679987154.833724356] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1679987154.851765866] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1679987154.852792004] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1679987154.852976915] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1679987154.881891490] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-3]: process has finished cleanly [pid 5654]
[INFO] [spawner-4]: process started with pid [5700]
[ros2_control_node-1] [INFO] [1679987155.230913909] [controller_manager]: Loading controller 'forward_velocity_controller'
[spawner-4] [INFO] [1679987155.278162614] [spawner_forward_velocity_controller]: Loaded forward_velocity_controller
[ros2_control_node-1] [INFO] [1679987155.279319824] [controller_manager]: Configuring controller 'forward_velocity_controller'
[ros2_control_node-1] [INFO] [1679987155.283580430] [forward_velocity_controller]: configure successful
[ros2_control_node-1] [INFO] [1679987155.291138315] [forward_velocity_controller]: activate successful
[spawner-4] [INFO] [1679987155.301738321] [spawner_forward_velocity_controller]: Configured and activated forward_velocity_controller
[INFO] [spawner-4]: process has finished cleanly [pid 5700]
[ros2_control_node-1] Domain: WC 1.
[ros2_control_node-1] Domain: State 1.
[ros2_control_node-1] STATE: Switch on Disabled
[ros2_control_node-1] Domain: WC 3.
[ros2_control_node-1] Domain: State 2.
[ros2_control_node-1] STATE: Ready to Switch On
[ros2_control_node-1] Master AL states: 0x08.
[ros2_control_node-1] Slave: State 0x08.
[ros2_control_node-1] Slave: operational.
[ros2_control_node-1] STATE: Switch On
[ros2_control_node-1] STATE: Operation Enabled
[ros2_control_node-1] [INFO] [1679987214.925400783] [controller_manager]: Loading controller 'forward_position_controller'
[ros2_control_node-1] [INFO] [1679987230.383485075] [controller_manager]: Configuring controller 'forward_position_controller'
[ros2_control_node-1] [INFO] [1679987230.385283519] [forward_position_controller]: configure successful
[ros2_control_node-1] [INFO] [1679987235.621231226] [forward_position_controller]: activate successful
mcbed commented 1 year ago

@NamanMo97, if you did your plugin correctly, you should be able to send commands with a position or trajectory controller from ros2_control and your drive should be able to move.