Nuttx: remove use of std::string, std::map, std::set

Nuttx complains about an unresolved _impure_ptr at link time.
This is a known issue when using STL templates in NuttX on ARM.

Created new ORBMap and ORBSet classes for NuttX.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

makefiles/nuttx/toolchain_gnu-arm-eabi.mk

@@ -125,7 +125,6 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 # note - requires corresponding support in NuttX
 INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 
-LIBSTDCXX	 := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libstdc++.a)
 LIBC	 := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a)
 
 # Language-specific flags
@@ -282,8 +281,7 @@ endef
 define LINK
 	@$(ECHO) "LINK:    $1"
 	@$(MKDIR) -p $(dir $1)
-	$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group
+	$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
-	#$(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group
 endef
 
 # Convert $1 from a linked object to a raw binary in $2

src/modules/uORB/ORBSet.h

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+class ORBSet
+{
+public:
+	struct Node {
+		struct Node *next;
+		const char * node_name;
+	};
+
+	ORBSet() : 
+		_top(nullptr),
+		_end(nullptr)
+	{ }
+	~ORBSet() {
+		while (_top != nullptr) {
+			unlinkNext(_top);
+			if (_top->next == nullptr) {
+				free((void *)_top->node_name);
+				free(_top);
+				_top = nullptr;
+			}
+		}				
+	}
+	void insert(const char *node_name)
+	{
+		Node **p;
+		if (_top == nullptr)
+			p = &_top;
+		else 
+			p = &_end->next;
+
+		*p = (Node *)malloc(sizeof(Node));
+		if (_end)
+			_end = _end->next;
+		else {
+			_end = _top;
+		}
+		_end->next = nullptr;
+		_end->node_name = strdup(node_name);
+	}
+
+	bool find(const char *node_name)
+	{
+		Node *p = _top;
+		while (p) {
+			if (strcmp(p->node_name, node_name) == 0) {
+				return true;
+			}
+			p = p->next;
+		}
+		return false;
+	}
+
+	bool erase(const char *node_name)
+	{
+		Node *p = _top;
+		if (_top && (strcmp(_top->node_name, node_name) == 0)) {
+			p = _top->next;
+			free((void *)_top->node_name);
+			free(_top);
+			_top = p;
+			if (_top == nullptr) {
+				_end = nullptr;
+			}
+			return true;
+		}
+		while (p->next) {
+			if (strcmp(p->next->node_name, node_name) == 0) {
+				unlinkNext(p);
+			}
+		}
+		return nullptr;
+	}
+
+private:
+
+	void unlinkNext(Node *a)
+	{
+		Node *b = a->next;
+		if (b != nullptr) {
+			if (_end == b) {
+				_end = a;
+			}
+			a->next = b->next;
+			free((void *)b->node_name);
+			free(b);
+		}
+	}
+
+	Node *_top;
+	Node *_end;
+};
+

src/modules/uORB/uORBCommunicator.hpp

@@ -70,7 +70,7 @@ public:
 	 * 		Note: This does not mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0;
+	virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0;
 
 
 	/**
@@ -84,7 +84,7 @@ public:
 	 * 		Note: This does not necessarily mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t remove_subscription( const std::string& messageName ) = 0;
+	virtual int16_t remove_subscription( const char *messageName ) = 0;
 
 	/**
 	 * Register Message Handler.  This is internal for the IChannel implementer*
@@ -110,7 +110,7 @@ public:
 	 * 		Note: This does not mean that the receiver as received it.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data) = 0;
+	virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0;
 };
 
 /**
@@ -133,7 +133,7 @@ public:
 	 *  	handler.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t process_add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0;
+	virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0;
 
 	/**
 	 * Interface to process a received control msg to remove subscription
@@ -145,7 +145,7 @@ public:
 	 *  	handler.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t process_remove_subscription( const std::string& messageName ) = 0;
+	virtual int16_t process_remove_subscription( const char *messageName ) = 0;
 
 	/**
 	 * Interface to process the received data message.
@@ -161,7 +161,7 @@ public:
 	 *  	handler.
 	 *  otherwise = failure.
 	 */
