Proposal for solving multiple issues with implict interface in hardware. #714

destogl · 2022-04-30T15:35:13Z

This PR makes a proposal to address some of the issues mentioned in #712. For now, only minimal changes to the System interface to trigger discussion.

Explicitly there are the following changes as the base for the discussion

Variant as base storage of values (address long-standing issue of data-types different than double), related to expose complex data types through interfaces #490
PIMPL-classes actually store variants to simplify management from the user's perspective and provide more explicit read and write.

Open questions

Should we actually store Handles inside PMPL-Classes to preserve the explicit connection between Variant and joint/interface name? This would probably result in using multiple maps, one in ResouceManager and one in PIMPL-Class --> searching through 2 maps to create a loaned interface (real-time critical).

Other issues (please comment on this too)

using templates in specific places to save the code
using unique_ptr for Variants instead of a raw pointer (?)

@tylerjw and @AndyZe, please provide your feedback on this. Does this go in the direction you would have imagined?

P.S. This is just a concept, the code is not working!

tylerjw · 2022-05-01T17:38:55Z

When I said the base type should be a function and not a pointer to a double here is closer to what I was thinking: https://compiler-explorer.com/z/dq1s4baGG

This could also help with solving the problem of chaining controllers. Controllers themselves could provide command interfaces that other controllers could use.

I didn't implement reading, but my general design for that is that it would work in a similar way. The hardware interfaces would implement a function that returns a map of names to functions, those functions would return values instead of taking them as parameters. Controllers that need to read values from hardware could non-exclusively get copies of these functions to read the values from the hardware interfaces.

Note that my example did not implement different data types. I did this to keep it simpler, but I believe std::variant<double, int, bool> could be used as the base type in ros2_control or something similar to make it work with different types. To use it this way though you'll need to write something that takes the variant and the string from the urdf and returns the concrete type.

Lastly, I don't think it is generally a good idea to have vectors of values be a valid base type. The reason for this is that vectors allocate dynamic memory when you copy them. One of the goals of ros2_control should be to have no dynamic memory allocation during the running of the control loop.

AndyZe

I don't completely understand the pros and cons but here are some things to think about.

AndyZe · 2022-05-02T13:46:55Z

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -166,7 +170,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   *
   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
   */
-  virtual return_type read() = 0;
+  virtual return_type read(std::vector<Variant> & states) = 0;


I would support what @tylerjw suggested here: NoDiscard.

This will force the user to check the return value.

Suggested change

virtual return_type read(std::vector<Variant> & states) = 0;

virtual [[nodiscard]] return_type read(std::vector<Variant> & states) = 0;

I don't understand why a controller should deal with a failure of an interface that belongs to a hardware. Controller should do it's thing.

The issue you actually have is missing functionality in controller manager. Everything is known for months already, but someone has to write some code at some point. Unfortunately, most of the users decide to do some external solutions instead of writing 50 lines of code in the controller manager.

AndyZe · 2022-05-02T13:47:35Z

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -175,7 +179,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   *
   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
   */
-  virtual return_type write() = 0;
+  virtual return_type write(std::vector<Variant> & commands) = 0;


NoDiscard would be good here, too.

Agree NoDiscard should be used on these and the return here should be propagated to the controllers so the controller fails when the hardware it is commanding fails.

This is not, and it will never be propagated to controllers. Controller manager should deal with this. (see comment above)

The controller is the interface to the user in many cases as the controllers create ros interfaces. This means that failures are communicated out of band from how external ros processes are interacting with ros2_control. I want to get rid of all of these sorts of "out of band", "side-affect", "implicit" communication channels and make the handling of errors part of the explicit path of the code.

AndyZe · 2022-05-02T13:49:45Z

hardware_interface/include/hardware_interface/handle.hpp

+};
+
+/// A variant class used to store value on a given interface
+class Variant


I think a better name is needed. Maybe InterfaceVariant, HWInterfaceVariant, or HardwareInterfaceVariant

I think it would be better to just use std::variant from c++17, it is a much nicer implementation of a sum type than this.

