Bipedal-locomotion-framework: Expose Advanceable object owned by the AdvanceableRunner

[prashanthr05]

Currently, in order to set the Advanceable object for the AdvanceableRunner, we use a std::unique_ptr. This is done so by creating a unique pointer to Advanceable and moving it to the runner, thereby transferring the ownership to the runner. However, the current implementation of the AdvanceableRunner has a limitation that the Advanceable object can be accessed only from within the thread created by the runner.

Oftentimes, an arbitrary external command which is different from commands called repeatedly from within the runner thread (setInput(), advance() and getOutput()) needs to be used for any Advanceable object, for example advanceable->resetAdvanceable() and further, these calls might be through other threads have instantiated or which have access to the AdvanceableRunner.

Given the addition of get by reference methods for the resource and mutex maintained by SharedResource<T> (https://github.com/dic-iit/bipedal-locomotion-framework/pull/398), would it be useful to pass, the Advanceable object as a SharedResource<std::unique_ptr<_Advanceable> > to the AdvanceableRunner? But, this will mean that the ownership of the Advanceable object is not with the AdvanceableRunner.

Alternate solution considered

Alternatively, we could expose a const reference to the unique_ptr of the Advanceable object maintained by the AdvanceableRunner and expose an associate mutex for the Advanceable object as a reference and remind the user to lock the mutex before accessing the pointer to the advanceable object.

Links

• AdvanceableRunner
• Advanceable
• SharedResource

cc @GiulioRomualdi @S-Dafarra @traversaro

[S-Dafarra]

Given the addition of get by reference methods for the resource and mutex maintained by SharedResource<T> (#398), would it be useful to pass, the Advanceable object as a SharedResource<std::unique_ptr<_Advanceable> > to the AdvanceableRunner? But, this will mean that the ownership of the Advanceable object is not with the AdvanceableRunner.

SharedResource<std::unique_ptr<_Advanceable> > seems a bit in contrast. I am wondering why the unique_ptr in the first place.

Maybe it can be easier to understand what you need if you had an example of code that cannot run at the moment

[prashanthr05]

Given the addition of get by reference methods for the resource and mutex maintained by SharedResource<T> (#398), would it be useful to pass, the Advanceable object as a SharedResource<std::unique_ptr<_Advanceable> > to the AdvanceableRunner? But, this will mean that the ownership of the Advanceable object is not with the AdvanceableRunner.

SharedResource<std::unique_ptr<_Advanceable> > seems a bit in contrast. I am wondering why the unique_ptr in the first place.

Yes, after you mention it, it doesn't seem to make sense. Indeed, I was trying to understand if I can use SharedResource<std::shared_ptr<_Advanceable> > without using the AdvanceableRunner which maintains a unique_ptr<Advanceable>. It makes perfect sense for the AdvanceableRunner to do so, because it means that that block will be the sole owner of the Advanceable object. Just for the clarity and considering what you mentioned, I will change the title of this issue.

Maybe it can be easier to understand what you need if you had an example of code that cannot run at the moment

Here's a simple example I was implementing trying to simulate the data-flow for the KinDynVIO software architecture. I have also removed more details from the use-case example below for readability.

• We have an Advanceable object called SherlockHolmes.
  
  
  

  SherlockHolmes

// Someone needs to solve the crime. Choose Robert Downy Jr.,
// Benedict Cumberbatch is busy doing Dr. Strange or
// are there any new Sherlocks in town?
class SherlockHolmes : public Advanceable<std::string, std::string>
{
public:
    bool initialize(std::weak_ptr<const IParametersHandler> handler) final
    {
        return true;
    }

    const std::string& getOutput() const final
    {
        return m_out;
    }

    bool setInput(const std::string& input) final
    {
        m_in = input;
        return true;
    }

    bool isOutputValid() const final
    {
        return true;
    }

    void callToWork()
    {
        std::cout << "SherlockHolmes going to work." << m_in << std::endl;
        mustWork = true;
    }

    void askToRest()
    {
         mustWork = false;
    }

    bool advance() final
    {
        std::cout << "SherlockHolmes entering advance with input: " << m_in << std::endl;
        // Sherlock works only on even days
        if ( (id % 2 == 0 && m_in == instinct) || (mustWork && m_in == instinct) )
            m_out = std::to_string(id) + ": Klepto stole the pie. ";
        else
            m_out = std::to_string(id) + ": Sherlock out of office. ";
        id++;
        return true;
    }

private:
    bool mustWork{false};
    std::string instinct{"Something's missing."};
    long long int id{0};
    std::string m_out{""};
    std::string m_in{" "};
};

• SherlockHolmes works in a PieFactory class.

PieFactory class

class PieFactory
{
public:
    PieFactory()
    {
        m_number = SharedResource<double>::create();
        m_word = SharedResource<std::string>::create();
        m_rumour = SharedResource<std::string>::create();
        m_clue = SharedResource<std::string>::create();
        m_caseFile = SharedResource<std::string>::create();
        m_idx = SharedResource<long long int>::create();
    }

    bool setNumber(const double& number)
    {
        if (!m_number)
            return false;

        m_number->set(number);
        return true;
    }

    bool assembleRunners()
    {
        std::shared_ptr param = std::make_shared<StdImplementation>();
        param->setParameter("sampling_time", 0.05);
        param->setParameter("enable_telemetry", true);

        bool ok{true};
        param->setParameter("sampling_time", 0.2);
        auto sherlock = std::make_unique<SherlockHolmes>();
        ok = ok && sherlockRunner.initialize(param);
        ok = ok && sherlockRunner.setInputResource(m_clue);
        ok = ok && sherlockRunner.setOutputResource(m_caseFile);
        ok = ok && sherlockRunner.setAdvanceable(std::move(sherlock));
        if (!ok)
        {
            BipedalLocomotion::log()->error("Without a detective, what are we going to do.");
            return false;
        }

        return true;
    }

    bool manageLogistics()
    {
        // do something useful
        // if (something)
       //       sherlockRunner.getAdvanceable()->askToWork();  // this is not doable now
       //  else
       //       sherlockRunner.getAdvanceable()->askToRest();  // this is not doable now
        return true;
    }

    void start()
    {
        sherlockThread = sherlockRunner.run();
    }

    void stop()
    {
        sherlockRunner.stop();

        // print some information
        BipedalLocomotion::log()->info("SherlockRunner : Number of deadline miss {}",
                                       sherlockRunner.getInfo().deadlineMiss);

        // join the threads before closing
        if (sherlockThread.joinable())
        {
            sherlockThread.join();
            sherlockThread = std::thread();
        }
    }

private:
    AdvanceableRunner<SherlockHolmes> sherlockRunner;
    std::thread sherlockThread;

    SharedResource<double>::Ptr m_number;
    SharedResource<std::string>::Ptr m_word, m_rumour, m_caseFile, m_clue;
    SharedResource<long long int>::Ptr m_idx;
};

• Running the PieFactory from the main thread which initializes and dispatches other AdvanceableRunner threads. And PieFactory has it's own advance() like logic which needs to be run in loop. Within this logic, the PieFactory may/may not ask the composed SherlockHolmes object to do something. But currently, this cannot be done since PieFactory will not have access to the SherlockHolmes advanceable object (See manageLogistics() method of the PieFactory class).

Main()

TEST_CASE("AdvanceableRunner test")
{
    // A Kindness Monster, A Kleptomaniac and A Sherlock Holmes in a PieFactory

    // Install a signal handler
    std::signal(SIGINT, signal_handler);

    bool thisIsTheEnd{false};

    PieFactory pie;
    REQUIRE(pie.assembleRunners());
    pie.start();

    auto starTime = BipedalLocomotion::clock().now();
    double timeElapsed{0};
    long long int seq{0};
    while (!thisIsTheEnd)
    {
        if (gSignalStatus == SIGINT || timeElapsed > 3)
        {
            thisIsTheEnd = true;
        }

        double number{22.0};
        if (seq % 7 == 0)
            number = 3.141519;

        pie.setNumber(number);
        pie.manageLogistics();

        BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.2));
        timeElapsed = (BipedalLocomotion::clock().now() - starTime).count();
        seq++;
    }

    pie.stop();
}

[S-Dafarra]

That's awesome! :joy:

[S-Dafarra]

From what I see, it seems to me that instead of passing a unique_ptr to the AdvanceableRunner, we should pass a shared_ptr. The main point would be to deal with the concurrent access to the Advanceable. This is the advantage of using a unique_ptr. You can create the object, but once you pass it, it's gone. Instead, with the shared_ptr, we need to make sure to not access the advanceable at the same time from two places.

In order to handle the concurrency in the Advanceable then, I see the following alternatives:

1. we add a mutex as a parameter of Advanceable
2. we add another object that contains a shared pointer to an Advanceable, and an internal mutex (something like ThreadSafeAdvanceable). This is what is passed to the AdvanceableRunner. This class could handle the lock/unlock of the mutex every time it is needed, but it would be difficult to guarantee the RAII (i.e. you always have to remember to lock and unlock by yourself)

[prashanthr05]

(1) seems to be an easier alternative and establishes clear ownership of the Advanceable.
(2) can be done already by using SharedResource<std::shared_ptr<T> >, however one must remember to call make_shared<T>() at some point, since SharedResource<std::shared_ptr<T> >::create() creates a shared pointer only for the shared resource and the contained resource which is another shared pointer might still be invalid. But also needs a bit more of restructuring the AdvanceableRunner class.

As you say, in both the alternatives, we must remember to lock and unlock (sob!).

[prashanthr05]

Maybe for the moment, I will try introducing changes relevant to (1) in my fork. If we reach a consensus on it, I can open a PR.

[prashanthr05]

1. we add a mutex as a parameter of Advanceable

Just to be clear, we add a mutex in the AdvanceableRunner (std::mutex m_advanceableMutex) associated to the advanceable. And expose both mutex and advanceable? Is this what you meant? (Because I understood that way, or wanted to understand that way maybe ;D)

[S-Dafarra]

1. we add a mutex as a parameter of Advanceable

Just to be clear, we add a mutex in the AdvanceableRunner (std::mutex m_advanceableMutex) associated to the advanceable. And expose both mutex and advanceable? Is this what you meant? (Because I understood that way, or wanted to understand that way maybe ;D)

I was thinking more to have a mutex inside Advanceable. But actually, this has a big problem. The user might not use the mutex in his implementation of Advanceable. Hence, we should lock it before calling its methods in the AdvanceableRunner then. But, if the user actually locks it inside the advance method for example, then we have a deadlock.

So I guess that your idea makes more sense.

[prashanthr05]

I was thinking something like this,

Changes to AdvanceableRunner

     std::unique_ptr<_Advanceable> m_advanceable; /**< Advanceable contained in the runner */
+    std::mutex m_advanceableMutex; /**< Mutex used to protect the Advanceable */
     typename SharedResource<Input>::Ptr m_input; /**< Input shared resource */
     typename SharedResource<Output>::Ptr m_output; /**< Output shared resource */

@@ -87,6 +88,10 @@ public:
      */
     bool setAdvanceable(std::unique_ptr<_Advanceable> advanceable);

+    const std::unique_ptr<_Advanceable>& getAdvanceable() const;
+
+    std::mutex& getAdvanceableMutex();
+
     /**
      * Set the input resource
      * @param resource pointer representing the input resource
@@ -188,6 +193,18 @@ bool AdvanceableRunner<_Advanceable>::setAdvanceable(std::unique_ptr<_Advanceabl
     return true;
 }

+template <class _Advanceable>
+const std::unique_ptr<_Advanceable>& AdvanceableRunner<_Advanceable>::getAdvanceable() const
+{
+    return m_advanceable;
+}
+
+template <class _Advanceable>
+std::mutex& AdvanceableRunnder<_Advanceable>::getAdvanceableMutex()
+{
+    return m_advanceableMutex;
+}
+
 template <class _Advanceable>
 bool AdvanceableRunner<_Advanceable>::setInputResource(std::shared_ptr<SharedResource<Input>> input)
 {
@@ -281,6 +298,9 @@ template <class _Advanceable> std::thread AdvanceableRunner<_Advanceable>::run()
             // advance the wake-up time
             wakeUpTime += m_dT;

+            // lock mutex
+            this->m_advanceableMutex.lock();
+
             if (!this->m_advanceable->setInput(this->m_input->get()))
             {
                 m_isRunning = false;
@@ -300,6 +320,9 @@ template <class _Advanceable> std::thread AdvanceableRunner<_Advanceable>::run()
             // set the output
             this->m_output->set(this->m_advanceable->getOutput());

+            // unlock mutex
+            this->m_advanceableMutex.unlock();
+
             // release the CPU
             BipedalLocomotion::clock().yield();

with the danger of hoping that user will remember to get the mutex to lock and unlock before getting the const ref to unique pointer.

[S-Dafarra]

There are two things I don't like:

1. returning a reference to a unique_ptr. What about returning a reference to the Advanceable itself?
2. the manual lock.

What if, instead of returning a pointer to the Advanceable, we return an object that locks the mutex automatically when it is created, and unlocks it when it goes out of scope?

Something like

class AdvanceableAccessor
{
    Advanceable& m_ref;
    std::lock_guard<std::mutex> m_lock;

    AdvanceableAccessor(Advanceable& ref, std::mutex& mutex)
    : m_ref(ref)
    , m_lock(mutex)
    { }

friend class AdvanceableRunner<Advanceable>;

public:

    Advanceable& get()
    {
        return m_ref;
    }
}

AdvanceableRunner would create and return this object.

This object should be created and passed by move, since there is no copy constructor for std::lock_guard. I am not sure if it works :thinking:

[prashanthr05]

This object should be created and passed by move, since there is no copy constructor for std::lock_guard. I am not sure if it works

Seems to work without move.

AdvanceableAccessor

--- a/src/System/include/BipedalLocomotion/System/AdvanceableRunner.h
+++ b/src/System/include/BipedalLocomotion/System/AdvanceableRunner.h
@@ -25,6 +25,29 @@ namespace BipedalLocomotion
 namespace System
 {

+template <class _Advanceable> class AdvanceableRunner;
+
+template <class _Advanceable> class AdvanceableAccessor
+{
+    using Advanceable = _Advanceable;
+    Advanceable& m_ref;
+    std::lock_guard<std::mutex> m_lock;
+
+    AdvanceableAccessor(Advanceable& ref, std::mutex& mutex)
+    : m_ref(ref)
+    , m_lock(mutex)
+    { }
+
+friend class AdvanceableRunner<Advanceable>;
+
+public:
+
+    Advanceable& get()
+    {
+        return m_ref;
+    }
+};
+
 /**
  * AdvanceableRunner is an helper class that can be used to run a advanceable at a given period.
  * Different AdvanceableRunners can communicate trough the SharedResource class. The
@@ -51,9 +74,10 @@ private:
     std::chrono::duration<double> m_dT{std::chrono::duration_values<double>::zero()}; /**< Period of
                                                                                          the runner
                                                                                        */
-    std::atomic<bool> m_isRunning{false}; /**> If True the runner is running */
+    std::atomic<bool> m_isRunning{false}; /**< If True the runner is running */

     std::unique_ptr<_Advanceable> m_advanceable; /**< Advanceable contained in the runner */
+    std::mutex m_advanceableMutex; /**< Mutex used to protect the Advanceable */
     typename SharedResource<Input>::Ptr m_input; /**< Input shared resource */
     typename SharedResource<Output>::Ptr m_output; /**< Output shared resource */

@@ -87,6 +111,33 @@ public:
      */
     bool setAdvanceable(std::unique_ptr<_Advanceable> advanceable);

+    /**
+     * Get the reference to the advanceable object inside the runner.
+     * @note please remember to get the advanceableMutex to perform
+     *       necessary locking/unlocking before/after use of this ref to ptr.
+     * An example could be,
+     * \code{.cpp}
+     * advanceableRunner.getAdvanceableMutex().lock();
+     * advanceableRunner.getAdvanceable().doSomething();
+     * advanceableRunner.getAdvanceableMutex().unlock();
+     * \endcode
+     * or
+     * \code{.cpp}
+     * {
+     *    std::lock_guard<std::mutex>(advanceableRunner.getAdvanceableMutex());
+     *    advanceableRunner.getAdvanceable().doSomething();
+     * }
+     * \endcode
+     * @return const reference to the pointer of the advanceable
+     */
+    const std::unique_ptr<_Advanceable>& getAdvanceable() const;
+
+    /**
+     * Get the mutex protecting the advanceable object
+     * @return reference to the advanceable mutex
+     */
+    std::mutex& getAdvanceableMutex();
+
     /**
      * Set the input resource
      * @param resource pointer representing the input resource
@@ -94,6 +145,8 @@ public:
      */
     bool setInputResource(std::shared_ptr<SharedResource<Input>> resource);

+    AdvanceableAccessor<_Advanceable> getAdvanceableAccessor();
+
     /**
      * Set the output resource
      * @param resource pointer representing the output resource
@@ -281,6 +334,9 @@ template <class _Advanceable> std::thread AdvanceableRunner<_Advanceable>::run()
             // advance the wake-up time
             wakeUpTime += m_dT;

+            // lock mutex
+            this->m_advanceableMutex.lock();
+
             if (!this->m_advanceable->setInput(this->m_input->get()))
             {
                 m_isRunning = false;
@@ -300,6 +356,9 @@ template <class _Advanceable> std::thread AdvanceableRunner<_Advanceable>::run()
             // set the output
             this->m_output->set(this->m_advanceable->getOutput());

+            // unlock mutex
+            this->m_advanceableMutex.unlock();
+
             // release the CPU
             BipedalLocomotion::clock().yield();

@@ -331,6 +390,25 @@ template <class _Advanceable> std::thread AdvanceableRunner<_Advanceable>::run()
     return std::thread(function);
 }

+template <class _Advanceable>
+const std::unique_ptr<_Advanceable>& AdvanceableRunner<_Advanceable>::getAdvanceable() const
+{
+    return m_advanceable;
+}
+
+template <class _Advanceable>
+std::mutex& AdvanceableRunner<_Advanceable>::getAdvanceableMutex()
+{
+    return m_advanceableMutex;
+}
+
+template <class _Advanceable>
+AdvanceableAccessor<_Advanceable> AdvanceableRunner<_Advanceable>::getAdvanceableAccessor()
+{
+    return AdvanceableAccessor<_Advanceable>(*m_advanceable, m_advanceableMutex);
+}
+
+
 template <class _Advanceable> void AdvanceableRunner<_Advanceable>::stop()
 {
     // m_isRunning is an atomic<bool> the mutex is not required here
diff --git a/src/System/tests/AdvanceableRunnerTest.cpp b/src/System/tests/AdvanceableRunnerTest.cpp
index 3ff9770..dd6fbc8 100644
--- a/src/System/tests/AdvanceableRunnerTest.cpp
+++ b/src/System/tests/AdvanceableRunnerTest.cpp
@@ -43,6 +43,11 @@ public:
     {
         return true;
     }
+
+    void printSomething()
+    {
+        BipedalLocomotion::log()->info("DummyBlock: Ok! Printing something.");
+    }
 };

 TEST_CASE("Test Block")
@@ -79,7 +84,8 @@ TEST_CASE("Test Block")

     while (!output0->get() || !output1->get())
     {
-        BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.01));
+        runner1.getAdvanceableAccessor().get().printSomething();
+        BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.01));
     }

So, maybe I can remove the const ref of advanceable pointer and mutex getters?

[S-Dafarra]

Seems to work without move.

Nice! I guess that if you get the accessor and perform the operations in a single line, should be ok. This still does not prevent the user to do something like:

Advanceable& wrong = runner1.getAdvanceableAccessor().get();
wrong.doSomething();

since this would not be protected :thinking:

So, maybe I can remove the const ref of advanceable pointer and mutex getters?

Yes, I would prefer.

[prashanthr05]

Nice! I guess that if you get the accessor and perform the operations in a single line, should be ok. This still does not prevent the user to do something like:

But to be safe, I think let's move it. ~About the latter, yes I was worried about it. In case of an extended scope this will be a problem.~
I see what you mean. The protection is only during construction of the accessor. (I was confused, since strangely the test doesn't seem to be crashing, when doing it the wrong way)

Haha, too many "walking on ice" scenarios.

[S-Dafarra]

Maybe, we can get away with the following:

class AdvanceableAccessor
{
    Advanceable* m_ptr;
    std::lock_guard<std::mutex> m_lock;

    AdvanceableAccessor(Advanceable* ptr, std::mutex& mutex)
    : m_ptr(ptr)
    , m_lock(mutex)
    { }

friend class AdvanceableRunner<Advanceable>;

public:

    Advanceable* operator->()
    {
        return m_ptr;
    }
}

This should discourage the local copy :thinking:

[prashanthr05]

That's an awesome neat trick!

[prashanthr05]

That's an awesome neat trick!

But ends up in unexpected results. The reference worked better and consistent.

[S-Dafarra]

That's an awesome neat trick!

But ends up in unexpected results. The reference worked better and consistent.

I guess that's because the pointer is returned by copy. This probably triggers the deletion of the object. In the other version instead, returning a reference probably triggered a lifetime extension. But that's just a guess.

A test could be to return a reference to the pointer 🤔

[prashanthr05]

That's an awesome neat trick!

But ends up in unexpected results. The reference worked better and consistent.

I guess that's because the pointer is returned by copy. This probably triggers the deletion of the object. In the other version instead, returning a reference probably triggered a lifetime extension. But that's just a guess.

A test could be to return a reference to the pointer

Sorry, actually it was my bad. I was trying a variety of combinations to understand what's happening. So, in both cases of the pointer and reference, we are able to have consistent (but not deterministic) behaviors.

The inconsistent behavior in the AdvanceableRunnerTest rises when I split the accessor into multiple lines.

    while (!output0->get() || !output1->get())
    {
         auto wrong = runner1.getAdvanceableAccessor();
        //runner1.getAdvanceableAccessor()->printSomething();
        wrong.get().printSomething();
        BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.001));
    }

or

    auto wrong = runner1.getAdvanceableAccessor();
    while (!output0->get() || !output1->get())
    {
        //runner1.getAdvanceableAccessor()->printSomething();
        wrong.get().printSomething();
        BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.001));
    }

By consistent behavior, I mean the test case terminates when one of the threads hits 10 count. (sometimes, exits early both threads print 9, sometimes exits late one or both threads print 10, nevertheless exits cleanly).

By inconsistent behavior, the test case keeps running indefinitely until a ctrl-c is passed, and the counter for one of the threads keeps increasing.

[S-Dafarra]

That's my fault. lock_guard is not movable. Maybe with unique_lock?

[GiulioRomualdi]

Im just jumping in without reading all the details you wrote 🥲

It would be nice to have something like

AdvanceableRunner runner;
...
...
// Access to the object using murex with an operator
runner().myFunction();

A possibility could be also

runner(advanceable::myFunction)

The last approach passes a pointer to the member function and in this case we are sure that the function is called within murex.
I understand that this have some limitations:

1. How handle function returning stuff
2. probably is a bit controintuitive

Last concern, is it efficient to lock unlock the mutex in the thread of the advanceable runner? I don't think there are other options