From 5bc915452e2d2d5f61c90664443da5add9d5a275 Mon Sep 17 00:00:00 2001 From: Simone Date: Wed, 12 Apr 2023 14:15:56 +0000 Subject: [PATCH 01/20] Replanning trigger for the navigation stack. --- .../dcm_walking_with_joypad.ini | 5 + .../WalkingControllers/WalkingModule/Module.h | 12 +++ src/WalkingModule/src/Module.cpp | 96 +++++++++++++++++++ 3 files changed, 113 insertions(+) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini index 73717ec9..8792c43c 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini @@ -54,6 +54,11 @@ 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 +# Navigation trigger delay in seconds. Time to wait before sending the replanning command after exiting the double support stance. +navigationReplanningDelay 0.9 +# Loop rate for the thread computing the navigation trigger +navigationTriggerLoopRate 100 + # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 9c415cff..bd51a517 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -141,6 +141,7 @@ namespace WalkingControllers yarp::os::BufferedPort m_desiredUnyciclePositionPort; /**< Desired robot position port. */ bool m_newTrajectoryRequired; /**< if true a new trajectory will be merged soon. (after m_newTrajectoryMergeCounter - 2 cycles). */ + bool m_newTrajectoryMerged; /**< true if a new trajectory has been just merged. */ size_t m_newTrajectoryMergeCounter; /**< The new trajectory will be merged after m_newTrajectoryMergeCounter - 2 cycles. */ bool m_useRootLinkForHeight; @@ -155,6 +156,12 @@ namespace WalkingControllers size_t m_feedbackAttempts; double m_feedbackAttemptDelay; + std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ + bool m_runThreads; + yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ + 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*/ + // debug std::unique_ptr m_velocityIntegral{nullptr}; @@ -277,6 +284,11 @@ namespace WalkingControllers */ void applyGoalScaling(yarp::sig::Vector &plannerInput); + /** + * Parallel thread for publishing the trigger for the navigation's planner. + */ + void computeNavigationTrigger(); + public: /** diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 9b30624a..e4a3543a 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -11,6 +11,7 @@ #include #include #include +#include // YARP #include @@ -20,6 +21,8 @@ #include #include #include +#include +#include // iDynTree #include @@ -264,6 +267,20 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Bottle ellipseMangerOptions = rf.findGroup("FREE_SPACE_ELLIPSE_MANAGER"); trajectoryPlannerOptions.append(generalOptions); trajectoryPlannerOptions.append(ellipseMangerOptions); + m_navigationReplanningDelay = trajectoryPlannerOptions.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64(); + m_navigationTriggerLoopRate = trajectoryPlannerOptions.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); + // 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; + } + if(!m_trajectoryGenerator->initialize(trajectoryPlannerOptions)) { yError() << "[configure] Unable to initialize the planner."; @@ -442,6 +459,18 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); + // open port for navigation trigger + std::string replanningTriggerPortPortName = "/" + getName() + "/replanning_trigger:o"; + if (!m_replanningTriggerPort.open(replanningTriggerPortPortName)) + { + yError() << "[WalkingModule::configure] Could not open" << replanningTriggerPortPortName << " port."; + return false; + } + + // start the threads used for computing navigation needed infos + m_runThreads = true; + m_navigationTriggerThread = std::thread(&WalkingModule::computeNavigationTrigger, this); + yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; return true; @@ -470,6 +499,14 @@ bool WalkingModule::close() { if(m_dumpData) m_loggerPort.close(); + //Close parallel threads + m_runThreads = false; + yInfo() << "Closing m_navigationTriggerThread"; + if(m_navigationTriggerThread.joinable()) + { + m_navigationTriggerThread.join(); + m_navigationTriggerThread = std::thread(); + } // restore PID m_robotControlHelper->getPIDHandler().restorePIDs(); @@ -480,6 +517,7 @@ bool WalkingModule::close() // close the ports m_rpcPort.close(); m_desiredUnyciclePositionPort.close(); + m_replanningTriggerPort.close(); // close the connection with robot if(!m_robotControlHelper->close()) @@ -770,6 +808,7 @@ bool WalkingModule::updateModule() } m_newTrajectoryRequired = false; resetTrajectory = true; + m_newTrajectoryMerged = true; } m_newTrajectoryMergeCounter--; @@ -1751,3 +1790,60 @@ bool WalkingModule::stopWalking() m_robotState = WalkingFSM::Stopped; 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 WalkingModule::computeNavigationTrigger() +{ + bool trigger = false; //flag used to fire the wait for sending the navigation replanning trigger + yInfo() << "[WalkingModule::computeNavigationTrigger] Starting Thread"; + yarp::os::NetworkClock myClock; + myClock.open("/clock", "/navigationTriggerClock"); + bool enteredDoubleSupport = false, exitDoubleSupport = true; + while (m_runThreads) + { + // Block the thread until the robot is in the walking state + if (m_robotState != WalkingFSM::Walking) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate)); + continue; + } + + //double support check + if (m_leftInContact.size()>0 && m_rightInContact.size()>0) //external consistency check + { + if (m_leftInContact[0] && m_rightInContact[0]) + { + 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; + } + } + + //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 + myClock.delay(m_navigationReplanningDelay); + yDebug() << "[WalkingModule::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() << "[WalkingModule::computeNavigationTrigger] Terminating thread"; +} From 995090213782a024893a4bc82444ba97eaa4edaf Mon Sep 17 00:00:00 2001 From: Simone Date: Mon, 17 Apr 2023 15:42:57 +0000 Subject: [PATCH 02/20] moved all navigation pertinent functions and parameters to a separate file --- src/CMakeLists.txt | 1 + src/NavigationHelper/CMakeLists.txt | 10 ++ .../NavigationHelper/NavigationHelper.h | 48 +++++ src/NavigationHelper/src/NavigationHelper.cpp | 166 ++++++++++++++++++ src/WalkingModule/CMakeLists.txt | 2 +- .../dcm_walking/common/navigation.ini | 10 ++ .../dcm_walking_with_joypad.ini | 6 +- .../WalkingControllers/WalkingModule/Module.h | 10 +- src/WalkingModule/src/Module.cpp | 40 ++--- 9 files changed, 252 insertions(+), 41 deletions(-) create mode 100644 src/NavigationHelper/CMakeLists.txt create mode 100644 src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h create mode 100644 src/NavigationHelper/src/NavigationHelper.cpp create mode 100644 src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9f079c7a..71c5d5d1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -14,3 +14,4 @@ add_subdirectory(KinDynWrapper) add_subdirectory(RetargetingHelper) add_subdirectory(WalkingModule) add_subdirectory(JoypadModule) +add_subdirectory(NavigationHelper) diff --git a/src/NavigationHelper/CMakeLists.txt b/src/NavigationHelper/CMakeLists.txt new file mode 100644 index 00000000..1171ea8e --- /dev/null +++ b/src/NavigationHelper/CMakeLists.txt @@ -0,0 +1,10 @@ +# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +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) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h new file mode 100644 index 00000000..219d50b7 --- /dev/null +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -0,0 +1,48 @@ +/** + * @file NavigationHelper.h + * @authors Simone Micheletti + * @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 +#include +#include +#include +#include + +namespace WalkingControllers +{ + class NavigationHelper + { + private: + yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ + std::atomic m_runThreads{false}; + std::deque* m_leftInContact; + std::deque* 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*/ + + std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ + + public: + NavigationHelper(); + ~NavigationHelper(); + + bool setThreads(bool status); + bool closeThreads(); + bool closeHelper(); + bool init(const yarp::os::Searchable& config, + std::deque &leftInContact, + std::deque &rightInContact + ); + + void computeNavigationTrigger(); + }; +} + +#endif diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp new file mode 100644 index 00000000..751c1d3d --- /dev/null +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -0,0 +1,166 @@ +/** + * @file NavigationHelper.cpp + * @authors Simone Micheletti + * @copyright 2023 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2023 + */ + +#include +#include +#include + +#include +#include + +#include + + +using namespace WalkingControllers; + +NavigationHelper::NavigationHelper() +{ +} + +NavigationHelper::~NavigationHelper() +{ +} + +bool NavigationHelper::setThreads(bool status) +{ + m_runThreads = status; + return true; +} + +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, std::deque &leftInContact, std::deque &rightInContact ) +{ + m_leftInContact = &leftInContact; + m_rightInContact = &rightInContact; + + std::string replanningTriggerPortPortName = "/replanning_trigger:o"; + if (!m_replanningTriggerPort.open(replanningTriggerPortPortName)) + { + yError() << "[NavigationHelper::init] Could not open" << replanningTriggerPortPortName << " port."; + return false; + } + + yarp::os::Bottle& options = config.findGroup("NAVIGATION"); + m_navigationReplanningDelay = options.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64(); + m_navigationTriggerLoopRate = options.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); + // 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; + } + + 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() +{ + bool trigger = false; //flag used to fire the wait for sending the navigation replanning trigger + yInfo() << "[NavigationHelper::computeNavigationTrigger] Starting Thread"; + yarp::os::NetworkClock myClock; + myClock.open("/clock", "/navigationTriggerClock"); + bool enteredDoubleSupport = false, exitDoubleSupport = true; + while (m_runThreads) + { + // Block the thread until the robot is in the walking state + if (m_leftInContact == nullptr || m_rightInContact == nullptr) + { + //kill thread + yError() << "[NavigationHelper::computeNavigationTrigger] null pointer to m_leftInContact or m_rightInContact. Killing thread."; + return; + } + + //double support check + if (m_leftInContact->size()>0 && m_rightInContact->size()>0) //external consistency check + { + if (m_leftInContact->at(0) && m_rightInContact->at(0)) + { + 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; + yWarning() << "[NavigationHelper::computeNavigationTrigger] one of the feet deques is empty" ; + } + + //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 + myClock.delay(m_navigationReplanningDelay); + yDebug() << "[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"; +} \ No newline at end of file diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index a01fbf76..72ca89a2 100644 --- a/src/WalkingModule/CMakeLists.txt +++ b/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) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini new file mode 100644 index 00000000..1fc5bf3b --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini @@ -0,0 +1,10 @@ +# Navigation trigger delay in seconds. Time to wait before sending the replanning command after exiting the double support stance. +navigationReplanningDelay 0.9 +# Loop rate for the thread computing the navigation trigger +navigationTriggerLoopRate 100 + +# 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 +plannerMode navigation \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini index 8792c43c..aa7ee84b 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini @@ -54,10 +54,8 @@ 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 -# Navigation trigger delay in seconds. Time to wait before sending the replanning command after exiting the double support stance. -navigationReplanningDelay 0.9 -# Loop rate for the thread computing the navigation trigger -navigationTriggerLoopRate 100 +# include navigation parameters +[include NAVIGATION "./dcm_walking/common/navigation.ini"] # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index bd51a517..ca562a9b 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -50,6 +50,8 @@ #include +#include + // iCub-ctrl #include @@ -156,12 +158,8 @@ namespace WalkingControllers size_t m_feedbackAttempts; double m_feedbackAttemptDelay; - std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ - bool m_runThreads; - yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ - 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*/ - + NavigationHelper m_navHelper; + // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index e4a3543a..d2591fcf 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -267,19 +267,6 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Bottle ellipseMangerOptions = rf.findGroup("FREE_SPACE_ELLIPSE_MANAGER"); trajectoryPlannerOptions.append(generalOptions); trajectoryPlannerOptions.append(ellipseMangerOptions); - m_navigationReplanningDelay = trajectoryPlannerOptions.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64(); - m_navigationTriggerLoopRate = trajectoryPlannerOptions.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); - // 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; - } if(!m_trajectoryGenerator->initialize(trajectoryPlannerOptions)) { @@ -459,17 +446,17 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); - // open port for navigation trigger - std::string replanningTriggerPortPortName = "/" + getName() + "/replanning_trigger:o"; - if (!m_replanningTriggerPort.open(replanningTriggerPortPortName)) + yarp::os::Bottle navigationOptions = rf.findGroup("NAVIGATION"); + if (/* condition */) { - yError() << "[WalkingModule::configure] Could not open" << replanningTriggerPortPortName << " port."; - return false; + /* code */ } - + // start the threads used for computing navigation needed infos - m_runThreads = true; - m_navigationTriggerThread = std::thread(&WalkingModule::computeNavigationTrigger, this); + if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact)) + { + yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper"; + } yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -499,14 +486,8 @@ bool WalkingModule::close() { if(m_dumpData) m_loggerPort.close(); - //Close parallel threads - m_runThreads = false; - yInfo() << "Closing m_navigationTriggerThread"; - if(m_navigationTriggerThread.joinable()) - { - m_navigationTriggerThread.join(); - m_navigationTriggerThread = std::thread(); - } + + m_navHelper.closeHelper(); // restore PID m_robotControlHelper->getPIDHandler().restorePIDs(); @@ -517,7 +498,6 @@ bool WalkingModule::close() // close the ports m_rpcPort.close(); m_desiredUnyciclePositionPort.close(); - m_replanningTriggerPort.close(); // close the connection with robot if(!m_robotControlHelper->close()) From 2d144465dfde0b16344934cda5e84c875e044c06 Mon Sep 17 00:00:00 2001 From: Simone Date: Mon, 17 Apr 2023 15:47:50 +0000 Subject: [PATCH 03/20] removed unused dependencies --- src/NavigationHelper/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NavigationHelper/CMakeLists.txt b/src/NavigationHelper/CMakeLists.txt index 1171ea8e..d448d045 100644 --- a/src/NavigationHelper/CMakeLists.txt +++ b/src/NavigationHelper/CMakeLists.txt @@ -6,5 +6,5 @@ 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 + PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities ctrlLib PRIVATE_LINK_LIBRARIES Eigen3::Eigen) From 2ac553154247189ba567ae0bd9869456983fda82 Mon Sep 17 00:00:00 2001 From: Simone Date: Mon, 17 Apr 2023 15:48:19 +0000 Subject: [PATCH 04/20] bugfix: Removed unused code --- src/WalkingModule/src/Module.cpp | 61 -------------------------------- 1 file changed, 61 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index d2591fcf..fe26ada0 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -447,10 +447,6 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); yarp::os::Bottle navigationOptions = rf.findGroup("NAVIGATION"); - if (/* condition */) - { - /* code */ - } // start the threads used for computing navigation needed infos if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact)) @@ -1770,60 +1766,3 @@ bool WalkingModule::stopWalking() m_robotState = WalkingFSM::Stopped; 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 WalkingModule::computeNavigationTrigger() -{ - bool trigger = false; //flag used to fire the wait for sending the navigation replanning trigger - yInfo() << "[WalkingModule::computeNavigationTrigger] Starting Thread"; - yarp::os::NetworkClock myClock; - myClock.open("/clock", "/navigationTriggerClock"); - bool enteredDoubleSupport = false, exitDoubleSupport = true; - while (m_runThreads) - { - // Block the thread until the robot is in the walking state - if (m_robotState != WalkingFSM::Walking) - { - std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate)); - continue; - } - - //double support check - if (m_leftInContact.size()>0 && m_rightInContact.size()>0) //external consistency check - { - if (m_leftInContact[0] && m_rightInContact[0]) - { - 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; - } - } - - //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 - myClock.delay(m_navigationReplanningDelay); - yDebug() << "[WalkingModule::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() << "[WalkingModule::computeNavigationTrigger] Terminating thread"; -} From f5a0dbb4253bfa6b1b4d9b5c0774efa7e829bf88 Mon Sep 17 00:00:00 2001 From: Simone Date: Mon, 17 Apr 2023 15:55:14 +0000 Subject: [PATCH 05/20] removed continuous print --- src/NavigationHelper/src/NavigationHelper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 751c1d3d..8cb6a9b2 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -144,7 +144,6 @@ void NavigationHelper::computeNavigationTrigger() else { trigger = false; - yWarning() << "[NavigationHelper::computeNavigationTrigger] one of the feet deques is empty" ; } //send the replanning trigger after a certain amount of seconds From 3d6ec2965188d605e712741b59307b7439b5cfdf Mon Sep 17 00:00:00 2001 From: Simone Date: Mon, 17 Apr 2023 16:06:48 +0000 Subject: [PATCH 06/20] removed setStatus. Added comments in header --- .../NavigationHelper/NavigationHelper.h | 33 +++++++++++++++---- src/NavigationHelper/src/NavigationHelper.cpp | 10 ++---- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index 219d50b7..5e78855a 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -21,26 +21,45 @@ namespace WalkingControllers { private: yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ - std::atomic m_runThreads{false}; - std::deque* m_leftInContact; - std::deque* 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*/ + std::atomic m_runThreads{false}; /**< Global flag that allows the looping of the threads. */ + std::deque* m_leftInContact; /**< Pointer to the deques in Module of the left feet contacts status. */ + std::deque* m_rightInContact; /**< Pointer to 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*/ - std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ + std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ public: NavigationHelper(); ~NavigationHelper(); - bool setThreads(bool status); + /** + * 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. + * @param leftInContact deque in Module.cpp of the left feet contacts status. + * @param rightInContact deque in Module.cpp of the right feet contacts status. + * @return true/false in case of success/failure. + */ bool init(const yarp::os::Searchable& config, std::deque &leftInContact, std::deque &rightInContact ); + /** + * Function launched by the looping thread + */ void computeNavigationTrigger(); }; } diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 8cb6a9b2..cdb0434e 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -21,15 +21,9 @@ using namespace WalkingControllers; NavigationHelper::NavigationHelper() { } - -NavigationHelper::~NavigationHelper() -{ -} -bool NavigationHelper::setThreads(bool status) +NavigationHelper::~NavigationHelper() { - m_runThreads = status; - return true; } bool NavigationHelper::closeThreads() @@ -113,7 +107,7 @@ void NavigationHelper::computeNavigationTrigger() bool enteredDoubleSupport = false, exitDoubleSupport = true; while (m_runThreads) { - // Block the thread until the robot is in the walking state + //ptr check to avoid coredump if (m_leftInContact == nullptr || m_rightInContact == nullptr) { //kill thread From 04a99e4f6f9a19f4710dd25d3b5abfb939f6ce2d Mon Sep 17 00:00:00 2001 From: Simone Date: Tue, 18 Apr 2023 16:49:38 +0000 Subject: [PATCH 07/20] added port prefix and publish info config flag --- .../NavigationHelper/NavigationHelper.h | 4 ++- src/NavigationHelper/src/NavigationHelper.cpp | 31 ++++++++++++++----- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index 5e78855a..9f17955f 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -26,9 +26,11 @@ namespace WalkingControllers std::deque* m_rightInContact; /**< Pointer to 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; 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(); diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index cdb0434e..8b67d9eb 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -69,16 +69,20 @@ bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque m_leftInContact = &leftInContact; m_rightInContact = &rightInContact; - std::string replanningTriggerPortPortName = "/replanning_trigger:o"; - if (!m_replanningTriggerPort.open(replanningTriggerPortPortName)) + 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(); + if (config.check("plannerMode", yarp::os::Value("manual")).asString() == "navigation") { - yError() << "[NavigationHelper::init] Could not open" << replanningTriggerPortPortName << " port."; - return false; + //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; } - yarp::os::Bottle& options = config.findGroup("NAVIGATION"); - m_navigationReplanningDelay = options.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64(); - m_navigationTriggerLoopRate = options.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); // format check if (m_navigationTriggerLoopRate<=0) { @@ -91,6 +95,13 @@ bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque 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; @@ -100,6 +111,10 @@ bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque // Writes on a port a boolean value when to replan the path void NavigationHelper::computeNavigationTrigger() { + if (!m_publishInfo) + { //exit the funnction 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; @@ -156,4 +171,4 @@ void NavigationHelper::computeNavigationTrigger() std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate)); } yInfo() << "[NavigationHelper::computeNavigationTrigger] Terminated thread"; -} \ No newline at end of file +} From 2dfffb760feaa0d7091a553370c9e2e245ec4099 Mon Sep 17 00:00:00 2001 From: Simone Micheletti <86918431+SimoneMic@users.noreply.github.com> Date: Thu, 27 Apr 2023 18:47:00 +0200 Subject: [PATCH 08/20] Update src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini Co-authored-by: Stefano Dafarra --- .../robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini index 1fc5bf3b..e2b9ab06 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini @@ -7,4 +7,4 @@ navigationTriggerLoopRate 100 # 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 -plannerMode navigation \ No newline at end of file +plannerMode navigation From d87e0c634259edfbe5035e778f08638b7d7bf903 Mon Sep 17 00:00:00 2001 From: Simone Micheletti <86918431+SimoneMic@users.noreply.github.com> Date: Thu, 27 Apr 2023 18:47:18 +0200 Subject: [PATCH 09/20] Update src/NavigationHelper/src/NavigationHelper.cpp Co-authored-by: Stefano Dafarra --- src/NavigationHelper/src/NavigationHelper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 8b67d9eb..f1206772 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -112,7 +112,7 @@ bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque void NavigationHelper::computeNavigationTrigger() { if (!m_publishInfo) - { //exit the funnction if the infos are not needed + { //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 From 0a8c6afc2058217b3ed69353055483be1bd09886 Mon Sep 17 00:00:00 2001 From: Simone Date: Thu, 27 Apr 2023 17:11:24 +0000 Subject: [PATCH 10/20] removed leftovers variables and include not needed --- .../include/WalkingControllers/WalkingModule/Module.h | 6 ------ src/WalkingModule/src/Module.cpp | 2 -- 2 files changed, 8 deletions(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index ca562a9b..1ac5a28d 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -143,7 +143,6 @@ namespace WalkingControllers yarp::os::BufferedPort m_desiredUnyciclePositionPort; /**< Desired robot position port. */ bool m_newTrajectoryRequired; /**< if true a new trajectory will be merged soon. (after m_newTrajectoryMergeCounter - 2 cycles). */ - bool m_newTrajectoryMerged; /**< true if a new trajectory has been just merged. */ size_t m_newTrajectoryMergeCounter; /**< The new trajectory will be merged after m_newTrajectoryMergeCounter - 2 cycles. */ bool m_useRootLinkForHeight; @@ -282,11 +281,6 @@ namespace WalkingControllers */ void applyGoalScaling(yarp::sig::Vector &plannerInput); - /** - * Parallel thread for publishing the trigger for the navigation's planner. - */ - void computeNavigationTrigger(); - public: /** diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index fe26ada0..73d47bea 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -11,7 +11,6 @@ #include #include #include -#include // YARP #include @@ -784,7 +783,6 @@ bool WalkingModule::updateModule() } m_newTrajectoryRequired = false; resetTrajectory = true; - m_newTrajectoryMerged = true; } m_newTrajectoryMergeCounter--; From 95b59cffcd154726982b4eb603d610cad8f9f523 Mon Sep 17 00:00:00 2001 From: Simone Date: Thu, 27 Apr 2023 17:21:42 +0000 Subject: [PATCH 11/20] navigation helper is initialized only if its conf options are found --- src/WalkingModule/src/Module.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 73d47bea..827845f5 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -448,9 +448,12 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yarp::os::Bottle navigationOptions = rf.findGroup("NAVIGATION"); // start the threads used for computing navigation needed infos - if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact)) + if (!navigationOptions.isNull()) { - yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper"; + if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact)) + { + yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper"; + } } yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; From a0855711c42517d8d6f63245b0fe81345d72c725 Mon Sep 17 00:00:00 2001 From: Simone Date: Thu, 27 Apr 2023 17:24:23 +0000 Subject: [PATCH 12/20] removed Yarp stamp and clock header includes --- src/NavigationHelper/src/NavigationHelper.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index f1206772..810a62e9 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -10,9 +10,6 @@ #include #include -#include -#include - #include From c48bd21c04f889399220ee312b8f1aaec9537902 Mon Sep 17 00:00:00 2001 From: Simone Date: Fri, 28 Apr 2023 11:52:38 +0000 Subject: [PATCH 13/20] updated comments and improved explainations --- .../ergoCubGazeboV1/dcm_walking/common/navigation.ini | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini index e2b9ab06..602b4304 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini @@ -1,10 +1,10 @@ # Navigation trigger delay in seconds. Time to wait before sending the replanning command after exiting the double support stance. navigationReplanningDelay 0.9 -# Loop rate for the thread computing the navigation trigger -navigationTriggerLoopRate 100 +# 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: +# 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 +# B- in personFollowing it tries to follow the path like an unicycle using state feedback linearization plannerMode navigation From 6725373a67db0453535a382e4026cc102b38a138 Mon Sep 17 00:00:00 2001 From: Simone Date: Fri, 28 Apr 2023 12:35:19 +0000 Subject: [PATCH 14/20] added thread safety for updating feet contacts deques --- .../NavigationHelper/NavigationHelper.h | 8 ++- src/NavigationHelper/src/NavigationHelper.cpp | 54 ++++++++++--------- .../WalkingControllers/WalkingModule/Module.h | 1 + src/WalkingModule/src/Module.cpp | 12 ++++- 4 files changed, 47 insertions(+), 28 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index 9f17955f..4b94de51 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -14,6 +14,7 @@ #include #include #include +#include namespace WalkingControllers { @@ -22,12 +23,13 @@ namespace WalkingControllers private: yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ std::atomic m_runThreads{false}; /**< Global flag that allows the looping of the threads. */ - std::deque* m_leftInContact; /**< Pointer to the deques in Module of the left feet contacts status. */ - std::deque* m_rightInContact; /**< Pointer to the deques in Module of the left feet contacts status. */ + std::deque m_leftInContact; /**< Pointer to the deques in Module of the left feet contacts status. */ + std::deque m_rightInContact; /**< Pointer to 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; std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ + std::mutex m_updateFeetMutex; const std::string m_portPrefix = "/navigation_helper"; @@ -63,6 +65,8 @@ namespace WalkingControllers * Function launched by the looping thread */ void computeNavigationTrigger(); + + bool updateFeetDeques(std::deque left, std::deque right); }; } diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 810a62e9..26d780d7 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -63,8 +63,8 @@ bool NavigationHelper::closeHelper() bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque &leftInContact, std::deque &rightInContact ) { - m_leftInContact = &leftInContact; - m_rightInContact = &rightInContact; + m_leftInContact = leftInContact; + m_rightInContact = rightInContact; m_navigationReplanningDelay = config.check("navigationReplanningDelay", yarp::os::Value(0.9)).asFloat64(); m_navigationTriggerLoopRate = config.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); @@ -119,38 +119,33 @@ void NavigationHelper::computeNavigationTrigger() bool enteredDoubleSupport = false, exitDoubleSupport = true; while (m_runThreads) { - //ptr check to avoid coredump - if (m_leftInContact == nullptr || m_rightInContact == nullptr) - { - //kill thread - yError() << "[NavigationHelper::computeNavigationTrigger] null pointer to m_leftInContact or m_rightInContact. Killing thread."; - return; - } - //double support check - if (m_leftInContact->size()>0 && m_rightInContact->size()>0) //external consistency check { - if (m_leftInContact->at(0) && m_rightInContact->at(0)) + const std::lock_guard lock(m_updateFeetMutex); + if (m_leftInContact.size()>0 && m_rightInContact.size()>0) //external consistency check { - if (exitDoubleSupport) + if (m_leftInContact.at(0) && m_rightInContact.at(0)) + { + if (exitDoubleSupport) + { + enteredDoubleSupport = true; + exitDoubleSupport = false; + } + } + else { - enteredDoubleSupport = true; - exitDoubleSupport = false; + if (! exitDoubleSupport) + { + trigger = true; //in this way we have only one trigger each exit of double support + } + exitDoubleSupport = true; } } else { - if (! exitDoubleSupport) - { - trigger = true; //in this way we have only one trigger each exit of double support - } - exitDoubleSupport = true; + trigger = false; + } } - } - else - { - trigger = false; - } //send the replanning trigger after a certain amount of seconds if (trigger) @@ -169,3 +164,12 @@ void NavigationHelper::computeNavigationTrigger() } yInfo() << "[NavigationHelper::computeNavigationTrigger] Terminated thread"; } + + +bool NavigationHelper::updateFeetDeques(std::deque left, std::deque right) +{ + const std::lock_guard lock(m_updateFeetMutex); + m_leftInContact = left; + m_rightInContact = right; + return true; +} diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 1ac5a28d..397025f0 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -158,6 +158,7 @@ namespace WalkingControllers double m_feedbackAttemptDelay; NavigationHelper m_navHelper; + bool m_navHelperUsed; // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 827845f5..77009f64 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -450,11 +450,17 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) // 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"; } } + else + { + m_navHelperUsed = false; + } + yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; @@ -485,7 +491,8 @@ bool WalkingModule::close() if(m_dumpData) m_loggerPort.close(); - m_navHelper.closeHelper(); + if(m_navHelperUsed) + m_navHelper.closeHelper(); // restore PID m_robotControlHelper->getPIDHandler().restorePIDs(); @@ -1562,6 +1569,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); From 56e6997a333deda8e0f1a01caed5651a70230f0e Mon Sep 17 00:00:00 2001 From: Simone Date: Fri, 28 Apr 2023 13:48:07 +0000 Subject: [PATCH 15/20] added config param to switch between system and network clock for navigation trigger delay --- .../NavigationHelper/NavigationHelper.h | 3 ++- src/NavigationHelper/src/NavigationHelper.cpp | 16 ++++++++++++++-- .../dcm_walking/common/navigation.ini | 6 ++++++ 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index 4b94de51..e4321573 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -29,7 +29,8 @@ namespace WalkingControllers int m_navigationTriggerLoopRate; /**< Loop rate for the thread computing the navigation trigger*/ bool m_publishInfo; std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ - std::mutex m_updateFeetMutex; + std::mutex m_updateFeetMutex; + bool m_simulationMode{false}; const std::string m_portPrefix = "/navigation_helper"; diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 26d780d7..7cc06c56 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -69,6 +69,7 @@ bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque 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 @@ -115,7 +116,10 @@ void NavigationHelper::computeNavigationTrigger() bool trigger = false; //flag used to fire the wait for sending the navigation replanning trigger yInfo() << "[NavigationHelper::computeNavigationTrigger] Starting Thread"; yarp::os::NetworkClock myClock; - myClock.open("/clock", "/navigationTriggerClock"); + if (m_simulationMode) + { + myClock.open("/clock", "/navigationTriggerClock"); + } bool enteredDoubleSupport = false, exitDoubleSupport = true; while (m_runThreads) { @@ -152,7 +156,15 @@ void NavigationHelper::computeNavigationTrigger() { trigger = false; //waiting -> could make it dependant by the current swing step duration - myClock.delay(m_navigationReplanningDelay); + //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(m_navigationReplanningDelay*1000)); + } yDebug() << "[NavigationHelper::computeNavigationTrigger] Triggering navigation replanning"; auto& b = m_replanningTriggerPort.prepare(); b.clear(); diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini index 602b4304..73b2a070 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini @@ -8,3 +8,9 @@ navigationTriggerLoopRate 100 #Hz # 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 From c78bb24142103b62941357cf1176f765977439c3 Mon Sep 17 00:00:00 2001 From: Simone Date: Fri, 28 Apr 2023 14:39:30 +0000 Subject: [PATCH 16/20] updated with minor improvements and comment fix --- .../NavigationHelper/NavigationHelper.h | 35 ++++++++----------- src/NavigationHelper/src/NavigationHelper.cpp | 18 ++++------ 2 files changed, 22 insertions(+), 31 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index e4321573..5002aa68 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -22,18 +22,23 @@ namespace WalkingControllers { private: yarp::os::BufferedPort m_replanningTriggerPort; /**< Publishes the flag triggering the navigation's global planner. */ - std::atomic m_runThreads{false}; /**< Global flag that allows the looping of the threads. */ - std::deque m_leftInContact; /**< Pointer to the deques in Module of the left feet contacts status. */ - std::deque m_rightInContact; /**< Pointer to 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; - std::thread m_navigationTriggerThread; /**< Thread for publishing the flag triggering the navigation's global planner. */ + std::atomic m_runThreads{false}; /**< Global flag that allows the looping of the threads. */ + std::deque m_leftInContact; /**< Copy of the deques in Module of the left feet contacts status. */ + std::deque 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}; + 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(); @@ -53,21 +58,11 @@ namespace WalkingControllers /** * Initialize the Navigation Helper * @param config yarp searchable object of the configuration. - * @param leftInContact deque in Module.cpp of the left feet contacts status. - * @param rightInContact deque in Module.cpp of the right feet contacts status. * @return true/false in case of success/failure. */ - bool init(const yarp::os::Searchable& config, - std::deque &leftInContact, - std::deque &rightInContact - ); - - /** - * Function launched by the looping thread - */ - void computeNavigationTrigger(); + bool init(const yarp::os::Searchable& config); - bool updateFeetDeques(std::deque left, std::deque right); + bool updateFeetDeques(const std::deque &left, const std::deque &right); }; } diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 7cc06c56..2d53e89e 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -61,11 +61,8 @@ bool NavigationHelper::closeHelper() return true; } -bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque &leftInContact, std::deque &rightInContact ) +bool NavigationHelper::init(const yarp::os::Searchable& config) { - m_leftInContact = leftInContact; - m_rightInContact = rightInContact; - 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(); @@ -123,12 +120,11 @@ void NavigationHelper::computeNavigationTrigger() bool enteredDoubleSupport = false, exitDoubleSupport = true; while (m_runThreads) { - //double support check { - const std::lock_guard lock(m_updateFeetMutex); + std::unique_lock lock(m_updateFeetMutex); if (m_leftInContact.size()>0 && m_rightInContact.size()>0) //external consistency check { - if (m_leftInContact.at(0) && m_rightInContact.at(0)) + if (m_leftInContact.at(0) && m_rightInContact.at(0)) //double support check { if (exitDoubleSupport) { @@ -149,7 +145,7 @@ void NavigationHelper::computeNavigationTrigger() { trigger = false; } - } + } //send the replanning trigger after a certain amount of seconds if (trigger) @@ -165,7 +161,7 @@ void NavigationHelper::computeNavigationTrigger() { std::this_thread::sleep_for(std::chrono::duration(m_navigationReplanningDelay*1000)); } - yDebug() << "[NavigationHelper::computeNavigationTrigger] Triggering navigation replanning"; + yInfo() << "[NavigationHelper::computeNavigationTrigger] Triggering navigation replanning"; auto& b = m_replanningTriggerPort.prepare(); b.clear(); b.add((yarp::os::Value)true); //send the planning trigger @@ -178,9 +174,9 @@ void NavigationHelper::computeNavigationTrigger() } -bool NavigationHelper::updateFeetDeques(std::deque left, std::deque right) +bool NavigationHelper::updateFeetDeques(const std::deque &left, const std::deque &right) { - const std::lock_guard lock(m_updateFeetMutex); + std::unique_lock lock(m_updateFeetMutex); m_leftInContact = left; m_rightInContact = right; return true; From d9ab01988f1bbc883f28f2831f69d266334dc453 Mon Sep 17 00:00:00 2001 From: Simone Date: Fri, 28 Apr 2023 14:40:19 +0000 Subject: [PATCH 17/20] updateFeetDeques also in advanceReferenceSignals --- src/WalkingModule/src/Module.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 77009f64..dad6b244 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -80,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()); @@ -455,6 +458,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) { yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper"; } + yInfo() << "[WalkingModule::configure] Configured Navigation Helper."; } else { From 8ba752d242b5c8feb91fe8c8f94591f159c40367 Mon Sep 17 00:00:00 2001 From: Simone Micheletti <86918431+SimoneMic@users.noreply.github.com> Date: Fri, 28 Apr 2023 17:53:57 +0200 Subject: [PATCH 18/20] Update m_navHelperUsed{false} Co-authored-by: Stefano Dafarra --- .../include/WalkingControllers/WalkingModule/Module.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 397025f0..08f9caca 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -158,7 +158,7 @@ namespace WalkingControllers double m_feedbackAttemptDelay; NavigationHelper m_navHelper; - bool m_navHelperUsed; + bool m_navHelperUsed{false}; // debug std::unique_ptr m_velocityIntegral{nullptr}; From 5827344c9c4181ce5279b9f1d99cda1600e427a0 Mon Sep 17 00:00:00 2001 From: Simone Micheletti <86918431+SimoneMic@users.noreply.github.com> Date: Fri, 28 Apr 2023 17:54:11 +0200 Subject: [PATCH 19/20] Formatting Co-authored-by: Stefano Dafarra --- src/NavigationHelper/src/NavigationHelper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp index 2d53e89e..bab1c638 100644 --- a/src/NavigationHelper/src/NavigationHelper.cpp +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -134,7 +134,7 @@ void NavigationHelper::computeNavigationTrigger() } else { - if (! exitDoubleSupport) + if (!exitDoubleSupport) { trigger = true; //in this way we have only one trigger each exit of double support } From f947612c78a05407efc501911eb0b5c75f73a22c Mon Sep 17 00:00:00 2001 From: Simone Date: Tue, 2 May 2023 12:38:11 +0000 Subject: [PATCH 20/20] moved closeThreads to private, added comments --- .../NavigationHelper/NavigationHelper.h | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h index 5002aa68..ebabbeb5 100644 --- a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -29,7 +29,7 @@ namespace WalkingControllers 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; + std::mutex m_updateFeetMutex; /**< Mutex that regulates the access to m_leftInContact and m_rightInContact. */ bool m_simulationMode{false}; /**< Flag that syncs the trigger delay with the external clock if in simulation. */ const std::string m_portPrefix = "/navigation_helper"; @@ -39,15 +39,22 @@ namespace WalkingControllers */ void computeNavigationTrigger(); + /** + * Close the Navigation Helper Threads + * @return true/false in case of success/failure. + */ + bool closeThreads(); + public: + /** + * Default constructor + */ NavigationHelper(); - ~NavigationHelper(); /** - * Close the Navigation Helper Threads and ports - * @return true/false in case of success/failure. + * Default destructor */ - bool closeThreads(); + ~NavigationHelper(); /** * Close the Navigation Helper Threads and ports @@ -62,6 +69,12 @@ namespace WalkingControllers */ bool init(const yarp::os::Searchable& config); + /** + * Updates the internal variables containing the relative feet contacts status + * @param left left feet contact status deque. + * @param right right feet contact status deque. + * @return true/false in case of success/failure. + */ bool updateFeetDeques(const std::deque &left, const std::deque &right); }; }