diff --git a/README.md b/README.md index 64488b5..24cd89a 100644 --- a/README.md +++ b/README.md @@ -19,12 +19,351 @@ These dependencies need to be installed, in addition to ROS: sudo apt install -y libgecode-dev ``` -## How to use? +## Diagram +The following diagram illustrates how HBBA Lite works. -### Create Motivation Modules +![HBBA Lite Diagram](images/hbba_lite_diagram.png) + +## Documentation +A hybrid robot architecture means that perceptual nodes can communicate with the behavior nodes and the planning modules. + +### Definitions + +#### Filters +In order to disable a node, a HBBA filter blocks all messages. Also, a filter can reduce CPU usage by letting pass 1 message out of N. +An on-off filter lets pass all messages or blocks all messages. A throttling filter lets pass 1 message out of N or blocks all messages. + +#### Perceptual Nodes +The perceptual nodes process sensor data to extract useful information from it. +Generally, the subscriber has an on-off filter for audio data or a throttling filter for image data. + +#### Behavior Nodes +The behavior nodes publish messages to perform actions. For example, a behavior node can move the robot in order to follow a person. +Generally, the action message publisher has an on-off filter to disable the behavior. + +#### Resources +A resource represents something limited in the robot. +For example, a resource can be declared for each actuator to prevent multiple behavior nodes to use the same actuator. +Also, a resource can be declared to represent the available processing time in percent. + +#### Desire +A desire represents something to be fulffilled by the robot. +A desire has an intensity value that representes the level of priority of the desire. Desires with the highest intensity are priotized. +Also, it may have parameters used by the strategy. + +#### Strategy +A strategy represents a way to fulfill a desire. The strategy will accomplish the desire once the strategy is activated. +At least one strategy is required for each desire type. +If there are many strategies for a desire type, the strategy with the highest utility is prioritized. +A strategy has a list of filters to enable and a list resources required to be enabled. +The resources can be used to prevent conflicts of actuators between strategies. +If resources are used to manage the processing time in percent, the strategy resources can contain the estimated processing time in percent. +So, HBBA will manage the processing time of the robot. + +#### Desire Set +The desire set contains the current desires to fulfill. An observer pattern is implemented in the desire to get notified of changes. + +#### Motivations +The motivations modify the content of the desire set to make the robot do something useful. +Generally, the motivations subscribe to perceptual nodes in order to add or remove desires with respect to the robot surrounding. + +#### Solver +Once the solver gets the content of the desire set, it will find the best desires to fulfill with the best strategies. + +The solver steps are: +1. For each type of desire in the set, find the most intense desire. +2. Find the desire-strategy combinations that maximize the sum of intensity-utility products that respect the resource constraints. + +### Creating Motivation Modules See the [demos](https://github.com/introlab/t-top/tree/main/ros/demos) for the [T-Top](https://github.com/introlab/t-top) robot. +### Creating Desires +The following example shows how to declare a simple desire that does not have any parameter. +```cpp +class Camera3dRecordingDesire : public Desire +{ +public: + explicit Camera3dRecordingDesire(uint16_t intensity = 1) : Desire(intensity) {} + + ~Camera3dRecordingDesire() override = default; + + // The following macro overrides the required methods to declare a Desire. + DECLARE_DESIRE_METHODS(Camera3dRecordingDesire) + + // The macro is expanded to : + // std::unique_ptr clone() override { return std::make_unique(*this); } \ + // DesireType type() override { return DesireType::get(); } +}; +``` + +The following example shows how to declare a desire that contains a parameter. +```cpp +class TalkDesire : public Desire +{ + // Declare the desire parameters here. + std::string m_text; + +public: + // Add the desire parameters as arguments of the constructor. + TalkDesire::TalkDesire(string text, uint16_t intensity) : Desire(intensity), m_text(move(text)) {} + ~TalkDesire() override = default; + + // The following macro overrides the required methods to declare a Desire. + DECLARE_DESIRE_METHODS(TalkDesire) + + // Add the getters of the desire parameters here. + const std::string& text() const { return m_text; } +}; +``` + +### Creating Strategies +Strategies that only change the state of filters do not require creating a subclass of the strategy class. +The following example shows how to create the strategy for the `Camera3dRecordingDesire` previously defined. +```cpp +// The template argument of the class declares the desire type associated with the strategy. +auto strategy = make_unique>( + utility, // The utility of the strategy + unordered_map{}, // Declare the resources used by the strategy + unordered_map{ + {"video_recorder_camera_3d/filter_state", FilterConfiguration::onOff()}}, // Declare the filters to enable and their type. + move(filterPool)); // The instance of the class to change the filter state. +``` +Generally, the previous code is put inside a function to simplify its use in other projects. +The association between the desire and the strategy is done with the template argument of the `Strategy` class. + +The following example shows how to create a subclass in order to publish a message when the strategy is enabled. +Also, the strategy removes the desire of the set once the desire is completed. +```cpp +// The template argument of the Strategy class declares the desire type associated with the strategy. +class TalkStrategy : public Strategy +{ + std::shared_ptr m_desireSet; + ros::NodeHandle& m_nodeHandle; + ros::Publisher m_talkPublisher; + ros::Subscriber m_talkDoneSubscriber; + +public: + TalkStrategy( + uint16_t utility, // The utility of the strategy + std::shared_ptr filterPool, // The instance of the class to change the filter state. + std::shared_ptr desireSet, + ros::NodeHandle& nodeHandle) + : Strategy( + utility, + {{"sound", 1}}, // Declare the resources used by the strategy. + {{"talk/filter_state", FilterConfiguration::onOff()}}, // Declare the filters to enable and their type. + move(filterPool)), + m_desireSet(move(desireSet)), + m_nodeHandle(nodeHandle) + { + // Create the publisher to send the text to say. + m_talkPublisher = nodeHandle.advertise("talk/text", 1, true); + + // Create the subscriber to be notified when the text has been said. + m_talkDoneSubscriber = nodeHandle.subscribe("talk/done", 10, &TalkStrategy::talkDoneSubscriberCallback, this); + } + + DECLARE_NOT_COPYABLE(TalkStrategy); + DECLARE_NOT_MOVABLE(TalkStrategy); + +protected: + // Override the method to publish a message when the strategy is enabled. + void onEnabling(const TalkDesire& desire) override + { + Strategy::onEnabling(desire); // Enable the filters declared in the constructor. + + // Publish the text to be said. + talk::Text msg; + msg.text = desire.text(); + msg.id = desire.id(); + m_talkPublisher.publish(msg); + } + +private: + void talkDoneSubscriberCallback(const talk::Done::ConstPtr& msg) + { + // Remove the desire once the text is said. + if (msg->id == desireId()) + { + m_desireSet->removeDesire(msg->id); + } + } +}; +``` + +### Using the Desire Set +The following subsections show how to use the desire set. The DesireSet class is thread-safe, so it can be called by any thread. +Once a change has been made to the desire set, the solver will be run to update the filter states. +To prevent this behavior, a transaction can be created, so the solver will be run when the transaction is destroyed. + +#### Add a Desire +```cpp +auto desire = make_unique("The text to say"); // Create the desire. +desireSet->addDesire(std::move(desire)); // Add the desire to the set. +``` + +If you don't need to have access to the desire instance, there is a simplified syntax. +```cpp +auto id = desireSet->addDesire("The text to say"); // Create the desire, add the desire to the set and return the id. +``` + +#### Remove a Desire +```cpp +desireSet->remove(id); // Remove the desire that has the provided id. +``` + +#### Remove All Desires +```cpp +desireSet->clear(); // Remove all desires +``` + +#### Remove All Desires of a Specific Type +```cpp +desireSet->removeAllDesiresOfType(DesireType::get()); // Remove all TalkDesire instances. +``` + +A simplied syntax exists. +```cpp +desireSet->removeAllDesiresOfType(); // Remove all TalkDesire instances. +``` + +#### Check Whether the Set Contains a Desire +```cpp +desireSet->contains(id); // Return a boolean indicating whether the set contains a desire that has the provided id. +``` + +#### Check Whether the Set Contains a Desire of a Specific Type +```cpp +// Return a boolean indicating whether the set contains a desire of type TalkDesire. +desireSet->containsAnyDesiresOfType(DesireType::get()); +``` + +A simplied syntax exists. +```cpp +// Return a boolean indicating whether the set contains a desire of type TalkDesire. +desireSet->containsAnyDesiresOfType(); +``` + +#### How to Create a Transaction +The following example shows how to use transactions. + +```cpp +{ + auto transaction = desireSet->beginTransaction(); + + // Change the desire set here. +} +// When the transaction is destroyed, the changes will be applied. +``` + +#### How to Use the Observer Pattern +The following example shows how to create and add an observer to the desire set. + +```cpp +#include +#include + +class LogDesireSetObserver : public DesireSetObserver +{ +public: + void onDesireSetChanged(const std::vector>& desires) override + { + for (auto& desire : desires) + { + std::cout << desire->type().name() << " "; + } + std::cout << std::endl; + } +}; + +int main(int argc, char** argv) +{ + LogDesireSetObserver observer; + auto desireSet = make_shared(); // Create the desire set. + desireSet->addObserver(&observer); + + // All changes to the desire will be logged to the terminal. + return 0; +} +``` + +### Initializing HBBA Lite +The following examples show how to initialize the HBBA Lite. + +#### Normal Usage +```cpp +constexpr bool WAIT_FOR_SERVICE = true; + +auto desireSet = make_shared(); // Create the desire set. +// Create the filter pool useful to change the filter states. +// If WAIT_FOR_SERVICE is true, the pool will wait until the service become available. +auto filterPool = make_shared(nodeHandle, WAIT_FOR_SERVICE); + +vector> strategies; +// Add the strategies related to the application into the vector. + +auto solver = make_unique(); // Create the solver. +HbbaLite hbba(desireSet, + move(strategies), + {{"sound", 1}}, // The resource available on the robot. + move(solver)); // The constructor starts a thread for the solver. + + +// Add desires to the set. +``` + +#### Strategy State Logger +To log the strategy state changed of the strategy, you can use the `RosLogStrategyStateLogger` class. + +```cpp +constexpr bool WAIT_FOR_SERVICE = true; + +auto desireSet = make_shared(); // Create the desire set. +// Create the filter pool useful to change the filter states. +// If WAIT_FOR_SERVICE is true, the pool will wait until the service become available. +auto filterPool = make_shared(nodeHandle, WAIT_FOR_SERVICE); + +vector> strategies; +// Add the strategies related to the application into the vector. + +auto solver = make_unique(); // Create the solver. +HbbaLite hbba(desireSet, + move(strategies), + {{"sound", 1}}, // The resource available on the robot. + move(solver), // The constructor starts a thread for the solver. + make_unique()); + + +// Add desires to the set. +``` + +#### Filter State Logger +To log the filter state changed of the strategy, you can use the `RosLogStrategyStateLogger` class. + +```cpp +constexpr bool WAIT_FOR_SERVICE = true; + +auto desireSet = make_shared(); // Create the desire set. +// Create the filter pool useful to change the filter states. +// If WAIT_FOR_SERVICE is true, the pool will wait until the service become available. +auto rosFilterPool = make_unique(nodeHandle, WAIT_FOR_SERVICE); +auto filterPoos = make_shared(move(rosFilterPool)); + +vector> strategies; +// Add the strategies related to the application into the vector. + +auto solver = make_unique(); // Create the solver. +HbbaLite hbba(desireSet, + move(strategies), + {{"sound", 1}}, // The resource available on the robot. + move(solver)); // The constructor starts a thread for the solver. + + +// Add desires to the set. +``` + + ### Adding a Filter to C++ Nodes #### Subscriber diff --git a/images/hbba_lite_diagram.drawio b/images/hbba_lite_diagram.drawio new file mode 100644 index 0000000..a37b041 --- /dev/null +++ b/images/hbba_lite_diagram.drawio @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/images/hbba_lite_diagram.png b/images/hbba_lite_diagram.png new file mode 100644 index 0000000..6156838 Binary files /dev/null and b/images/hbba_lite_diagram.png differ diff --git a/include/hbba_lite/core/HbbaLite.h b/include/hbba_lite/core/HbbaLite.h index 5fa5c2f..b0e9915 100644 --- a/include/hbba_lite/core/HbbaLite.h +++ b/include/hbba_lite/core/HbbaLite.h @@ -55,7 +55,7 @@ class HbbaLite : public DesireSetObserver std::vector> strategies, std::unordered_map resourcesByNames, std::unique_ptr solver, - std::unique_ptr strategyStateLogger = std::make_unique()); + std::unique_ptr strategyStateLogger = std::make_unique()); ~HbbaLite(); DECLARE_NOT_COPYABLE(HbbaLite); diff --git a/include/hbba_lite/core/RosFilterPool.h b/include/hbba_lite/core/RosFilterPool.h index d39456e..ad423e9 100644 --- a/include/hbba_lite/core/RosFilterPool.h +++ b/include/hbba_lite/core/RosFilterPool.h @@ -16,6 +16,11 @@ class RosFilterPool : public FilterPool public: RosFilterPool(ros::NodeHandle& nodeHandle, bool waitForService); + ~RosFilterPool() override = default; + + DECLARE_NOT_COPYABLE(RosFilterPool); + DECLARE_NOT_MOVABLE(RosFilterPool); + void add(const std::string& name, FilterType type) override; protected: @@ -50,5 +55,23 @@ void RosFilterPool::call(const std::string& name, ServiceType& srv) } } +class RosLogFilterPoolDecorator : public FilterPool +{ + std::unique_ptr m_filterPool; + +public: + RosLogFilterPoolDecorator(std::unique_ptr filterPool); + ~RosLogFilterPoolDecorator() override = default; + + DECLARE_NOT_COPYABLE(RosLogFilterPoolDecorator); + DECLARE_NOT_MOVABLE(RosLogFilterPoolDecorator); + + void add(const std::string& name, FilterType type) override; + +protected: + void applyEnabling(const std::string& name, const FilterConfiguration& configuration) override; + void applyDisabling(const std::string& name) override; +}; + #endif diff --git a/include/hbba_lite/core/RosStrategyStateLogger.h b/include/hbba_lite/core/RosStrategyStateLogger.h index 7a731e4..2684d3f 100644 --- a/include/hbba_lite/core/RosStrategyStateLogger.h +++ b/include/hbba_lite/core/RosStrategyStateLogger.h @@ -5,16 +5,30 @@ #include -class RosStrategyStateLogger : public StrategyStateLogger +class RosLogStrategyStateLogger : public StrategyStateLogger +{ +public: + RosLogStrategyStateLogger() = default; + ~RosLogStrategyStateLogger() override = default; + + DECLARE_NOT_COPYABLE(RosLogStrategyStateLogger); + DECLARE_NOT_MOVABLE(RosLogStrategyStateLogger); + + void log(DesireType desireType, StrategyType strategyType, bool enabled) override; +}; + + +class RosTopicStrategyStateLogger : public StrategyStateLogger { ros::NodeHandle& m_nodeHandle; ros::Publisher m_strategyStatePub; public: - RosStrategyStateLogger(ros::NodeHandle& nodeHandle); + RosTopicStrategyStateLogger(ros::NodeHandle& nodeHandle); + ~RosTopicStrategyStateLogger() override = default; - DECLARE_NOT_COPYABLE(RosStrategyStateLogger); - DECLARE_NOT_MOVABLE(RosStrategyStateLogger); + DECLARE_NOT_COPYABLE(RosTopicStrategyStateLogger); + DECLARE_NOT_MOVABLE(RosTopicStrategyStateLogger); void log(DesireType desireType, StrategyType strategyType, bool enabled) override; }; diff --git a/include/hbba_lite/core/Strategy.h b/include/hbba_lite/core/Strategy.h index e5de7bc..8813f0d 100644 --- a/include/hbba_lite/core/Strategy.h +++ b/include/hbba_lite/core/Strategy.h @@ -125,6 +125,7 @@ class FilterPool public: FilterPool() = default; + virtual ~FilterPool() = default; DECLARE_NOT_COPYABLE(FilterPool); DECLARE_NOT_MOVABLE(FilterPool); @@ -136,6 +137,10 @@ class FilterPool protected: virtual void applyEnabling(const std::string& name, const FilterConfiguration& configuration) = 0; virtual void applyDisabling(const std::string& name) = 0; + + // Useful for decorators + void callApplyEnabling(FilterPool& filterPool, const std::string& name, const FilterConfiguration& configuration); + void callApplyDisabling(FilterPool& filterPool, const std::string& name); }; class BaseStrategy diff --git a/include/hbba_lite/core/StrategyStateLogger.h b/include/hbba_lite/core/StrategyStateLogger.h index 9c7459d..ad68b5e 100644 --- a/include/hbba_lite/core/StrategyStateLogger.h +++ b/include/hbba_lite/core/StrategyStateLogger.h @@ -9,11 +9,24 @@ class StrategyStateLogger { public: StrategyStateLogger(); + virtual ~StrategyStateLogger() = default; DECLARE_NOT_COPYABLE(StrategyStateLogger); DECLARE_NOT_MOVABLE(StrategyStateLogger); - virtual void log(DesireType desireType, StrategyType strategyType, bool enabled); + virtual void log(DesireType desireType, StrategyType strategyType, bool enabled) = 0; +}; + +class DummyStrategyStateLogger : public StrategyStateLogger +{ +public: + DummyStrategyStateLogger(); + ~DummyStrategyStateLogger() override = default; + + DECLARE_NOT_COPYABLE(DummyStrategyStateLogger); + DECLARE_NOT_MOVABLE(DummyStrategyStateLogger); + + void log(DesireType desireType, StrategyType strategyType, bool enabled) override; }; #endif diff --git a/src/hbba_lite_cpp/core/RosFilterPool.cpp b/src/hbba_lite_cpp/core/RosFilterPool.cpp index 3d19d5e..b8ef288 100644 --- a/src/hbba_lite_cpp/core/RosFilterPool.cpp +++ b/src/hbba_lite_cpp/core/RosFilterPool.cpp @@ -83,3 +83,37 @@ void RosFilterPool::applyDisabling(const string& name) throw HbbaLiteException("Not supported filter type"); } } + +RosLogFilterPoolDecorator::RosLogFilterPoolDecorator(unique_ptr filterPool) : m_filterPool(move(filterPool)) +{ +} + +void RosLogFilterPoolDecorator::add(const std::string& name, FilterType type) +{ + m_filterPool->add(name, type); +} + +void RosLogFilterPoolDecorator::applyEnabling(const std::string& name, const FilterConfiguration& configuration) +{ + callApplyEnabling(*m_filterPool, name, configuration); + + switch (configuration.type()) + { + case FilterType::ON_OFF: + ROS_INFO_STREAM("HBBA filter state changed: " << name << " -> enabled"); + break; + + case FilterType::THROTTLING: + ROS_INFO_STREAM("HBBA filter state changed: " << name << " -> enabled (" << configuration.rate() << ")"); + break; + + default: + throw HbbaLiteException("Not supported filter type"); + } +} + +void RosLogFilterPoolDecorator::applyDisabling(const std::string& name) +{ + callApplyDisabling(*m_filterPool, name); + ROS_INFO_STREAM("HBBA filter state changed: " << name << " -> disabled"); +} diff --git a/src/hbba_lite_cpp/core/RosStrategyStateLogger.cpp b/src/hbba_lite_cpp/core/RosStrategyStateLogger.cpp index 47e4d11..b9c4d66 100644 --- a/src/hbba_lite_cpp/core/RosStrategyStateLogger.cpp +++ b/src/hbba_lite_cpp/core/RosStrategyStateLogger.cpp @@ -2,12 +2,22 @@ #include -RosStrategyStateLogger::RosStrategyStateLogger(ros::NodeHandle& nodeHandle) : m_nodeHandle(nodeHandle) +using namespace std; + +void RosLogStrategyStateLogger::log(DesireType desireType, StrategyType strategyType, bool enabled) +{ + ROS_INFO_STREAM( + "HBBA strategy state changed: " + << "( " << desireType.name() << ", " << strategyType.name() << ") -> " << enabled); +} + + +RosTopicStrategyStateLogger::RosTopicStrategyStateLogger(ros::NodeHandle& nodeHandle) : m_nodeHandle(nodeHandle) { m_strategyStatePub = m_nodeHandle.advertise("hbba_strategy_state_log", 1000); } -void RosStrategyStateLogger::log(DesireType desireType, StrategyType strategyType, bool enabled) +void RosTopicStrategyStateLogger::log(DesireType desireType, StrategyType strategyType, bool enabled) { hbba_lite::StrategyState msg; msg.desire_type_name = desireType.name(); diff --git a/src/hbba_lite_cpp/core/Strategy.cpp b/src/hbba_lite_cpp/core/Strategy.cpp index 4bb3ca0..0a3e28d 100644 --- a/src/hbba_lite_cpp/core/Strategy.cpp +++ b/src/hbba_lite_cpp/core/Strategy.cpp @@ -79,6 +79,19 @@ void FilterPool::disable(const string& name) } } +void FilterPool::callApplyEnabling( + FilterPool& filterPool, + const std::string& name, + const FilterConfiguration& configuration) +{ + filterPool.applyEnabling(name, configuration); +} + +void FilterPool::callApplyDisabling(FilterPool& filterPool, const std::string& name) +{ + filterPool.applyDisabling(name); +} + BaseStrategy::BaseStrategy( uint16_t utility, unordered_map resourcesByName, diff --git a/src/hbba_lite_cpp/core/StrategyStateLogger.cpp b/src/hbba_lite_cpp/core/StrategyStateLogger.cpp index d4e87e4..4350716 100644 --- a/src/hbba_lite_cpp/core/StrategyStateLogger.cpp +++ b/src/hbba_lite_cpp/core/StrategyStateLogger.cpp @@ -2,4 +2,6 @@ StrategyStateLogger::StrategyStateLogger() {} -void StrategyStateLogger::log(DesireType desireType, StrategyType strategyType, bool enabled) {} +DummyStrategyStateLogger::DummyStrategyStateLogger() {} + +void DummyStrategyStateLogger::log(DesireType desireType, StrategyType strategyType, bool enabled) {}