Controller interface now accepts yaml as node parameters

[ddengster]

2 new(rather, moved) functions that allow yaml to be parsed and fed into the lifecycle node of the controller; allowing our controllers to use them. One just takes in a filename, the other takes in an already custom-parsed yaml node.

If this goes in there shouldn't be any more reason to have controller_parameter_server as a seperate node, nor library nor class. We could use the testing code around though. Let me know if it goes against any of your philosophies.

Thanks Karsten for the initial yaml parsing code.

[ddengster]

Hm not sure if the ament_uncrustify errors are some kind of false-positive? Is it not happy with how the TODOs are only in one if-clause?
Edit: Found a fix 👍

[Karsten1987]

How do you envision this to be used? Do you see one yaml file per controller?

Do you envision that there is one yaml file per controller instance? How do you pass these parameters to a controller when being dynamically loaded?

Then further, why not using the ROS2 inbuilt functionality to pass parameters of a YAML file to a ros node? https://index.ros.org/doc/ros2/Tutorials/Node-arguments/#setting-parameters-from-yaml-files

But I generally agree that the parameter server has to go.

[Karsten1987]

this line is superfluous when also being specified as ament_target_dependencies as seen below.

[ddengster]

Fixed in 1256483

[ddengster]

How do you envision this to be used? Do you see one yaml file per controller?

I wouldn't enforce a one yaml file per controller rule but it's a lazy option.

Do you envision that there is one yaml file per controller instance? How do you pass these parameters to a controller when being dynamically loaded?

Again, no. I'm testing on a yaml file that has parameters for 2 controllers. Ideally, the user would do YAML::LoadFile to load the appropriate file, go deeper into the correct depth, and call the function with the YAML::Node argument to do the rest.

YAML::Node root_node = YAML::LoadFile(dir + "/params/coffeebot_controllers.yaml"); 
auto controller = controller_manager.load_controller(name, type);
yaml_node = root_node->get_some_child(); //pseudocode
controller->load_params_from_yaml_node(yaml_node);

Then further, why not using the ROS2 inbuilt functionality to pass parameters of a YAML file to a ros node? https://index.ros.org/doc/ros2/Tutorials/Node-arguments/#setting-parameters-from-yaml-files

I spent a couple of hours on this and I wasn't able to get my parameter files to automatically supply parameters to the nodes. I suppose one would be able to grab yaml filenames from the arguments somehow? But if anyone wants to tackle this feel free to go on ahead.

[guru-florida]

I may be able to help with the yaml file issue using inbuilt ROS2 parameters. I've done a lot with ROS2 yaml config and parameter parsing. I've run into some issues too including out-dated documentation.

• What version of ROS2 are you using? dashing or eloquent?
• Silly issue, but got me....did you put two underscores in the ros__parameters: line?

[ddengster]

@guru-florida
Im using eloquent.
This is the cmd line:

ros2 run coffeebot_custom_controller coffeebot_custom_controller --ros-args --params_file /home/ddeng/rmf_ws/install/coffeebot_controller/share/coffeebot_controller/params/coffeebot_controllers.yaml

This is the file I'm using.

