Potential memory issues in tests

[fmauch]

While implementing #884 I've experienced tests failing / crashing without any directly visible cause locally on my machine, while things seemed to work in CI. This lead me to further investigate this using asan.

So far, it seems to me that I've found one small memory leak and a couple of heap-buffer-overflows. While I've fixed one of them, I'm currently struggling fixing this one.

It looks like cmd_cfg = {controller_interface::interface_configuration_type::ALL}; from https://github.com/fmauch/ros2_control/blob/5ee5d75f8a973cb8b1e1f970e101e01aedc81d23/controller_manager/test/test_controller_manager_srvs.cpp#L141 does not generate the expected list of all command interfaces, which from my understanding of the structure should be the available interfaces.

Before going down the rabbit hole any deeper, I wanted to do some sanity checking: It seems that the tests in CI actually run pretty fine (without the asan check). That could obviously only be coincidence, but given the long history of green pipelines I'm starting to doubt myself. I don't have very much experience in using asan, but the output seems to be pretty clear and also adding a small debug output in test_controller.cpp

    std::cout << "Setting command interface configuration for " << cfg.names.size() << " command interfaces " << std::endl;
  cmd_iface_cfg_ = cfg;
  for (size_t i = 0; i < cmd_iface_cfg_.names.size(); ++i) {
    std::cout << cmd_iface_cfg_.names[i] << std::endl;
  }

yields

Setting command interface configuration for 0 command interfaces

for this call.

Further debugging, I printed the sizes of the two vectors used for setting actual command values in https://github.com/fmauch/ros2_control/blob/9c4c40e732f07f7dbc0c6881bd7462c72c2c179e/controller_manager/test/test_controller/test_controller.cpp#L70:

  else
  {
    std::cout << "External commands for testing: " << external_commands_for_testing_.size() << std::endl;
    std::cout << "command_interfaces: " << command_interfaces_.size() << std::endl;
    for (size_t i = 0; i < command_interfaces_.size(); ++i)
    {
      RCLCPP_INFO(
        get_node()->get_logger(), "Setting value of command interface '%s' to %f",
        command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
      command_interfaces_[i].set_value(external_commands_for_testing_[i]);
    }
  }

which yields

External commands for testing: 0
command_interfaces: 6
[INFO] [1690409930.834575178] [test_controller_name]: Setting value of command interface 'joint2/velocity' to 0.000000
[INFO] [1690409930.834596904] [test_controller_name]: Setting value of command interface 'joint3/velocity' to 0.000000
[INFO] [1690409930.834602730] [test_controller_name]: Setting value of command interface 'joint2/max_acceleration' to 0.000000
[INFO] [1690409930.834616991] [test_controller_name]: Setting value of command interface 'configuration/max_tcp_jerk' to 0.000000
[INFO] [1690409930.834623252] [test_controller_name]: Setting value of command interface 'joint1/position' to 0.000000
[INFO] [1690409930.834628960] [test_controller_name]: Setting value of command interface 'joint1/max_velocity' to -506577792064973123481741778091385948717180152704209650017236786429018946419365460635084090788019703529466616856496751059124971611136897297922900458958266932598261413558271502303854142266073302843639656277997635673394806428918672848419705349380859279117440662057384832385377322518807189815676183391502336.000000

This indeed doesn't look too well and if we look at the latest rolling binary pipeline, we see similar patterns: https://github.com/ros-controls/ros2_control/actions/runs/5663856751/job/15346240987#step:6:9426

6: [INFO] [1690337862.674514955] [test_controller_name]: Setting value of command interface 'joint2/velocity' to 0.000000
  6: [INFO] [1690337862.674541255] [test_controller_name]: Setting value of command interface 'joint3/velocity' to 0.000000
  6: [INFO] [1690337862.674555255] [test_controller_name]: Setting value of command interface 'joint2/max_acceleration' to 0.000000
  6: [INFO] [1690337862.674565855] [test_controller_name]: Setting value of command interface 'configuration/max_tcp_jerk' to 0.000000
  6: [INFO] [1690337862.674573955] [test_controller_name]: Setting value of command interface 'joint1/position' to 90768020600537550690355795355609914311510966455944709568706285852316565960728138898454173784184348145140287581085818843740114465991328596296394015145882489584376577698822723628988771637622155201495529595607010606959195848704.000000
  6: [INFO] [1690337862.674612355] [test_controller_name]: Setting value of command interface 'joint1/max_velocity' to 60217901368638647324400147812533106655286241722534895864590481736422608088806945493737388297261772858937601187577006568694501507168178194577626964688896.000000

So, I guess, I'm not following some hoax...

[fmauch]

Update: I proposed a fix/workaround for the controller_manager_srvs test in fmauch@1987311. From what I understand, there is no easy way of getting ALL command_interfaces in the test file itself, so we could pass them to set_state_interface_configuration()