Remove the '/' from tf_prefix when using the namespace of diff_drive_controller as tf_prefix

[kjvanteeffelen]

I am using a Gazebo simulated diff drive robot with the ros2_control diff_drive_controller. In some cases, I want to spawn multiple robots and therefor I would like to use namespacing, including a tf_prefix. So my frames look like

• Without namespacing: base_link and odom
• With namespacing: robot_name/base_link and robot_name/odom

I could set the tf_frame_prefix argument to my robot name, but I want to be flexible in the name of my robot. Unfortunately, using Gazebo it is hard to be flexible in the parameters of the ros2_controller as you need to load in a yaml file containing all controller parameters in the ros2_control plugin in the URDF. Luckily, the diff_drive_controller also has the option to use the namespace of the node. However, then I get my frames as: /robot_name/base_link and /robot_name/odom.

Starting the frames with / is not common in TF and results into problems with for instance robot_localization.
Currently it's in the code like this:

  // Append the tf prefix if there is one
  std::string tf_prefix = "";
  if (params_.tf_frame_prefix_enable)
  {
    if (params_.tf_frame_prefix != "")
    {
      tf_prefix = params_.tf_frame_prefix;
    }
    else
    {
      tf_prefix = std::string(get_node()->get_namespace());
    }

    if (tf_prefix == "/")
    {
      tf_prefix = "";
    }
    else
    {
      tf_prefix = tf_prefix + "/";
    }
  }

I think a simple fix could be added, where after the get_namespace command, the initial / is removed.
As can be seen, in case tf_prefix == "/", the / is actually removed. I think this should be done as well in case the node's namespace is used.

[christophfroehlich]

This sounds reasonable. Could you create a PR with your fix?

[kjvanteeffelen]

Yes, I was on holiday last week so will do that this week.

[kjvanteeffelen]

I opened a PR for the humble branch (#1059) but got notified I should do this for master. So please see the fix in #1060.