Replanning trigger for the navigation stack.

src/CMakeLists.txt

@@ -14,3 +14,4 @@ add_subdirectory(KinDynWrapper)
 add_subdirectory(RetargetingHelper)
 add_subdirectory(WalkingModule)
 add_subdirectory(JoypadModule)
+add_subdirectory(NavigationHelper)

src/NavigationHelper/CMakeLists.txt

@@ -0,0 +1,10 @@
+# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Giulio Romualdi <giulio.romualdi@iit.it>
+
+add_walking_controllers_library(
+  NAME NavigationHelper
+  SOURCES src/NavigationHelper.cpp
+  PUBLIC_HEADERS include/WalkingControllers/NavigationHelper/NavigationHelper.h
+  PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities ctrlLib
+  PRIVATE_LINK_LIBRARIES Eigen3::Eigen)

src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h

@@ -0,0 +1,69 @@
+/**
+ * @file NavigationHelper.h
+ * @authors Simone Micheletti <simone.micheletti@iit.it>
+ * @copyright 2023 iCub Facility - Istituto Italiano di Tecnologia
+ *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2023
+ */
+
+#ifndef NAVIGATION_HELPER__HPP
+#define NAVIGATION_HELPER__HPP
+
+#include <yarp/os/BufferedPort.h>
+#include <yarp/os/Bottle.h>
+#include <atomic>
+#include <thread>
+#include <deque>
+#include <mutex>
+
+namespace WalkingControllers
+{
+    class NavigationHelper
+    {
+    private:
+        yarp::os::BufferedPort<yarp::os::Bottle> m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */
+        std::atomic<bool> m_runThreads{false};      /**< Global flag that allows the looping of the threads. */
+        std::deque<bool> m_leftInContact;           /**< Copy of the deques in Module of the left feet contacts status. */
+        std::deque<bool> m_rightInContact;          /**< Copy of the deques in Module of the left feet contacts status. */
+        double m_navigationReplanningDelay;         /**< Delay in seconds of how much to wait before sending the trigger to the navigation stack after exiting double support. */
+        int m_navigationTriggerLoopRate;            /**< Loop rate for the thread computing the navigation trigger*/
+        bool m_publishInfo;                         /**< Flag to whether publish information. */
+        std::thread m_navigationTriggerThread;      /**< Thread for publishing the flag triggering the navigation's global planner. */
+        std::mutex m_updateFeetMutex;    
+        bool m_simulationMode{false};               /**< Flag that syncs the trigger delay with the external clock if in simulation. */  
+
+        const std::string m_portPrefix = "/navigation_helper";
+
+        /**
+         * Function launched by the looping thread
+         */
+        void computeNavigationTrigger();
+
+    public:
+        NavigationHelper();
+        ~NavigationHelper();
+
+        /**
+         * Close the Navigation Helper Threads and ports
+         * @return true/false in case of success/failure.
+         */
+        bool closeThreads();
+
+        /**
+         * Close the Navigation Helper Threads and ports
+         * @return true/false in case of success/failure.
+         */
+        bool closeHelper();
+
+        /**
+         * Initialize the Navigation Helper
+         * @param config yarp searchable object of the configuration.
+         * @return true/false in case of success/failure.
+         */
+        bool init(const yarp::os::Searchable& config);
+
+        bool updateFeetDeques(const std::deque<bool> &left, const std::deque<bool> &right);
+    };
+}
+
+#endif

src/NavigationHelper/src/NavigationHelper.cpp

