Remove the '/' from tf_prefix when using the namespace of diff_drive_controller as tf_prefix #1048
Description
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_linkandodom - With namespacing:
robot_name/base_linkandrobot_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.