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..d448d045 --- /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::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..ebabbeb5 --- /dev/null +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -0,0 +1,82 @@ +/** + * @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 +#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}; /**< 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; /**< 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"; + + /** + * Function launched by the looping thread + */ + void computeNavigationTrigger(); + + /** + * Close the Navigation Helper Threads + * @return true/false in case of success/failure. + */ + bool closeThreads(); + + public: + /** + * Default constructor + */ + NavigationHelper(); + + /** + * Default destructor + */ + ~NavigationHelper(); + + /** + * 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); + + /** + * 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); + }; +} + +#endif diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp new file mode 100644 index 00000000..bab1c638 --- /dev/null +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -0,0 +1,183 @@ +/** + * @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 + + +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 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(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 &left, const std::deque &right) +{ + std::unique_lock lock(m_updateFeetMutex); + m_leftInContact = left; + m_rightInContact = right; + return true; +} 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..73b2a070 --- /dev/null +++ b/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 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..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,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"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 9c415cff..08f9caca 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 @@ -155,6 +157,9 @@ namespace WalkingControllers size_t m_feedbackAttempts; double m_feedbackAttemptDelay; + NavigationHelper m_navHelper; + bool m_navHelperUsed{false}; + // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 9b30624a..dad6b244 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include // iDynTree #include @@ -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);