From 86743f33e2998fbb185cb6c827b4a52a232beb0d Mon Sep 17 00:00:00 2001
From: "brian.lee" <lkm1321@gmail.com>
Date: Fri, 28 Aug 2020 06:44:35 +1000
Subject: [PATCH] add support for boolean;

---
 canopen_chain_node/src/ros_chain.cpp            | 2 ++
 canopen_master/include/canopen_master/objdict.h | 3 +++
 canopen_master/src/objdict.cpp                  | 5 +++++
 3 files changed, 10 insertions(+)

diff --git a/canopen_chain_node/src/ros_chain.cpp b/canopen_chain_node/src/ros_chain.cpp
index 5414a6740..e59d7dfc1 100644
--- a/canopen_chain_node/src/ros_chain.cpp
+++ b/canopen_chain_node/src/ros_chain.cpp
@@ -2,6 +2,7 @@
 
 #include <canopen_chain_node/ros_chain.h>
 
+#include <std_msgs/Bool.h>
 #include <std_msgs/Int8.h>
 #include <std_msgs/Int16.h>
 #include <std_msgs/Int32.h>
@@ -40,6 +41,7 @@ PublishFuncType createPublishFunc(ros::NodeHandle &nh,  const std::string &name,
     ObjectStorageSharedPtr s = node->getStorage();
 
     switch(ObjectDict::DataTypes(s->dict_->get(key)->data_type)){
+        case ObjectDict::DEFTYPE_BOOLEAN:        return create< std_msgs::Bool,    ObjectDict::DEFTYPE_BOOLEAN        >(nh, name, s, key, force);
         case ObjectDict::DEFTYPE_INTEGER8:       return create< std_msgs::Int8,    ObjectDict::DEFTYPE_INTEGER8       >(nh, name, s, key, force);
         case ObjectDict::DEFTYPE_INTEGER16:      return create< std_msgs::Int16,   ObjectDict::DEFTYPE_INTEGER16      >(nh, name, s, key, force);
         case ObjectDict::DEFTYPE_INTEGER32:      return create< std_msgs::Int32,   ObjectDict::DEFTYPE_INTEGER32      >(nh, name, s, key, force);
diff --git a/canopen_master/include/canopen_master/objdict.h b/canopen_master/include/canopen_master/objdict.h
index d03f262c9..339b45af6 100644
--- a/canopen_master/include/canopen_master/objdict.h
+++ b/canopen_master/include/canopen_master/objdict.h
@@ -146,6 +146,7 @@ class ObjectDict{
         RECORD = 0x09
     };
     enum DataTypes{
+        DEFTYPE_BOOLEAN  = 0x0001,
         DEFTYPE_INTEGER8 = 0x0002,
         DEFTYPE_INTEGER16 = 0x0003,
         DEFTYPE_INTEGER32 = 0x0004,
@@ -539,6 +540,7 @@ typedef ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr;
 template<> String & ObjectStorage::Data::access();
 template<> String & ObjectStorage::Data::allocate();
 
+template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_BOOLEAN>  { typedef bool type; };
 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER8> { typedef int8_t type;};
 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER16> { typedef int16_t type;};
 template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER32> { typedef int32_t type;};
@@ -559,6 +561,7 @@ template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN> { typedef
 
 template<typename T, typename R> static R *branch_type(const uint16_t data_type){
     switch(ObjectDict::DataTypes(data_type)){
+        case ObjectDict::DEFTYPE_BOOLEAN:  return T::template func< ObjectDict::DEFTYPE_BOOLEAN >;
         case ObjectDict::DEFTYPE_INTEGER8: return T::template func< ObjectDict::DEFTYPE_INTEGER8 >;
         case ObjectDict::DEFTYPE_INTEGER16: return T::template func< ObjectDict::DEFTYPE_INTEGER16 >;
         case ObjectDict::DEFTYPE_INTEGER32: return T::template func< ObjectDict::DEFTYPE_INTEGER32 >;
diff --git a/canopen_master/src/objdict.cpp b/canopen_master/src/objdict.cpp
index 929ef6d80..b0fbbfc11 100644
--- a/canopen_master/src/objdict.cpp
+++ b/canopen_master/src/objdict.cpp
@@ -109,6 +109,10 @@ void set_access( ObjectDict::Entry &entry, std::string access){
 
 template<typename T> T int_from_string(const std::string &s);
 
+template<> bool   int_from_string(const std::string &s){
+    return strtol(s.c_str(), 0, 0 ) > 0;
+}
+
 template<> int8_t int_from_string(const std::string &s){
     return strtol(s.c_str(), 0, 0);
 }
@@ -165,6 +169,7 @@ struct ReadAnyValue{
         return branch_type<ReadAnyValue, HoldAny (boost::property_tree::iptree &, const std::string &)>(data_type)(pt, key);
     }
 };
+template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_BOOLEAN>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<bool>(pt, key); }
 template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER8>(boost::property_tree::iptree &pt, const std::string &key){  return parse_int<int8_t>(pt,key); }
 template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER16>(boost::property_tree::iptree &pt, const std::string &key){  return parse_int<int16_t>(pt,key); }
 template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER32>(boost::property_tree::iptree &pt, const std::string &key){  return parse_int<int32_t>(pt,key); }