Avoid moving to zero position on hardware activate

[ggorjup]

Hello, this PR addresses the issue of robot moving to zero position on hardware interface activation, as described in #44.

To resolve it, this PR assigns the current state read from EGM to the joint commands in the on_activate callback (see diff).

I am aware that a PR addressing this is already open (#45), but the proposed solution:

• Does not require changes in the abb_egm_rws_managers repository.
• Does not require additional configuration in the description files (xacro).
• Is robust to external motion commands that bypass EGM (deactivating ROS2 controller, moving robot with pendant, activating ROS2 controller again).

This was tested in RobotStudio, using the IRB1200_5_90.rspag project and RobotWare 6.16.

[Yadunund]

This is a more elegant solution without relying on changes to upstream libraries. Thanks for PR!

[gavanderhoorn]

It's also arguably the correct way to fix this :)

[ardimou]

This requires the service start_egm_joint to be active before launching the controllers? If yes, maybe getting the initial joints from rws using the update function would be preferable? (to avoid using start_egm_joint before the launch)

[ggorjup]

Hi @ardimou, yes - this requires EGM to be active when starting the hardware interface.
The new addition is in the on_activate callback, which (I think) assumes EGM to be active anyway, since it queries the EGM manager for connection before doing anything else.

Looking at the code block before the new addition, hardware activation should fail if EGM is not connected in a given number of retries:

  size_t counter = 0;
  RCLCPP_INFO(LOGGER, "Connecting to robot...");
  while (rclcpp::ok() && ++counter < NUM_CONNECTION_TRIES)
  {
    // Wait for a message on any of the configured EGM channels.
    if (egm_manager_->waitForMessage(500))
    {
      RCLCPP_INFO(LOGGER, "Connected to robot");
      break;
    }

    RCLCPP_INFO(LOGGER, "Not connected to robot...");
    if (counter == NUM_CONNECTION_TRIES)
    {
      RCLCPP_ERROR(LOGGER, "Failed to connect to robot");
      return CallbackReturn::ERROR;
    }
    rclcpp::sleep_for(500ms);
  }

However, it never gets to the ERROR because the while loop exits before counter == NUM_CONNECTION_TRIES, so the HW interface still reports a successful connection after timeout (I just realized this now when testing).

So if the above bug wasn't present (the fix is easy so I guess it won't be much longer), the robot hardware would fail to activate without EGM active. Which I think makes sense, since "active" hardware should be ready to read state and write commands (which requires EGM).

In addition, the start_egm_joint service comes with a couple of assumptions:

• RWS manager is supported: This is currently not true for RobotWare > 7.0 (Omnicore).
• The state machine add-in is used on the robot controller: This is useful, but not necessary (see example .mod files in this repo).

Finally, a couple of suggestions to get around your issue:

• The hardware interface can potentially be initialized as inactive by setting the hardware_components_initial_state parameter. Then, once EGM is activated (one way or another), the HW interface can be set to active manually.
• Hardware interface initialization can be delayed in the launch file until EGM is activated through event handlers. For example, an OnExecutionComplete handler could be used to launch ABB HW interface after the start_egm_joint service.

The above is just what I think makes sense - I'm happy to discuss alternatives though.