00001 #ifndef BT_BASIC_TYPES_H
00002 #define BT_BASIC_TYPES_H
00003
00004 #include <iostream>
00005 #include <vector>
00006 #include <sstream>
00007 #include <unordered_map>
00008 #include <unordered_set>
00009 #include <typeinfo>
00010 #include <functional>
00011 #include <chrono>
00012 #include <memory>
00013 #include "behaviortree_cpp/utils/string_view.hpp"
00014 #include "behaviortree_cpp/utils/safe_any.hpp"
00015 #include "behaviortree_cpp/exceptions.h"
00016 #include "behaviortree_cpp/utils/expected.hpp"
00017 #include "behaviortree_cpp/utils/make_unique.hpp"
00018
00019 namespace BT
00020 {
00022 enum class NodeType
00023 {
00024 UNDEFINED = 0,
00025 ACTION,
00026 CONDITION,
00027 CONTROL,
00028 DECORATOR,
00029 SUBTREE
00030 };
00031
00035 enum class NodeStatus
00036 {
00037 IDLE = 0,
00038 RUNNING,
00039 SUCCESS,
00040 FAILURE
00041 };
00042
00043 enum class PortDirection{
00044 INPUT,
00045 OUTPUT,
00046 INOUT
00047 };
00048
00049
00050 typedef nonstd::string_view StringView;
00051
00060 template <typename T> inline
00061 T convertFromString(StringView )
00062 {
00063 auto type_name = BT::demangle( typeid(T) );
00064
00065 std::cerr << "You (maybe indirectly) called BT::convertFromString() for type [" <<
00066 type_name <<"], but I can't find the template specialization.\n" << std::endl;
00067
00068 throw LogicError(std::string("You didn't implement the template specialization of "
00069 "convertFromString for this type: ") + type_name );
00070 }
00071
00072 template <>
00073 std::string convertFromString<std::string>(StringView str);
00074
00075 template <>
00076 const char* convertFromString<const char*>(StringView str);
00077
00078 template <>
00079 int convertFromString<int>(StringView str);
00080
00081 template <>
00082 unsigned convertFromString<unsigned>(StringView str);
00083
00084 template <>
00085 double convertFromString<double>(StringView str);
00086
00087 template <>
00088 std::vector<int> convertFromString<std::vector<int>>(StringView str);
00089
00090 template <>
00091 std::vector<double> convertFromString<std::vector<double>>(StringView str);
00092
00093 template <>
00094 bool convertFromString<bool>(StringView str);
00095
00096 template <>
00097 NodeStatus convertFromString<NodeStatus>(StringView str);
00098
00099 template <>
00100 NodeType convertFromString<NodeType>(StringView str);
00101
00102 template <>
00103 PortDirection convertFromString<PortDirection>(StringView str);
00104
00105
00106 typedef std::function<Any(StringView)> StringConverter;
00107
00108 typedef std::unordered_map<const std::type_info*, StringConverter> StringConvertersMap;
00109
00110
00111
00112 template <typename T> inline
00113 StringConverter GetAnyFromStringFunctor()
00114 {
00115 return [](StringView str){ return Any(convertFromString<T>(str)); };
00116 }
00117
00118 template <> inline
00119 StringConverter GetAnyFromStringFunctor<void>()
00120 {
00121 return {};
00122 }
00123
00124
00125
00126 template <typename T>
00127 std::string toStr(T value)
00128 {
00129 return std::to_string(value);
00130 }
00131
00132 std::string toStr(std::string value);
00133
00134 template<> std::string toStr<BT::NodeStatus>(BT::NodeStatus status);
00135
00139 std::string toStr(BT::NodeStatus status, bool colored);
00140
00141 std::ostream& operator<<(std::ostream& os, const BT::NodeStatus& status);
00142
00146 template<> std::string toStr<BT::NodeType>(BT::NodeType type);
00147
00148 std::ostream& operator<<(std::ostream& os, const BT::NodeType& type);
00149
00150
00151 template<> std::string toStr<BT::PortDirection>(BT::PortDirection direction);
00152
00153 std::ostream& operator<<(std::ostream& os, const BT::PortDirection& type);
00154
00155
00156 std::vector<StringView> splitString(const StringView& strToSplit, char delimeter);
00157
00158 template <typename Predicate>
00159 using enable_if = typename std::enable_if< Predicate::value >::type*;
00160
00161 template <typename Predicate>
00162 using enable_if_not = typename std::enable_if< !Predicate::value >::type*;
00163
00164
00181 template <typename T> using Optional = nonstd::expected<T, std::string>;
00182
00183
00184
00201 using Result = Optional<void>;
00202
00203 class PortInfo
00204 {
00205
00206 public:
00207 PortInfo( PortDirection direction = PortDirection::INOUT ):
00208 _type(direction), _info(nullptr) {}
00209
00210 PortInfo( PortDirection direction,
00211 const std::type_info& type_info,
00212 StringConverter conv):
00213 _type(direction),
00214 _info( &type_info ),
00215 _converter(conv)
00216 {}
00217
00218 PortDirection direction() const;
00219
00220 const std::type_info* type() const;
00221
00222 Any parseString(const char *str) const;
00223
00224 Any parseString(const std::string& str) const;
00225
00226 template <typename T>
00227 Any parseString(const T& ) const
00228 {
00229
00230 return {};
00231 }
00232
00233 void setDescription(StringView description);
00234
00235 void setDefaultValue(StringView default_value_as_string);
00236
00237 const std::string& description() const;
00238
00239 const std::string& defaultValue() const;
00240
00241 private:
00242
00243 PortDirection _type;
00244 const std::type_info* _info;
00245 StringConverter _converter;
00246 std::string description_;
00247 std::string default_value_;
00248 };
00249
00250 template <typename T = void>
00251 std::pair<std::string,PortInfo> CreatePort(PortDirection direction,
00252 StringView name,
00253 StringView description = {})
00254 {
00255 std::pair<std::string,PortInfo> out;
00256
00257 if( std::is_same<T, void>::value)
00258 {
00259 out = {name.to_string(), PortInfo(direction) };
00260 }
00261 else{
00262 out = {name.to_string(), PortInfo(direction, typeid(T),
00263 GetAnyFromStringFunctor<T>() ) };
00264 }
00265 if( !description.empty() )
00266 {
00267 out.second.setDescription(description);
00268 }
00269 return out;
00270 }
00271
00272
00273 template <typename T = void> inline
00274 std::pair<std::string,PortInfo> InputPort(StringView name, StringView description = {})
00275 {
00276 return CreatePort<T>(PortDirection::INPUT, name, description );
00277 }
00278
00279 template <typename T = void> inline
00280 std::pair<std::string,PortInfo> OutputPort(StringView name, StringView description = {})
00281 {
00282 return CreatePort<T>(PortDirection::OUTPUT, name, description );
00283 }
00284
00285 template <typename T = void> inline
00286 std::pair<std::string,PortInfo> BidirectionalPort(StringView name, StringView description = {})
00287 {
00288 return CreatePort<T>(PortDirection::INOUT, name, description );
00289 }
00290
00291 template <typename T = void> inline
00292 std::pair<std::string,PortInfo> InputPort(StringView name, const T& default_value, StringView description)
00293 {
00294 auto out = CreatePort<T>(PortDirection::INPUT, name, description );
00295 out.second.setDefaultValue( BT::toStr(default_value) );
00296 return out;
00297 }
00298
00299 template <typename T = void> inline
00300 std::pair<std::string,PortInfo> OutputPort(StringView name, const T& default_value, StringView description)
00301 {
00302 auto out = CreatePort<T>(PortDirection::OUTPUT, name, description );
00303 out.second.setDefaultValue( BT::toStr(default_value) );
00304 return out;
00305 }
00306
00307 template <typename T = void> inline
00308 std::pair<std::string,PortInfo> BidirectionalPort(StringView name, const T& default_value, StringView description)
00309 {
00310 auto out = CreatePort<T>(PortDirection::INOUT, name, description );
00311 out.second.setDefaultValue( BT::toStr(default_value) );
00312 return out;
00313 }
00314
00315
00316 typedef std::unordered_map<std::string, PortInfo> PortsList;
00317
00318 template <typename T, typename = void>
00319 struct has_static_method_providedPorts: std::false_type {};
00320
00321 template <typename T>
00322 struct has_static_method_providedPorts<T,
00323 typename std::enable_if<std::is_same<decltype(T::providedPorts()), PortsList>::value>::type>
00324 : std::true_type {};
00325
00326 template <typename T> inline
00327 PortsList getProvidedPorts(enable_if< has_static_method_providedPorts<T> > = nullptr)
00328 {
00329 return T::providedPorts();
00330 }
00331
00332 template <typename T> inline
00333 PortsList getProvidedPorts(enable_if_not< has_static_method_providedPorts<T> > = nullptr)
00334 {
00335 return {};
00336 }
00337
00338 typedef std::chrono::high_resolution_clock::time_point TimePoint;
00339 typedef std::chrono::high_resolution_clock::duration Duration;
00340
00341 }
00342
00343
00344 #endif // BT_BASIC_TYPES_H