Conflict in Controlling Multiple Robotic Arms Simultaneously in ROS2 Control Using SOEM Due to Shared Global Variables

[JakeFishcode]

ROS2 Controls is very elegant and convenient, and I’ve deployed it on multiple robots. However, as I gradually add more hardware, I’m encountering some programming issues.

Currently, I’m facing a very tricky problem, but I haven’t yet pinpointed the exact cause. Here’s the situation: I have two robotic arms mounted on a robot, and I’m using an EtherCAT bus to control both arms. When my hardware driver is implemented using SOEM, a C language library that heavily relies on global variables, I encounter issues. If I use the driver for just a single arm and describe it like this:

      <ros2_control name="mcr_right_arm_control" type="system">
        <hardware>
          <plugin>moying_mcr_hardware/ArmHardwareInterface</plugin>
          <param name="config_path">moying_mcr_bringup</param>
          <param name="config_file">/config/right_arm_driver.yaml</param>
        </hardware>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="1"/>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="2"/>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="3"/>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="4"/>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="5"/>
        <xacro:prefix_elfin_joint_transmission headprefix="right_arm" prefix="6"/>
      </ros2_control>
      <ros2_control name="mcr_left_arm_control" type="system">
        <hardware>
          <plugin>moying_mcr_hardware/ArmHardwareInterface</plugin>
          <param name="config_path">moying_mcr_bringup</param>
          <param name="config_file">/config/left_arm_driver.yaml</param>
        </hardware>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="1"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="2"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="3"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="4"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="5"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="6"/>
      </ros2_control>

This setup causes problems: when one arm is successfully started, attempting to start the other arm results in an issue. However, if I start each arm individually, as shown below, there are no problems:

      <ros2_control name="mcr_left_arm_control" type="system">
        <hardware>
          <plugin>moying_mcr_hardware/ArmHardwareInterface</plugin>
          <param name="config_path">moying_mcr_bringup</param>
          <param name="config_file">/config/left_arm_driver.yaml</param>
        </hardware>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="1"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="2"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="3"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="4"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="5"/>
        <xacro:prefix_elfin_joint_transmission headprefix="left_arm" prefix="6"/>
      </ros2_control>

This leads me to suspect that ROS2 Control’s way of invoking the hardware might be using threads to call the two Hardware Interfaces, resulting in the C language sharing the same memory space, such as global variables.

On the other hand, I encountered a similar issue in ROS Control (ROS1), but when I used namespaces to describe the setup, the problem of not being able to start both arms simultaneously did not occur:

  <group ns="left">
    <node name="elfin_control" pkg="elfin_ros_control" type="elfin_hardware_interface" output="screen">
      <rosparam file="$(find mcr_bringup)/config/left_elfin_drivers.yaml" command="load"/>
    </node>

    <rosparam file="$(find elfin_robot_bringup)/config/joint_state_controller.yaml" command="load"/>
    <node name="elfin_joint_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>

    <rosparam file="$(find mcr_bringup)/config/left_elfin_arm_control.yaml" command="load"/>
    <node name="elfin_arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="load elfin_arm_controller" respawn="false" output="screen"/>
  </group>
  <group ns="right">
    <node name="elfin_control" pkg="elfin_ros_control" type="elfin_hardware_interface" output="screen">
      <rosparam file="$(find mcr_bringup)/config/right_elfin_drivers.yaml" command="load"/>
    </node>

    <rosparam file="$(find elfin_robot_bringup)/config/joint_state_controller.yaml" command="load"/>
    <node name="elfin_joint_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>

    <rosparam file="$(find mcr_bringup)/config/right_elfin_arm_control.yaml" command="load"/>
    <node name="elfin_arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="load elfin_arm_controller" respawn="false" output="screen"/>
  </group>

Since I want to achieve whole-body control and need to invoke information from both arms to integrate into a single controller, I’m wondering if there’s something similar to namespaces in the Hardware Interface that can isolate the C language driver code, such as SOEM.

Of course, I’m still verifying my hypothesis, and I’ll supplement with actual error screenshots and code snippets as I proceed.

example SOEM:

bool EtherCatManager::initSoem(const std::string& ifname) {
  // Copy string contents because SOEM library doesn't 
  // practice const correctness
  const static unsigned MAX_BUFF_SIZE = 1024;
  char buffer[MAX_BUFF_SIZE];
  size_t name_size = ifname_.size();
  if (name_size > sizeof(buffer) - 1) 
  {
    fprintf(stderr, "Ifname %s exceeds maximum size of %u bytes\n", ifname_.c_str(), MAX_BUFF_SIZE);
    return false;
  }
  std::strncpy(buffer, ifname_.c_str(), MAX_BUFF_SIZE);

  printf("Initializing etherCAT master\n");

  if (!ec_init(buffer))
  {
    fprintf(stderr, "Could not initialize ethercat driver\n");
    return false;
  }

  /* find and auto-config slaves */
  if (ec_config_init(FALSE) <= 0)
  {
    fprintf(stderr, "No slaves are found on %s\n", ifname_.c_str());
    return false;
  }

  printf("SOEM found and configured %d slaves\n", ec_slavecount);

  if (ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE*4) != EC_STATE_PRE_OP)
  {
    fprintf(stderr, "Could not set EC_STATE_PRE_OP\n");
    return false;
  }

  // configure IOMap
  int iomap_size = ec_config_map(iomap_);
  printf("SOEM IOMap size: %d\n", iomap_size);

  // locates dc slaves - ???
  ec_configdc();

  // '0' here addresses all slaves
  if (ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE*4) != EC_STATE_SAFE_OP)
  {
    fprintf(stderr, "Could not set EC_STATE_SAFE_OP\n");
    return false;
  }

  /* 
      This section attempts to bring all slaves to operational status. It does so
      by attempting to set the status of all slaves (ec_slave[0]) to operational,
      then proceeding through 40 send/recieve cycles each waiting up to 50 ms for a
      response about the status. 
  */
  ec_slave[0].state = EC_STATE_OPERATIONAL;
  ec_send_processdata();
  ec_receive_processdata(EC_TIMEOUTRET);

  ec_writestate(0);
  int chk = 40;
  do {
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); // 50 ms wait for state check
  } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

  if(ec_statecheck(0,EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE) != EC_STATE_OPERATIONAL)
  {
    fprintf(stderr, "OPERATIONAL state not set, exiting\n");
    return false;
  }

  ec_readstate();

  printf("\nFinished configuration successfully\n");
  return true;
}