coffeebot_arm_effort_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:
       - coffeebot_shoulder_pan_joint
       - coffeebot_shoulder_lift_joint
       - coffeebot_elbow_joint
       - coffeebot_wrist_1_joint
       - coffeebot_wrist_2_joint
       - coffeebot_wrist_3_joint
    constraints:
        goal_time: 0.6
        stopped_velocity_tolerance: 0.05
        coffeebot_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
        coffeebot_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
        coffeebot_elbow_joint: {trajectory: 0.1, goal: 0.1}
        coffeebot_wrist_1_joint: {trajectory: 0.1, goal: 0.1}
        coffeebot_wrist_2_joint: {trajectory: 0.1, goal: 0.1}
        coffeebot_wrist_3_joint: {trajectory: 0.1, goal: 0.1}
    gains:
        coffeebot_shoulder_pan_joint: {p: 10000, d: 150, i: 0, i_clamp: 1}
        coffeebot_shoulder_lift_joint: {p: 50000, d: 150, i: 10, i_clamp: 50}
        coffeebot_elbow_joint: {p: 50000, d: 150, i: 1, i_clamp: 25}
        coffeebot_wrist_1_joint: {p: 100, d: 5, i: 0, i_clamp: 1}
        coffeebot_wrist_2_joint: {p: 75, d: 2, i: 0, i_clamp: 1}
        coffeebot_wrist_3_joint: {p: 25, d: 1, i: 0, i_clamp: 1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10

coffeebot_gripper_effort_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:
      - coffeebot_gripper_left_joint
      - coffeebot_gripper_right_joint
    gains:
      coffeebot_gripper_left_joint: {p: 20, d: 0.3, i: 0.1, i_clamp: 1}
      coffeebot_gripper_right_joint: {p: 20, d: 0.3, i: 0.1, i_clamp: 1}
    stop_trajectory_duration: 0.5
    state_publish_rate: 25
    action_monitor_rate: 10

[guru-florida]

Looks like you are not specifying the node name perhaps? Try this, I added _-r _node:=node-name for the coffeebot_arm_effort_controller yaml desc:

ros2 run coffeebot_custom_controller coffeebot_custom_controller --ros-args -r __node:=coffeebot_arm_effort_controller --params_file /home/ddeng/rmf_ws/install/coffeebot_controller/share/coffeebot_controller/params/coffeebot_controllers.yaml

Usually the node_name is added automatically by the python launch facility.

[ddengster]

Looks like you are not specifying the node name perhaps? Try this, I added _-r _node:=node-name for the coffeebot_arm_effort_controller yaml desc:

ros2 run coffeebot_custom_controller coffeebot_custom_controller --ros-args -r __node:=coffeebot_arm_effort_controller --params_file /home/ddeng/rmf_ws/install/coffeebot_controller/share/coffeebot_controller/params/coffeebot_controllers.yaml

Usually the node_name is added automatically by the python launch facility.

I am still unable to get the correct parameter values. For more context, my c++ code makes a rclcpp::Node with the same name, then does node->has_parameter("type"), and it always returns false. I tried with these options:

rclcpp::NodeOptions opt;
  opt.automatically_declare_parameters_from_overrides(true);

And it still doesnt work.

[Karsten1987]

For more context, my c++ code makes a rclcpp::Node with the same name, then does node->has_parameter("type"), and it always returns false

Do you declare the parameters as well?

Also for reference, there might be a mismatch between rclcpp::Node and rclcpp_lifecycle::LifecycleNode. See here: ros2/rclcpp#898

[guru-florida]

rclcpp::NodeOptions opt;
opt.automatically_declare_parameters_from_overrides(true);

I dont use the option you mention. Like you, I have had issues with Parameter getters. Here's some of my code with a bool value. The secret sauce seems to be supplying a default value in the declare and the get_value() part of get_parameter.

    declare_parameter("frequency", rclcpp::ParameterValue(30.0f));
    declare_parameter("diagnostic_period", rclcpp::ParameterValue((rcl_duration_value_t)1));
    declare_parameter("self_manage", rclcpp::ParameterValue(false));
...
    if (get_parameter("self_manage").get_value<bool>()) { ... }

    rclcpp::Parameter frequency = get_parameter("frequency");
    double f = frequency.get_value<float>();

You say your c++ code "makes a Node with the same name", so is the node name coffeebot_custom_controller or coffeebot_arm_effort_controller and coffeebot_gripper_effort_controller? Since the ROS2 yaml files support config of multiple nodes the line before ros__parameters: much match the node name.

[guru-florida]

FYI. I am not sure about your { .. } values on some of your yaml values. I think I tried this and it wouldnt transfer into Parameter objects since Parameters only work with primitive types. Take a look at the get_value() methods and you'll see whats supported. For example, arrays are supported.

You can do parameter hierarchies using the parameter dotted name format. I.e.
std::vector<double> gravity = node.declare_parameter("ch0.gains.gravity", std::vector<double>());

Here is my yaml file with the channel and gains.gravity. The { ... } syntax should be the same as what I have here, so possibly the dotted syntax works with your { ... } format too.

my_joint_controller:
    ros__parameters:
        frequency: 20.0
        diagnostic_period: 30
        self_manage: true

        # Channel 0 - Torso and Arms
        ch0:
            port: /dev/ttyAMA1
            name: UpperBody
            joints: [17,18,19,27,28,29]
            offsets: [-0.136136, 1.4538626666666667, 1.1833360000000002, -0.6876613333333333, -1.5236759999999998, -0.691152]
            invert: [false, false, false, true, true, true]
            gravity: 1.2
            gains:
                gravity: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

        # Channel 1 - Left Leg
        ch1:
            port:  /dev/ttyAMA2
            name: LeftLeg
            joints: [21,22,23,24,25,26]
            offsets: [0.068068,	-1.85878, 0.05061466666666666, 1.4730613333333333, 0.04014266666666667,-1.5306573333333333]
            invert: [false,false,true,true,true,false]
            gravity: 1.2
            gains:
                gravity: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

My python launch file:

import os
from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    config_dir = os.path.join(get_package_share_directory('humanoid'), 'config')

    hardware_config = Path(config_dir, 'hardware.yaml')
    assert hardware_config.is_file()

    return LaunchDescription([
        # Joint State Publisher
        Node(
            node_name = 'my_joint_controller',
            package = 'lss_joint_publisher',
            node_executable = 'lss_joint_controller',
            output = 'screen',
            parameters=[hardware_config]
        )
    ])

[ddengster]

Ok, it seems like the way to get my yaml parameters in is to use launch files via ros2 launch; the ros2 run version doesnt seem to work at all.

You say your c++ code "makes a Node with the same name", so is the node name coffeebot_custom_controller or coffeebot_arm_effort_controller and coffeebot_gripper_effort_controller? Since the ROS2 yaml files support config of multiple nodes the line before ros__parameters: much match the node name.

I have 2 nodes named /coffeebot_arm_effort_controller. This link and this link are the problems I'm facing, we should fix them in another PR i think? Thanks @guru-florida !

[guru-florida]

I have 2 nodes named /coffeebot_arm_effort_controller. This link and this link are the problems I'm facing, I'll fix them. Thanks @guru-florida !

You're welcome. I reviewed those links and I see the shortcoming there. :( I didnt encounter that but only because of luck I guess. I have a joint controller that spans 3 serial buses. I made 1 overall node in the process, but it instantiates 3 JointChannel classes that are basically Lifecycle nodes themselves, I even used the same method prototypes. Figured later I might refactor to be multiple nodes in a process.

You could probably refactor a bit and have an initial ControlManager node that reads from yaml config params what nodes to create. It then creates the Node classes with the node name specified which will each also read their yaml config but with the right node-names. This means the launch file only specified the main controller node and the yaml does the rest. The launch can still manage lifecycle of all nodes though so nothing is really lost, just bringup is a bit different.

You should still be able to use the ros2 run command. I am able to here. I did a ros2 launch -d ... to get the cmd line from log output. It's exactly what I had above and I can run the exec directly and it reads yaml fine.

[Karsten1987]

@ddengster can you update on what's the status with this PR? Once more, I feel we should rely on rclcpp/rclcpp_lifecycle capabilities to load parameters from a yaml file instead of baking in some custom logic in the controller interface.

From what I am reading here in this conversation, I have the impression that you have solved it that way, is that right? In that case, can we close this PR?

[ddengster]

Yup, let's close it. Anyway, we should cook up an example with controller_manager.