Navigation

.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/home/user1/robotology-superbuild/build/src/walking-controllers/src/WalkingModule/",
+                "/home/user1/robotology-superbuild/build/install/include/**",
+                "/home/user1/robotology-superbuild/build/install/include/BipedalLocomotion/ParametersHandler/**",
+                "/home/user1/robotology-superbuild/build/install/include/BipedalLocomotion/**",
+                "/home/user1/robotology-superbuild/build/install/include/OsqpEigen/**",
+                "/home/user1/robotology-superbuild/src/UnicyclePlanner/**",
+                "/usr/include/eigen3/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "gnu17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,78 @@
+{
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "cinttypes": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "semaphore": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "*.ipp": "cpp"
+    }
+}

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 <[email protected]>
+
+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::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::StdUtilities ctrlLib
+  PRIVATE_LINK_LIBRARIES Eigen3::Eigen)

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

@@ -0,0 +1,73 @@
+/**
+ * @file NavigationHelper.h
+ * @authors Simone Micheletti <[email protected]>
+ * @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 <WalkingControllers/WalkingModule/Module.h>
+#include <WalkingControllers/KinDynWrapper/Wrapper.h>
+#include <WalkingControllers/TrajectoryPlanner/StableDCMModel.h>
+#include <WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h>
+
+namespace WalkingControllers
+{
+    class NavigationHelper
+    {
+    private:
+        yarp::os::BufferedPort<yarp::os::Bottle> m_unicyclePort; /**< Port that streams odometry info of the virtual unicycle. */
+        yarp::os::BufferedPort<yarp::os::Bottle> m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */
+        yarp::os::BufferedPort<yarp::os::Bottle> m_feetPort; /**< Feet port vector of feet positions (left, right). */
+        yarp::os::BufferedPort<yarp::sig::Vector> m_plannedCoMPositionPort; /**< Desired CoM position port for naviagation purposes. */
+        std::atomic<bool> m_runThreads{false};
+        std::deque<bool>* m_leftInContact;
+        std::deque<bool>* m_rightInContact;
+        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_wasInDoubleSupport;
+        bool m_publishInfo;
+        std::deque<iDynTree::Vector3> m_desiredCoM_Trajectory;  /**< Deque containing the desired CoM trajectory projected on the ground in pose x, y, theta. */
+
+        std::thread m_virtualUnicyclePubliserThread; /**< Thread for publishing the state of the unicycle used in the TrajectoryGenerator. */
+        std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */
+
+        const std::string m_portPrefix = "/navigation_helper";
+
+    public:
+        NavigationHelper();
+        ~NavigationHelper();
+
+        bool setThreads(bool status);
+        bool closeThreads();
+        bool closeHelper();
+        bool init(const yarp::os::Searchable& config, std::deque<bool> &leftInContact, std::deque<bool> &rightInContact,
+                    std::unique_ptr<WalkingFK> &FKSolver, 
+                    std::unique_ptr<StableDCMModel> &stableDCMModel, 
+                    std::unique_ptr<TrajectoryGenerator> &trajectoryGenerator
+                    );
+        void computeNavigationTrigger();
+        void computeVirtualUnicycleThread(std::unique_ptr<WalkingFK> &FKSolver, 
+                                            std::unique_ptr<StableDCMModel> &stableDCMModel, 
+                                            std::unique_ptr<TrajectoryGenerator> &trajectoryGenerator
+                                            );
+
+        bool publishPlannedFootsteps(std::unique_ptr<TrajectoryGenerator> &trajectoryGenerator);
+        bool publishCoM(bool newTrajectoryMerged, 
+                        std::deque<iDynTree::Vector2> &m_DCMPositionDesired, 
+                        std::unique_ptr<StableDCMModel> &m_stableDCMModel,
+                        std::deque<iDynTree::Transform> &m_leftTrajectory,
+                        std::deque<iDynTree::Transform> &m_rightTrajectory);
+    };
+}
+
+#endif