-	virtual int16_t process_received_message( const std::string& messageName, int32_t length, uint8_t* data ) = 0;
+	virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0;
 };
 
 #endif /* _uORBCommunicator_hpp_ */

src/modules/uORB/uORBDevices_nuttx.cpp

@@ -43,7 +43,7 @@
 #include "uORBCommunicator.hpp"
 #include <stdlib.h>
 
-std::map<std::string, uORB::DeviceNode*> uORB::DeviceMaster::_node_map;
+uORB::ORBMap uORB::DeviceMaster::_node_map;
 
 uORB::DeviceNode::DeviceNode
 (
@@ -608,7 +608,7 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
         else
         {
           // add to the node map;.
-          _node_map[std::string(nodepath)] = node;
+          _node_map.insert(nodepath, node);
         }
 
         group_tries++;
@@ -631,12 +631,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
   }
 }
 
-uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath )
+uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
 {
   uORB::DeviceNode* rc = nullptr;
-  if( _node_map.find( nodepath ) != _node_map.end() )
+  if( _node_map.find( nodepath ) )
   {
-    rc = _node_map[nodepath];
+    rc = _node_map.get(nodepath);
   }
   return rc;
 }

src/modules/uORB/uORBDevices_nuttx.hpp

@@ -35,8 +35,8 @@
 #define _uORBDevices_nuttx_hpp_
 
 #include <stdint.h>
-#include <string>
+#include <string.h>
-#include <map>
+#include <stdlib.h>
 #include "uORBCommon.hpp"
 
 
@@ -44,8 +44,93 @@ namespace uORB
 {
    class DeviceNode;
    class DeviceMaster;
+   class ORBMap;
 }
 
+class uORB::ORBMap
+{
+public:
+	struct Node {
+		struct Node *next;
+		const char * node_name;
+		uORB::DeviceNode *node;
+	};
+
+	ORBMap() : 
+		_top(nullptr),
+		_end(nullptr)
+	{ }
+	~ORBMap() {
+		while (_top != nullptr) {
+			unlinkNext(_top);
+			if (_top->next == nullptr) {
+				free((void *)_top->node_name);
+				free(_top);
+				_top = nullptr;
+				_end = nullptr;
+			}
+		}				
+	}
+	void insert(const char *node_name, uORB::DeviceNode*node)
+	{
+		Node **p;
+		if (_top == nullptr)
+			p = &_top;
+		else 
+			p = &_end->next;
+
+		*p = (Node *)malloc(sizeof(Node));
+		if (_end)
+			_end = _end->next;
+		else {
+			_end = _top;
+		}
+		_end->next = nullptr;
+		_end->node_name = strdup(node_name);
+		_end->node = node;
+	}
+
+	bool find(const char *node_name)
+	{
+		Node *p = _top;
+		while (p) {
+			if (strcmp(p->node_name, node_name) == 0) {
+				return true;
+			}
+			p = p->next;
+		}
+		return false;
+	}
+
+	uORB::DeviceNode* get(const char *node_name)
+	{
+		Node *p = _top;
+		while (p) {
+			if (strcmp(p->node_name, node_name) == 0) {
+				return p->node;
+			}
+		}
+		return nullptr;
+	}
+
+	void unlinkNext(Node *a)
+	{
+		Node *b = a->next;
+		if (b != nullptr) {
+			if (_end == b) {
+				_end = a;
+			}
+			a->next = b->next;
+			free((void *)b->node_name);
+			free(b);
+		}
+	}
+
+private:
+	Node *_top;
+	Node *_end;
+};
+
 /**
  * Per-object device instance.
  */
@@ -226,11 +311,11 @@ public:
   DeviceMaster(Flavor f);
   virtual ~DeviceMaster();
 
-  static uORB::DeviceNode* GetDeviceNode( const std::string& node_name );
+  static uORB::DeviceNode* GetDeviceNode( const char * node_name );
   virtual int   ioctl(struct file *filp, int cmd, unsigned long arg);
 private:
   Flavor      _flavor;
-  static std::map<std::string, uORB::DeviceNode*> _node_map;
+  static ORBMap _node_map;
 };
 
 

src/modules/uORB/uORBDevices_posix.cpp