@@ -0,0 +1,183 @@
+/**
+ * @file NavigationHelper.cpp
+ * @authors Simone Micheletti <simone.micheletti@iit.it>
+ * @copyright 2023 iCub Facility - Istituto Italiano di Tecnologia
+ *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2023
+ */
+
+#include <iostream>
+#include <thread>
+#include <WalkingControllers/NavigationHelper/NavigationHelper.h>
+
+#include <WalkingControllers/YarpUtilities/Helper.h>
+
+
+using namespace WalkingControllers;
+
+NavigationHelper::NavigationHelper()
+{
+}
+
+NavigationHelper::~NavigationHelper()
+{
+}
+
+bool NavigationHelper::closeThreads()
+{
+    //Close parallel threads
+    if (m_runThreads)
+    {
+        m_runThreads = false;
+
+        yInfo() << "Closing m_navigationTriggerThread";
+        if(m_navigationTriggerThread.joinable())
+        {
+            m_navigationTriggerThread.join();
+            m_navigationTriggerThread = std::thread();
+        }
+    }
+    return true;
+}
+
+bool NavigationHelper::closeHelper()
+{
+    try
+    {
+        //close threads first
+        if (m_runThreads)
+        {
+            closeThreads();
+        }
+        //close ports
+        if(!m_replanningTriggerPort.isClosed())
+            m_replanningTriggerPort.close();
+    }
+    catch(const std::exception& e)
+    {
+        std::cerr << e.what() << std::endl;
+        return false;
+    }
+    return true;
+}
+
+bool NavigationHelper::init(const yarp::os::Searchable& config)
+{
+    m_navigationReplanningDelay = config.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64();
+    m_navigationTriggerLoopRate = config.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32();
+    m_publishInfo = config.check("publishNavigationInfo", yarp::os::Value(false)).asBool();
+    m_simulationMode = config.check("simulationMode", yarp::os::Value(false)).asBool();
+    if (config.check("plannerMode", yarp::os::Value("manual")).asString() == "navigation")
+    {
+        //if in navigation mode we need this parameter to be true
+        m_publishInfo = true;
+    }
+    if (!m_publishInfo)
+    {   //exit the funnction if the infos are not needed
+        yInfo() << "[NavigationHelper::init] Configuring NavigationHelper without publishing infos on ports ";
+        return true;
+    }
+
+    // format check
+    if (m_navigationTriggerLoopRate<=0)
+    {
+        yError() << "[configure] navigationTriggerLoopRate must be strictly positive, instead is: " << m_navigationTriggerLoopRate;
+        return false;
+    }
+    if (m_navigationReplanningDelay<0)
+    {
+        yError() << "[configure] navigationTriggerLoopRate must be positive, instead is: " << m_navigationReplanningDelay;
+        return false;
+    }
+
+    std::string replanningTriggerPortPortName = m_portPrefix + "/replanning_trigger:o";
+    if (!m_replanningTriggerPort.open(replanningTriggerPortPortName))
+    {
+        yError() << "[NavigationHelper::init] Could not open" << replanningTriggerPortPortName << " port.";
+        return false;
+    }
+
+    m_runThreads = true;
+    m_navigationTriggerThread = std::thread(&NavigationHelper::computeNavigationTrigger, this);
+    return true;
+}
+
+// This thread synchronizes the walking-controller with the navigation stack.
+// Writes on a port a boolean value when to replan the path
+void NavigationHelper::computeNavigationTrigger()
+{
+    if (!m_publishInfo)
+    {   //exit the function if the infos are not needed
+        return;
+    }
+    bool trigger = false;   //flag used to fire the wait for sending the navigation replanning trigger
+    yInfo() << "[NavigationHelper::computeNavigationTrigger] Starting Thread";
+    yarp::os::NetworkClock myClock;
+    if (m_simulationMode)
+    {
+        myClock.open("/clock", "/navigationTriggerClock");
+    }
+    bool enteredDoubleSupport = false, exitDoubleSupport = true;
+    while (m_runThreads)
+    {
+        {
+            std::unique_lock<std::mutex> lock(m_updateFeetMutex);
+            if (m_leftInContact.size()>0 && m_rightInContact.size()>0)  //external consistency check
+            {
+                if (m_leftInContact.at(0) && m_rightInContact.at(0))    //double support check
+                {
+                    if (exitDoubleSupport)
+                    {
+                        enteredDoubleSupport = true;
+                        exitDoubleSupport = false;
+                    }
+                }
+                else
+                {
+                    if (! exitDoubleSupport)
+                    {
+                        trigger = true; //in this way we have only one trigger each exit of double support
+                    }
+                    exitDoubleSupport = true;
+                }
+            }
+            else
+            {
+                trigger = false;
+            }
+        }
+
+        //send the replanning trigger after a certain amount of seconds
+        if (trigger)
+        {
+            trigger = false;
+            //waiting -> could make it dependant by the current swing step duration
+            //if in simulation we need to sync the clock, we can't use the system clock
+            if (m_simulationMode)
+            {
+                myClock.delay(m_navigationReplanningDelay);
+            }
+            else
+            {
+                std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(m_navigationReplanningDelay*1000));
+            }
+            yInfo() << "[NavigationHelper::computeNavigationTrigger] Triggering navigation replanning";
+            auto& b = m_replanningTriggerPort.prepare();
+            b.clear();
+            b.add((yarp::os::Value)true);   //send the planning trigger
+            m_replanningTriggerPort.write();
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate));
+    }
+    yInfo() << "[NavigationHelper::computeNavigationTrigger] Terminated thread";
+}
+
+
+bool NavigationHelper::updateFeetDeques(const std::deque<bool> &left, const std::deque<bool> &right)
+{
+    std::unique_lock<std::mutex> lock(m_updateFeetMutex);
+    m_leftInContact = left;
+    m_rightInContact = right;
+    return true;
+}

src/WalkingModule/CMakeLists.txt

@@ -10,6 +10,6 @@ add_walking_controllers_application(
   NAME WalkingModule
   SOURCES src/main.cpp src/Module.cpp ${WalkingModule_THRIFT_GEN_FILES}
   HEADERS include/WalkingControllers/WalkingModule/Module.h
-  LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation ctrlLib
+  LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::NavigationHelper WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation ctrlLib
   SUBDIRECTORIES app)
 

