From 54bc4555c9238e2a0b35871a0f80e50be912ec5b Mon Sep 17 00:00:00 2001
From: Matthew Skolaut <matthew@rugged-robotics.com>
Date: Tue, 20 Sep 2022 11:27:41 -0500
Subject: [PATCH] fix nodeid parsing to allow parsing from both ends

---
 canopen_master/src/objdict.cpp      |  2 ++
 canopen_master/test/test_parser.cpp | 27 +++++++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/canopen_master/src/objdict.cpp b/canopen_master/src/objdict.cpp
index 929ef6d80..ea479380a 100644
--- a/canopen_master/src/objdict.cpp
+++ b/canopen_master/src/objdict.cpp
@@ -141,6 +141,8 @@ template<typename T> HoldAny parse_int(boost::property_tree::iptree &pt, const s
     std::string str = boost::trim_copy(pt.get<std::string>(key));
     if(boost::istarts_with(str,"$NODEID")){
         return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(str.find("+",7)+1)))));
+    }else if (boost::iends_with(str,"$NODEID")){
+        return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(0, str.find("+")+1)))));
     }else return HoldAny(int_from_string<T>(str));
 }
 
diff --git a/canopen_master/test/test_parser.cpp b/canopen_master/test/test_parser.cpp
index 20253dd67..6bb30b07d 100644
--- a/canopen_master/test/test_parser.cpp
+++ b/canopen_master/test/test_parser.cpp
@@ -37,6 +37,8 @@ TYPED_TEST(TestHexTypes, checkZero){
         TestFixture::test_hex_node(0,"0x0",i);
         TestFixture::test_hex_node(0+i,"$NODEID+0",i);
         TestFixture::test_hex_node(0+i,"$NODEID+0x0",i);
+        TestFixture::test_hex_node(0+i,"0+$NODEID",i);
+        TestFixture::test_hex_node(0+i,"0x0+$NODEID",i);
     }
 }
 TEST(TestHex, checkCamelCase){
@@ -73,6 +75,13 @@ TEST(TestHex, checkNodeSpaces){
         TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID +1",i);
         TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID +0x1 ",i);
         TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID + 0x1",i);
+        
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"1 + $NODEID",i);
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"1+ $NODEID",i);
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"1+$NODEID",i);
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"1 +$NODEID",i);
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"0x1 +  $NODEID ",i);
+        TestHexTypes<uint16_t>::test_hex_node(i+1,"0x1  +$NODEID",i);
     }
 }
 TEST(TestHex, checkCommonObjects){
@@ -95,6 +104,24 @@ TEST(TestHex, checkCommonObjects){
         TestHexTypes<uint32_t>::test_hex_node(0x600+i,"$NODEID+0x600",i); // RSDO
 
         TestHexTypes<uint32_t>::test_hex_node(0x700+i,"$NODEID+0x700",i); //NMT
+
+        TestHexTypes<uint32_t>::test_hex_node( 0x80+i,"0x80+$NODEID",i); // EMCY
+
+        TestHexTypes<uint32_t>::test_hex_node(0x180+i,"0x180+$NODEID",i); //TPDO1
+        TestHexTypes<uint32_t>::test_hex_node(0x200+i,"0x200+$NODEID",i); //RPDO1
+
+        TestHexTypes<uint32_t>::test_hex_node(0x280+i,"0x280+$NODEID",i); //TPDO2
+        TestHexTypes<uint32_t>::test_hex_node(0x300+i,"0x300+$NODEID",i); //RPDO2
+        TestHexTypes<uint32_t>::test_hex_node(0x380+i,"0x380+$NODEID",i); //TPDO3
+        TestHexTypes<uint32_t>::test_hex_node(0x400+i,"0x400+$NODEID",i); //RPDO3
+
+        TestHexTypes<uint32_t>::test_hex_node(0x480+i,"0x480+$NODEID",i); //TPDO4
+        TestHexTypes<uint32_t>::test_hex_node(0x500+i,"0x500+$NODEID",i); //RPDO4
+
+        TestHexTypes<uint32_t>::test_hex_node(0x580+i,"0x580+$NODEID",i); // TSDO
+        TestHexTypes<uint32_t>::test_hex_node(0x600+i,"0x600+$NODEID",i); // RSDO
+
+        TestHexTypes<uint32_t>::test_hex_node(0x700+i,"0x700+$NODEID",i); //NMT
     }
 }