@@ -652,12 +652,13 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
   }
 }
 
-uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath )
+uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath )
 {
   uORB::DeviceNode* rc = nullptr;
-  if( _node_map.find( nodepath ) != _node_map.end() )
+  std::string np(nodepath);
+  if( _node_map.find( np ) != _node_map.end() )
   {
-    rc = _node_map[nodepath];
+    rc = _node_map[np];
   }
   return rc;
 }

src/modules/uORB/uORBDevices_posix.hpp

@@ -159,7 +159,7 @@ public:
   DeviceMaster(Flavor f);
   ~DeviceMaster();
 
-  static uORB::DeviceNode* GetDeviceNode( const std::string& node_name );
+  static uORB::DeviceNode* GetDeviceNode( const char *node_name );
 
   virtual int   ioctl(device::file_t *filp, int cmd, unsigned long arg);
 private:

src/modules/uORB/uORBManager.hpp

@@ -36,10 +36,14 @@
 
 #include "uORBCommon.hpp"
 #include "uORBDevices.hpp"
-#include <map>
-#include <string>
 #include <stdint.h>
+#ifdef __PX4_NUTTX
+#include "ORBSet.h"
+#else
+#include <string>
 #include <set>
+#define ORBSet std::set<std::string>
+#endif
 
 #include "uORBCommunicator.hpp"
 
@@ -306,7 +310,7 @@ class uORB::Manager  : public uORBCommunicator::IChannelRxHandler
    * Utility method to check if there is a remote subscriber present
    * for a given topic
    */
-  bool is_remote_subscriber_present( const std::string& messageName );
+  bool is_remote_subscriber_present( const char * messageName );
 
  private: // class methods
   /**
@@ -345,7 +349,7 @@ class uORB::Manager  : public uORBCommunicator::IChannelRxHandler
   static Manager _Instance;
   // the communicator channel instance.
   uORBCommunicator::IChannel* _comm_channel;
-  std::set<std::string> _remote_subscriber_topics;
+  ORBSet _remote_subscriber_topics;
 
  private: //class methods
   Manager();
@@ -362,7 +366,7 @@ class uORB::Manager  : public uORBCommunicator::IChannelRxHandler
      *    handler.
      *  otherwise = failure.
      */
-    virtual int16_t process_add_subscription(const std::string& messageName,
+    virtual int16_t process_add_subscription(const char * messageName,
                                              int32_t msgRateInHz);
 
     /**
@@ -375,7 +379,7 @@ class uORB::Manager  : public uORBCommunicator::IChannelRxHandler
      *    handler.
      *  otherwise = failure.
      */
-    virtual int16_t process_remove_subscription(const std::string& messageName);
+    virtual int16_t process_remove_subscription(const char * messageName);
 
     /**
      * Interface to process the received data message.
@@ -391,7 +395,7 @@ class uORB::Manager  : public uORBCommunicator::IChannelRxHandler
      *    handler.
      *  otherwise = failure.
      */
-    virtual int16_t process_received_message(const std::string& messageName,
+    virtual int16_t process_received_message(const char * messageName,
                                              int32_t length, uint8_t* data);
 
 };

src/modules/uORB/uORBManager_nuttx.cpp

@@ -298,11 +298,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
-int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
+int16_t uORB::Manager::process_add_subscription(const char * messageName,
                                                 int32_t msgRateInHz)
 {
   warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
-          __LINE__, messageName.c_str() );
+          __LINE__, messageName );
   int16_t rc = 0;
   _remote_subscriber_topics.insert( messageName );
   char nodepath[orb_maxpath];
@@ -312,7 +312,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
     uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
     if ( node == nullptr) {
       warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
-             __LINE__, messageName.c_str() );
+             __LINE__, messageName );
     }
     else{
       // node is present.
@@ -327,10 +327,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::Manager::process_remove_subscription(
-    const std::string& messageName)
+    const char * messageName)
 {
   warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
-          __LINE__, messageName.c_str() );
+          __LINE__, messageName );
   int16_t rc = -1;
   _remote_subscriber_topics.erase( messageName );
   char nodepath[orb_maxpath];