[JakeFishcode]

I am certain that the problem is caused by C++ calling global variables in the C code. Currently, I solved the issue by creating local copies of the SOEM library (SOEM2, SOEM3) with renamed global variables. However, I still feel that this solution is neither convenient nor elegant.

[saikishor]

I am certain that the problem is caused by C++ calling global variables in the C code. Currently, I solved the issue by creating local copies of the SOEM library (SOEM2, SOEM3) with renamed global variables. However, I still feel that this solution is neither convenient nor elegant.

Hello @JakeFishcode !

Could you please show us how the global variables are defined in the SOEM ?

[JakeFishcode]

#include "soem2/ethercat.h"
// .... .....
// .... .....
// .... .....
bool EtherCatManager::initSoem(const std::string& ifname) {
  // Copy string contents because SOEM library doesn't 
  // practice const correctness
  const static unsigned MAX_BUFF_SIZE = 1024;
  char buffer[MAX_BUFF_SIZE];
  size_t name_size = ifname_.size();
  if (name_size > sizeof(buffer) - 1) 
  {
    fprintf(stderr, "Ifname %s exceeds maximum size of %u bytes\n", ifname_.c_str(), MAX_BUFF_SIZE);
    return false;
  }
  std::strncpy(buffer, ifname_.c_str(), MAX_BUFF_SIZE);

  printf("Initializing etherCAT master\n");

  if (!ec_init(buffer))
  {
    fprintf(stderr, "Could not initialize ethercat driver\n");
    return false;
  }

  /* find and auto-config slaves */
  if (ec_config_init(FALSE) <= 0)
  {
    fprintf(stderr, "No slaves are found on %s\n", ifname_.c_str());
    return false;
  }

  printf("SOEM found and configured %d slaves\n", ec_slavecount);

  if (ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE*4) != EC_STATE_PRE_OP)
  {
    fprintf(stderr, "Could not set EC_STATE_PRE_OP\n");
    return false;
  }

  // configure IOMap
  int iomap_size = ec_config_map(iomap_);
  printf("SOEM IOMap size: %d\n", iomap_size);

  // locates dc slaves - ???
  ec_configdc();

  // '0' here addresses all slaves
  if (ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE*4) != EC_STATE_SAFE_OP)
  {
    fprintf(stderr, "Could not set EC_STATE_SAFE_OP\n");
    return false;
  }

  /* 
      This section attempts to bring all slaves to operational status. It does so
      by attempting to set the status of all slaves (ec_slave[0]) to operational,
      then proceeding through 40 send/recieve cycles each waiting up to 50 ms for a
      response about the status. 
  */
    ec_slave[0].state = EC_STATE_OPERATIONAL;
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);

    ec_writestate(0);
    int chk = 40;
    do {
      ec_send_processdata();
      ec_receive_processdata(EC_TIMEOUTRET);
      ec_statecheck(0, EC_STATE_OPERATIONAL, 50000); // 50 ms wait for state check
    } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

    if(ec_statecheck(0,EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE) != EC_STATE_OPERATIONAL)
    {
      fprintf(stderr, "OPERATIONAL state not set, exiting\n");
      return false;
    }

  ec_readstate();

  printf("\nFinished configuration successfully\n");
  return true;
}

I open the ethercat bus like this ec_init(buffer), all slave info save in ec_slave ,when open another ethercat bus.This first one will be broken. SOEM is a hardware driver for ehtercat using C . It's low latency and common in robot hardware.

[bmagyar]

If you are working with globals, you can only have one "instance" of that library. Is there a way around that with C? Theoretically separate modules like plugins should 🤔 do that job though...

[JakeFishcode]

Plugins should indeed operate independently, which can facilitate the integration of hardware. However, I am currently unclear about the specific reasons for this. I resolved the issue by manually "instantiating" multiple instances of the SOEM library, and I think this is an area worth exploring further. This is because, in most cases, when using EtherCAT buses, the SOEM library is commonly employed. I will continue to update with new findings as I make progress.

[JakeFishcode]

When I use the Robotiq FT300, which communicates via an RS-485 bus, having more than two devices causes conflicts when setting up two hardware plugins. This issue stems from global variables in the C++ calls to the C driver. It seems there may not be another way to avoid this, so I am considering rebuilding a C library to selectively manage the calls. like this :

<xacro:robotiq_fts_ros2_control name="left_arm_" ftdi_id="ttyUSB3" tf_prefix="left_arm_" read_rate='100'/> 
<xacro:robotiq_fts_zombie_ros2_control name="right_arm_" ftdi_id="ttyUSB0" tf_prefix="right_arm_" read_rate='100'/>