As discussed, the problem with std::variant is that it does not support arrays or vectors – which we want to add in forceable future (cf. #490)

you can't copy a vector without calling new, I don't understand why you need vectors when you can use template arguments to specify the size of sets of values.

AndyZe · 2022-05-02T13:58:40Z

hardware_interface/test/test_components/test_system.cpp

+    states[0].set_value(false);    // there is still some implicit knowledge about order of interfaces (the same order as they info is exported)
+    states[1].set_value(1.2);
+
+    return return_type::OK;


There's no way for this function to fail. I think part of the issue @tylerjw brought up was to make sure states_ and commands_ are intiailized. I still don't see that happening -- return_type is useless.

On the other hand, I'm not sure that checking for initialization at every control cycle is even worth the CPU cycles. ¯_(ツ)_/¯

Because of how branch prediction works in modern CPUs if statements that nearly always go one way (in this case it will nearly always be OK) should be virtually no performance cost to checking on every iteration.

Typically, this function would fail if communication to hardware is broken.

AndyZe · 2022-05-02T14:00:05Z

hardware_interface/include/hardware_interface/handle.hpp

+  double double_value_;
+  std::vector<bool> vector_bool_value_;
+  std::vector<int> vector_int_value_;
+  std::vector<double> vector_double_value_;


Maybe you should add a member that would store whether this interface is initialized and a getter function.

bool is_initialized_;
bool is_initialized() { return is_initialized_; }

I would really like to be rid of initialized flag values and instead use RAII like it was meant to be used. All constructed objects are either valid or don't exist. We should use the type system for things like this.

AndyZe · 2022-05-02T14:01:08Z

hardware_interface/test/test_components/test_system.cpp

@@ -78,9 +131,22 @@ class TestSystem : public SystemInterface
    return command_interfaces;
  }

-  return_type read() override { return return_type::OK; }
+  return_type read(std::vector<hardware_interface::Variant> & states) override
+  {


Per my comment above, you might consider adding this:

if (!states.is_initialized())
return return_type::ERROR;

Although, we are setting values here, so it shouldn't be a problem. Read is anyway first called after hardware interface is configured and interfaces are exported.

I also don't like the output parameter being added to read here. It would be much better if read returned an either type (error or valid values), or if you must a tuple/pair of return_type and the states. It is also confusing here which state value should be written into which index of the vector... better to use a map or some sort of data type that makes it explicit instead of relying on implicit indexes.

tylerjw

I want to get rid of the pointer to data for communication because the hardware interface has no way of knowing when that value is being written into. The pointer to memory in the hardware interface is the source of the implicit contracts I want to do away with.

tylerjw · 2022-05-02T15:25:20Z

hardware_interface/include/hardware_interface/handle.hpp

+};
+
+/// A variant class used to store value on a given interface
+class Variant


I think it would be better to just use std::variant from c++17, it is a much nicer implementation of a sum type than this.

tylerjw · 2022-05-02T15:26:28Z

hardware_interface/include/hardware_interface/handle.hpp

 /// A handle used to get and set a value on a given interface.
 class ReadOnlyHandle
 {
 public:
  ReadOnlyHandle(
-    const std::string & name, const std::string & interface_name, double * value_ptr = nullptr)
+    const std::string & name, const std::string & interface_name, Variant * value_ptr = nullptr)


This doesn't fix the fundamental problem I see with the interfaces of ros2_control. With this change, you are still using a pointer to some memory in the hardware_interface for communication between the hardware_interface and the controllers.

I just don't know any other approach that would work will all the types we want to support and provides all the flexibility and strictness to manage this sensibly with controller manager.

Using functions does not solve fully all the issues, but adds complexity on the users' side of the code.

Using functions makes it explicit and not out of band.

tylerjw · 2022-05-02T15:27:27Z

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -175,7 +179,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   *
   * \return return_type::OK if the read was successful, return_type::ERROR otherwise.
   */
-  virtual return_type write() = 0;
+  virtual return_type write(std::vector<Variant> & commands) = 0;


Agree NoDiscard should be used on these and the return here should be propagated to the controllers so the controller fails when the hardware it is commanding fails.

tylerjw · 2022-05-02T15:29:57Z

hardware_interface/test/test_components/test_system.cpp

+    states[0].set_value(false);    // there is still some implicit knowledge about order of interfaces (the same order as they info is exported)
+    states[1].set_value(1.2);
+
+    return return_type::OK;


Because of how branch prediction works in modern CPUs if statements that nearly always go one way (in this case it will nearly always be OK) should be virtually no performance cost to checking on every iteration.

bmagyar · 2022-05-03T06:07:20Z

Variant is only a variant of #490, it does not propose a solution for the main issue of interface disconnect.

You already talk about tricks w no_discard, branch prediction, CPU cycles, not a good sign of design, but one of premature optimization, excitement and rushing to conclusions. On top of that, it is only 4days after #712. It feels like I missed a week's worth of discussions :D

With humble around the corner, don't expect any of this to be merged. Happy to have a discussion at the WG meeting, please come prepared to present your case ;)

tylerjw · 2022-05-03T06:11:05Z

I have no expectation of changing the API in Humble. I'd be satisfied if I can find a way to rectify the disconnect by the end of the year.

mergify · 2022-05-13T06:56:37Z