src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini

@@ -0,0 +1,16 @@
+# Navigation trigger delay in seconds. Time to wait before sending the replanning command after exiting the double support stance.
+navigationReplanningDelay   0.9
+# Loop rate in hertz for the thread computing the navigation trigger
+navigationTriggerLoopRate   100     #Hz
+
+# Planner Mode: manual i.e. typical utilization of the planner for joystick use  - navigation i.e. path following behavior
+# If in NAVIGATION mode (1), the type of the controller also specifies the behaviour of the planner:
+# A- in direct mode the planner behaves like an omnidirectional floating base that interpolates the path. 
+# B- in personFollowing it tries to follow the path like an unicycle using state feedback linearization
+plannerMode             navigation
+
+# Flag to publish Navigation info anyway (indipendently by the plannerMode)
+publishNavigationInfo   1
+
+# Set to 0 if on the real robot - to 1 if in simulation on Gazebo
+simulationMode          1

src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini

@@ -54,6 +54,9 @@ sampling_time           0.01
 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link
 height_reference_frame  root_link
 
+# include navigation parameters
+[include NAVIGATION "./dcm_walking/common/navigation.ini"]
+
 # include robot control parameters
 [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"]
 

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -50,6 +50,8 @@
 
 #include <WalkingControllers/TimeProfiler/TimeProfiler.h>
 
+#include <WalkingControllers/NavigationHelper/NavigationHelper.h>
+
 // iCub-ctrl
 #include <iCub/ctrl/filters.h>
 
@@ -155,6 +157,9 @@ namespace WalkingControllers
         size_t m_feedbackAttempts;
         double m_feedbackAttemptDelay;
 
+        NavigationHelper m_navHelper;
+        bool m_navHelperUsed;
+
         // debug
         std::unique_ptr<iCub::ctrl::Integrator> m_velocityIntegral{nullptr};
 

src/WalkingModule/src/Module.cpp

@@ -20,6 +20,8 @@
 #include <yarp/sig/Matrix.h>
 #include <yarp/sig/Vector.h>
 #include <yarp/os/LogStream.h>
+#include <yarp/os/Stamp.h>
+#include <yarp/os/NetworkClock.h>
 
 // iDynTree
 #include <iDynTree/Core/VectorFixSize.h>
@@ -78,6 +80,9 @@ bool WalkingModule::advanceReferenceSignals()
 
     m_leftInContact.pop_front();
     m_leftInContact.push_back(m_leftInContact.back());
+
+    if (m_navHelperUsed)
+        m_navHelper.updateFeetDeques(m_leftInContact, m_rightInContact);
 
     m_isLeftFixedFrame.pop_front();
     m_isLeftFixedFrame.push_back(m_isLeftFixedFrame.back());
@@ -264,6 +269,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf)
     yarp::os::Bottle ellipseMangerOptions = rf.findGroup("FREE_SPACE_ELLIPSE_MANAGER");
     trajectoryPlannerOptions.append(generalOptions);
     trajectoryPlannerOptions.append(ellipseMangerOptions);
+
     if(!m_trajectoryGenerator->initialize(trajectoryPlannerOptions))
     {
         yError() << "[configure] Unable to initialize the planner.";
@@ -442,6 +448,24 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf)
     m_qDesired.resize(m_robotControlHelper->getActuatedDoFs());
     m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs());
 
+    yarp::os::Bottle navigationOptions = rf.findGroup("NAVIGATION");
+
+    // start the threads used for computing navigation needed infos
+    if (!navigationOptions.isNull())
+    {
+        m_navHelperUsed = true;
+        if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact))
+        {
+            yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper";
+        }
+        yInfo() << "[WalkingModule::configure] Configured Navigation Helper.";
+    }
+    else
+    {
+        m_navHelperUsed = false;
+    }
+
+
     yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot.";
 
     return true;
@@ -470,6 +494,9 @@ bool WalkingModule::close()
 {
     if(m_dumpData)
         m_loggerPort.close();
+
+    if(m_navHelperUsed)
+        m_navHelper.closeHelper();
 
     // restore PID
     m_robotControlHelper->getPIDHandler().restorePIDs();
@@ -1546,6 +1573,9 @@ bool WalkingModule::updateTrajectories(const size_t& mergePoint)
     StdUtilities::appendVectorToDeque(leftInContact, m_leftInContact, mergePoint);
     StdUtilities::appendVectorToDeque(rightInContact, m_rightInContact, mergePoint);
 
+    if (m_navHelperUsed)
+        m_navHelper.updateFeetDeques(m_leftInContact, m_rightInContact);
+
     StdUtilities::appendVectorToDeque(comHeightTrajectory, m_comHeightTrajectory, mergePoint);
     StdUtilities::appendVectorToDeque(comHeightVelocity, m_comHeightVelocity, mergePoint);