@@ -340,7 +340,7 @@ int16_t uORB::Manager::process_remove_subscription(
     // get the node name.
     if ( node == nullptr) {
       warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
-              __LINE__, messageName.c_str());
+              __LINE__, messageName);
     } else {
       // node is present.
       node->process_remove_subscription();
@@ -352,10 +352,10 @@ int16_t uORB::Manager::process_remove_subscription(
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
-int16_t uORB::Manager::process_received_message(const std::string& messageName,
+int16_t uORB::Manager::process_received_message(const char * messageName,
                                                 int32_t length, uint8_t* data)
 {
-  //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() );
+  //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
 
   int16_t rc = -1;
   char nodepath[orb_maxpath];
@@ -365,7 +365,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName,
     // get the node name.
     if ( node == nullptr) {
       warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
-              __LINE__, messageName.c_str(), nodepath );
+              __LINE__, messageName, nodepath );
 
     } else {
       // node is present.
@@ -376,7 +376,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName,
   return rc;
 }
 
-bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName )
+bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
 {
-  return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() );
+  return _remote_subscriber_topics.find( messageName );
 }

src/modules/uORB/uORBManager_posix.cpp

@@ -312,11 +312,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void )
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
-int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
+int16_t uORB::Manager::process_add_subscription(const char *messageName,
                                                 int32_t msgRateInHz)
 {
   warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s",
-          __LINE__, messageName.c_str() );
+          __LINE__, messageName );
   int16_t rc = 0;
   _remote_subscriber_topics.insert( messageName );
   char nodepath[orb_maxpath];
@@ -326,7 +326,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
     uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath );
     if ( node == nullptr) {
       warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet",
-             __LINE__, messageName.c_str() );
+             __LINE__, messageName );
     }
     else{
       // node is present.
@@ -341,10 +341,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName,
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int16_t uORB::Manager::process_remove_subscription(
-    const std::string& messageName)
+    const char * messageName)
 {
   warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s",
-          __LINE__, messageName.c_str() );
+          __LINE__, messageName );
   int16_t rc = -1;
   _remote_subscriber_topics.erase( messageName );
   char nodepath[orb_maxpath];
@@ -354,7 +354,7 @@ int16_t uORB::Manager::process_remove_subscription(
     // get the node name.
     if ( node == nullptr) {
       warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
-              __LINE__, messageName.c_str());
+              __LINE__, messageName);
     } else {
       // node is present.
       node->process_remove_subscription();
@@ -366,10 +366,10 @@ int16_t uORB::Manager::process_remove_subscription(
 
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
-int16_t uORB::Manager::process_received_message(const std::string& messageName,
+int16_t uORB::Manager::process_received_message(const char * messageName,
                                                 int32_t length, uint8_t* data)
 {
-  //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() );
+  //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName );
 
   int16_t rc = -1;
   char nodepath[orb_maxpath];
@@ -379,7 +379,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName,
     // get the node name.
     if ( node == nullptr) {
       warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]",
-              __LINE__, messageName.c_str(), nodepath );
+              __LINE__, messageName, nodepath );
 
     } else {
       // node is present.
@@ -390,7 +390,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName,
   return rc;
 }
 
-bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName )
+bool uORB::Manager::is_remote_subscriber_present( const char * messageName )
 {
   return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() );
 }

src/modules/uORB/uORBUtils.cpp

@@ -65,14 +65,14 @@ int uORB::Utils::node_mkpath
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 int uORB::Utils::node_mkpath(char *buf, Flavor f,
-                               const std::string& orbMsgName )
+                               const char *orbMsgName )
 {
   unsigned len;
 
   unsigned index = 0;
 
   len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param",
-                 orbMsgName.c_str(), index );
+                 orbMsgName, index );
 
   if (len >= orb_maxpath)
     return -ENAMETOOLONG;

src/modules/uORB/uORBUtils.hpp

@@ -55,7 +55,7 @@ public:
   /**
    * same as above except this generators the path based on the string.
    */
-  static int node_mkpath(char *buf, Flavor f, const std::string& orbMsgName);
+  static int node_mkpath(char *buf, Flavor f, const char *orbMsgName);
 
 };