ros2_control - Created node by ControllerInterface gets nodename of controller_manager no matter what (diff_drive_controller)

edit

Hi!

I started to work with ros2_control and want to set it up with a diff_drive_controller. Therefore I wrote an Actuator that implements the handling of my motors. The issue I now have is that when loading the controller, the new created Node is getting the name of controller_manager, not the passed controller_name. Therefore I have a) two nodes with the same name and b) the defined parameters for the diffdrive_controller can not be loaded because the a node with that name is does not exist (caused by a) :) )

To find the cause of that issue I step by step cloned the ros2_controllers and then the ros2_control github repos into my workspace to be able to at least add some debugmessages.

So my setup is now the following:

• ros:foxy docker image with updated packages (apt update && apt upgrade)
• /ros2_ws/src contains { ros2_controllers, ros2_control, myrobot_control, myrobot_description }

whereas the launchfile from myrobot_control looks like that

#!/usr/bin/env python3

import os

import xacro
import launch
from launch_ros.actions import Node

from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch.conditions import IfCondition, UnlessCondition
from launch.actions import DeclareLaunchArgument, GroupAction, ExecuteProcess, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PythonExpression

from ament_index_python.packages import get_package_share_directory, get_package_prefix


def get_parsed_urdf_string():
    shared_dir = get_package_share_directory('myrobot_description')
    path = os.path.join(shared_dir, 'urdf', 'myrobot.xacro')
    return xacro.process(path)

def generate_launch_description():
    urdf_description_string = get_parsed_urdf_string()
    robot_description = { 'robot_description': urdf_description_string }

    myrobot_controller_config = os.path.join(
        get_package_share_directory("myrobot_control"),
        "config",
        "controller.yaml",
    )

    return LaunchDescription([
        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            name="robot_state_publisher",
            output="screen",
            parameters=[robot_description],
        ),
        Node(
            package="controller_manager",
            executable="ros2_control_node",
            name="controller_manager",
            parameters=[ {"update_rate": 1 },
                         robot_description,
                         myrobot_controller_config ],
            output={
                "stdout": "screen",
                "stderr": "screen",
            },
        ),
    ])

and the controller.yaml

controller_manager:
  ros__parameters:
    update_rate: 1 # Hz

    diffdrive_controller:
      type: diff_drive_controller/DiffDriveController

diffdrive_controller:
    left_wheel_names: [ 'front_left_wheel_joint', 'rear_left_wheel_joint' ]
    right_wheel_names: [ 'front_right_wheel_joint', 'rear_right_wheel_joint' ]

    wheel_separation: 0.51
    wheels_per_side: 2
    wheel_radius: 0.141

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    enable_odom_tf: False

Moreover, like initially said, I added some debug outputs to ros2_controls controller_interface.cpp

return_type
ControllerInterface::init(const std::string & controller_name)
{
    RCLCPP_INFO(rclcpp::get_logger("ControllerInterface"), "!!!!!!!!!! controller_name: %s", controller_name.c_str());

    node_ = std::make_shared<rclcpp::Node>(
        controller_name,
        rclcpp::NodeOptions().allow_undeclared_parameters(true));
    lifecycle_state_ = rclcpp_lifecycle::State(
        lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured");

    std::shared_ptr<rclcpp::Node> node2 = std::make_shared<rclcpp::Node>("test", rclcpp::NodeOptions().allow_undeclared_parameters(true));
    std::shared_ptr<rclcpp::Node> node3 = std::make_shared<rclcpp::Node>("test", "controller_manager", rclcpp::NodeOptions().allow_undeclared_parameters(true));

    std::shared_ptr<rclcpp::Node> node4 = std::make_shared<rclcpp::Node>("test");
    std::shared_ptr<rclcpp::Node> node5 = std::make_shared<rclcpp::Node>("test", "controller_manager");

    RCLCPP_INFO(rclcpp::get_logger("ControllerInterface"), "controller_name: %s, nodename: %s, fqn: %s", controller_name.c_str(), node_->get_name(), node_->get_fully_qualified_name());
    RCLCPP_INFO(rclcpp::get_logger("ControllerInterface"), "controller_name: %s, nodename: %s, fqn: %s", controller_name.c_str(), node2->get_name(), node2->get_fully_qualified_name());
    RCLCPP_INFO(rclcpp::get_logger("ControllerInterface"), "controller_name: %s, nodename: %s, fqn: %s", controller_name.c_str(), node3->get_name(), node3->get_fully_qualified_name());
    RCLCPP_INFO(rclcpp::get_logger("ControllerInterface"), "controller_name: %s, nodename: %s, fqn: %s", controller_name.c_str(), node4->get_name(), node4->get_fully_qualified_name());

    return return_type::OK;
}

Based on that infos I start the launchfile like

ros2 launch myrobot_control myrobot_control_launch.py

and try to load the diffdrive_controller in another terminal with

ros2 control load_controller diffdrive_controller

The output i get now is

[ros2_control_node-2] [INFO] [1620293060.380857877] [controller_manager]: update rate is 1 ...