解决首次编译的bug

Signed-off-by: a1012112796 <1012112796@qq.com>

.gitignore

@@ -1,4 +1,5 @@
 .vscode/
 build/
-devel/
+devel/*
+!/devel/include
 *.o

devel/include/test_topic/Bytes.h

@@ -0,0 +1,239 @@
+// Generated by gencpp from file test_topic/Bytes.msg
+// DO NOT EDIT!
+
+
+#ifndef TEST_TOPIC_MESSAGE_BYTES_H
+#define TEST_TOPIC_MESSAGE_BYTES_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace test_topic
+{
+template <class ContainerAllocator>
+struct Bytes_
+{
+  typedef Bytes_<ContainerAllocator> Type;
+
+  Bytes_()
+    : data()
+    , tick()
+    , length(0)
+    , id(0)
+    , type()  {
+    }
+  Bytes_(const ContainerAllocator& _alloc)
+    : data(_alloc)
+    , tick()
+    , length(0)
+    , id(0)
+    , type(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other >  _data_type;
+  _data_type data;
+
+   typedef ros::Time _tick_type;
+  _tick_type tick;
+
+   typedef int64_t _length_type;
+  _length_type length;
+
+   typedef int64_t _id_type;
+  _id_type id;
+
+   typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other >  _type_type;
+  _type_type type;
+
+
+
+
+
+  typedef boost::shared_ptr< ::test_topic::Bytes_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::test_topic::Bytes_<ContainerAllocator> const> ConstPtr;
+
+}; // struct Bytes_
+
+typedef ::test_topic::Bytes_<std::allocator<void> > Bytes;
+
+typedef boost::shared_ptr< ::test_topic::Bytes > BytesPtr;
+typedef boost::shared_ptr< ::test_topic::Bytes const> BytesConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::test_topic::Bytes_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::test_topic::Bytes_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator==(const ::test_topic::Bytes_<ContainerAllocator1> & lhs, const ::test_topic::Bytes_<ContainerAllocator2> & rhs)
+{
+  return lhs.data == rhs.data &&
+    lhs.tick == rhs.tick &&
+    lhs.length == rhs.length &&
+    lhs.id == rhs.id &&
+    lhs.type == rhs.type;
+}
+
+template<typename ContainerAllocator1, typename ContainerAllocator2>
+bool operator!=(const ::test_topic::Bytes_<ContainerAllocator1> & lhs, const ::test_topic::Bytes_<ContainerAllocator2> & rhs)
+{
+  return !(lhs == rhs);
+}
+
+
+} // namespace test_topic
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+
+
+template <class ContainerAllocator>
+struct IsMessage< ::test_topic::Bytes_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::test_topic::Bytes_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::test_topic::Bytes_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::test_topic::Bytes_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::test_topic::Bytes_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::test_topic::Bytes_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::test_topic::Bytes_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "fd8110ce2e0bad5f5f8d1acd96e889d9";
+  }
+
+  static const char* value(const ::test_topic::Bytes_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xfd8110ce2e0bad5fULL;
+  static const uint64_t static_value2 = 0x5f8d1acd96e889d9ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::test_topic::Bytes_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "test_topic/Bytes";
+  }
+
+  static const char* value(const ::test_topic::Bytes_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::test_topic::Bytes_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "uint8[] data\n"
+"time tick\n"
+"int64 length\n"
+"int64 id\n"
+"string type\n"
+;
+  }
+
+  static const char* value(const ::test_topic::Bytes_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::test_topic::Bytes_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.data);
+      stream.next(m.tick);
+      stream.next(m.length);
+      stream.next(m.id);
+      stream.next(m.type);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct Bytes_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::test_topic::Bytes_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::test_topic::Bytes_<ContainerAllocator>& v)
+  {
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
+    }
+    s << indent << "tick: ";
+    Printer<ros::Time>::stream(s, indent + "  ", v.tick);
+    s << indent << "length: ";
+    Printer<int64_t>::stream(s, indent + "  ", v.length);
+    s << indent << "id: ";
+    Printer<int64_t>::stream(s, indent + "  ", v.id);
+    s << indent << "type: ";
+    Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + "  ", v.type);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // TEST_TOPIC_MESSAGE_BYTES_H

src/dataservice/CMakeLists.txt

@@ -0,0 +1,211 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(dataservice)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  std_srvs
+  test_topic
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES dataservice
+  CATKIN_DEPENDS roscpp rospy std_msgs std_srvs test_topic
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories( include ${catkin_INCLUDE_DIRS})
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/dataservice.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/dataservice_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+add_executable(test_1 src/testAdapt.cpp src/dataServiceAdapter.cpp ../test_topic/src/minHandler.cpp)
+target_link_libraries(test_1 ${catkin_LIBRARIES})
+
+add_executable(test_2 src/testAdapt2.cpp src/dataServiceAdapter.cpp ../test_topic/src/minHandler.cpp)
+target_link_libraries(test_2 ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  test_1
+  test_2
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dataservice.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/dataservice/include/dataservice/DataServiceItem.h

@@ -0,0 +1,37 @@
+#ifndef _DATASERVICE_ITEM_H
+#define _DATASERVICE_ITEM_H
+
+#include <ros/ros.h>
+#include <stdint.h>
+#include "string.h"
+
+enum EnumerationPriority 
+{
+	Low, 
+	Medium,
+	High,
+	UltraHigh
+};
+
+struct DataServiceItem {
+	uint32_t ID;
+    uint8_t char_data[8];
+    EnumerationPriority _priority;
+};
+
+struct DataServiceItemValue {
+	ros::Time _tick;
+	uint32_t ID;	
+	double value;
+	uint8_t len;
+	float ratio;
+	EnumerationPriority _priority;
+	
+	uint8_t PlaneType;
+	uint8_t GroupID;
+	uint8_t PlaneID;
+	bool multiplane_flag;
+	bool Geo_Manage_flag;
+};
+
+#endif //_DATASERVICE_ITEM_H

src/dataservice/include/dataservice/Dataservice_lib.h

@@ -0,0 +1,45 @@
+#ifndef _LIBDATASERVICE_WAPPER_H
+#define _LIBDATASERVICE_WAPPER_H
+
+#include "dataservice/DataServiceItem.h"
+#include "test_topic/minHandler.h"
+
+using namespace xyfc_std;
+
+#define  DATATYPE_COLL_HIGH   	"imu_data/coll_high"
+#define  DATATYPE_COLL_MEDIUM   "imu_data/coll_mediun"
+#define  DATATYPE_COLL_LOW   	"imu_data/coll_low"
+#define  DATATYPE_CTRL   "control_data"
+#define  DATATYPE_LOCAL  "local_data"
+
+void initDataService(int argc, char **argv, const char *node);
+// TODO:
+// void initMultiPlaneDataService(struct data_service_env  env);
+
+int DataService_Run(ThreadFuncType &fn);
+bool DataService_ok(void);
+
+void DataService_registPublisher(const char *dataType);
+void DataService_registSubscriber(const char *dataType);
+bool publish(DataServiceItem * itemlist, int count, const char *dataType);
+// TODO:
+// bool PublishItemListValue(DataServiceItemValue * itemvaluelist, int count, int dataType);
+// bool PublishItemValue(DataServiceItemValue * item, int dataType);
+
+int GetData(DataServiceItem * itemlist, const char *dataType);
+// TODO:
+// bool GetDataValue(DataServiceItemValue * itemlist, int dataType,int processid);	
+// bool Check_Update(uint32_t dataid,int dataType,int processid);
+
+// bool Check_Item_Is_Update(uint8_t planetype,uint8_t groupid,uint8_t planeid,uint8_t dataid,int dataType,int processid);
+// double Get_One_Itemvalue(uint8_t planetype,uint8_t groupid,uint8_t planeid,rt_uint16_t dataid,int dataType,int processid);
+// int Get_More_itemvalue(DataServiceItemValue * itemvaluelist,int count,int dataType,int processid);
+// int Get_Oneplane_update_num(uint8_t planetype,uint8_t groupid,uint8_t planeid,int dataType,int processid);
+// int Get_Oneplane_update_infor(DataServiceItemValue * itemvaluelist,int count,int start_pos,uint8_t planetype,uint8_t groupid,uint8_t planeid,int dataType,int processid);
+// int Get_Allplane_item_num(rt_uint16_t dataid,int dataType);
+// int Get_Allplane_item_infor(DataServiceItemValue * itemvaluelist,int count,int start_pos,rt_uint16_t dataid,int dataType,int processid);
+	
+// int getNewDatafromPos(DataServiceItem * itemlist, int count, int dataType,int processid);
+// int getTotalNum(int dataType);	
+
+#endif

src/dataservice/launch/debug.launch

@@ -0,0 +1,4 @@
+<launch>
+    <node pkg="dataservice" type="test_2" name="test_2" output="screen" launch-prefix="xterm -e gdb -ex run --args">
+    </node>
+</launch>

src/dataservice/launch/test.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="dataservice" type="test_1" name="test_talker"/>
+  <node pkg="dataservice" type="test_2" name="test_listener" output="screen"/>
+</launch>

src/dataservice/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dataservice</name>
+  <version>0.0.0</version>
+  <description>The dataservice package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zzc@todo.todo">zzc</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/dataservice</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>test_topic</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>std_srvs</build_export_depend>
+  <build_export_depend>test_topic</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>test_topic</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/dataservice/src/dataServiceAdapter.cpp

@@ -0,0 +1,298 @@
+/**
+ * @file dataServiceAdapter.cpp
+ *
+ * @brief dataService 接口适配 ros topic 机制
+ *
+ * @version 0.1
+ * @date 2021-09-16
+ * 
+ * changelog:
+ * 
+ */
+
+#include "dataservice/Dataservice_lib.h"
+#include <map>
+#include <vector>
+#include <atomic>
+
+#define INDEX_OF_DataServiceItem 1
+
+inline void Lock(std::atomic_flag& lock)
+{
+    while ( lock.test_and_set());
+}
+
+inline void UnLock(std::atomic_flag& lock)
+{
+    lock.clear();
+}
+
+class topicCache
+{
+private:
+    MsgType *_msg;
+    // std::atomic_flag *_lock;
+    bool _updated;
+public:
+    topicCache();
+    ~topicCache();
+    void callback(const boost::shared_ptr<MsgType const> &msg);
+    bool get_data(MsgType *pData);
+};
+
+bool topicCache::get_data(MsgType *pData)
+{
+    // Lock(*_lock);
+    if (!_updated)
+    {
+        return false;
+    }
+
+    pData->id = _msg->id;
+    pData->length = _msg->length;
+    pData->tick = _msg->tick;
+    pData->type = _msg->type;
+    pData->data.clear();
+    for (int i = 0; i < _msg->length; i++)
+    {
+        pData->data.push_back(_msg->data[i]);
+    }
+
+    _updated = false;
+
+    // UnLock(*_lock);
+    return true;
+}
+
+topicCache::topicCache()
+{
+    _msg = new MsgType;
+    // _lock = new std::atomic_flag(ATOMIC_FLAG_INIT);
+    _updated = false;
+}
+
+topicCache::~topicCache()
+{
+}
+
+void topicCache::callback(const boost::shared_ptr<MsgType const> &msg)
+{
+    // Lock(*_lock);
+    // ROS_INFO("received data ...");
+
+    _msg->id = msg->id;
+    _msg->length = msg->length;
+    _msg->tick = msg->tick;
+    _msg->type = msg->type;
+    _msg->data.clear();
+    for (int i = 0; i < msg->length; i++)
+    {
+        _msg->data.push_back(msg->data[i]);
+    }
+
+    _updated = true;
+
+    // UnLock(*_lock);
+}
+
+class dataServiceAdapter: public MinHandler
+{
+public:
+    dataServiceAdapter(int argc, char **argv, const char *node);
+    ~dataServiceAdapter();
+    friend bool DataService_ok(void);
+    friend int DataService_Run(ThreadFuncType &fn);
+    bool publish(DataServiceItem * itemlist, int count, const char *dataType);
+    void regist_publisher(const char *dataType);
+    void regist_subscriber(const char *dataType);
+    int getData(DataServiceItem * itemlist, const char *dataType);
+
+protected:
+    virtual bool node_init(void);
+private:
+    std::map<std::string, ros::Publisher> _pubs;
+    void close_all_pubs(void);
+    std::vector<std::string> _pub_names;
+
+    std::vector<std::string> _sub_names;
+    std::vector<ros::Subscriber> _subs;
+    std::map<std::string, topicCache> _sub_caches;
+};
+
+dataServiceAdapter::dataServiceAdapter(int argc, char **argv, const char *node):
+    MinHandler(argc, argv, node),
+    _pubs()
+{
+}
+
+bool dataServiceAdapter::node_init(void)
+{
+    // init all publishers
+    for (auto dataType: _pub_names)
+    {
+        printf("type: %s\n", dataType.c_str());
+        _pubs[dataType] = _node->advertise<MsgType>(dataType, 200);
+    }
+
+    // init all subscribers
+    for (auto dataType: _sub_names)
+    {
+        printf("sub type: %s\n", dataType.c_str());
+        _sub_caches[dataType] = topicCache();
+        _subs.push_back(
+            _node->subscribe<MsgType>(dataType, 200, 
+                &topicCache::callback, &_sub_caches[dataType])
+        );
+
+    }
+
+    return true;
+}
+
+dataServiceAdapter::~dataServiceAdapter()
+{
+    close_all_pubs();
+}
+
+void dataServiceAdapter::close_all_pubs(void)
+{
+    auto iter = _pubs.begin();
+
+    while (iter != _pubs.end())
+    {
+        iter->second.shutdown();
+        iter++;
+    }
+}
+
+bool dataServiceAdapter::publish(DataServiceItem * itemlist, int count, const char *dataType)
+{
+    if (_pubs.find(dataType) == _pubs.end())
+    {
+        ROS_ERROR("publisher for '%s' not registed !", dataType);
+        return false;
+    }
+
+    MsgType msg;
+    msg.length = sizeof(DataServiceItem);
+    msg.type = dataType;
+    msg.id = INDEX_OF_DataServiceItem;
+    msg.tick = ros::Time::now();
+
+    uint8_t *pData = (uint8_t *) itemlist;
+    for(int i = 0; i < sizeof(DataServiceItem); i++)
+    {
+        msg.data.push_back(*(pData + i));
+    }
+
+    while (count-- > 0)
+    {
+        _pubs[dataType].publish(msg);
+    }
+
+    return true;
+}
+
+void dataServiceAdapter::regist_publisher(const char *dataType)
+{
+    if (std::find(_pub_names.begin(), _pub_names.end() , dataType) != _pub_names.end())
+    {
+        return;
+    }
+
+    _pub_names.push_back(dataType);
+}
+
+void dataServiceAdapter::regist_subscriber(const char *dataType)
+{
+    if (std::find(_sub_names.begin(), _sub_names.end() , dataType) != _sub_names.end())
+    {
+        return;
+    }
+
+    _sub_names.push_back(dataType);
+}
+
+int dataServiceAdapter::getData(DataServiceItem * itemlist, const char *dataType)
+{
+    MsgType msg = {};
+
+    _sub_caches[dataType].get_data(&msg);
+
+    if (msg.length != sizeof(DataServiceItem))
+    {
+        return 0;
+    }
+
+    uint8_t *pData = (uint8_t *) itemlist;
+    for (int i = 0; i < sizeof(DataServiceItem); i++)
+    {
+        *(pData + i) = msg.data[i];
+    }
+
+    return 1;
+}
+
+static dataServiceAdapter * _adapter = nullptr;
+
+void initDataService(int argc, char **argv, const char *node)
+{
+    if (_adapter != nullptr)
+    {
+        ROS_WARN("double call `initDataService`!");
+        return;
+    }
+
+    _adapter = new dataServiceAdapter(argc, argv, node);
+}
+
+int GetData(DataServiceItem * itemlist, const char *dataType)
+{
+    return _adapter->getData(itemlist, dataType);
+}
+
+int DataService_Run(ThreadFuncType &fn)
+{
+    if (_adapter == nullptr)
+    {
+        perror("must call `initDataService()` before `DataService_Run()`!");
+        return 1;
+    }
+
+    _adapter->add_thread(fn);
+    _adapter->run(4);
+
+    free(_adapter);
+
+    return 0;
+}
+
+bool DataService_ok(void)
+{
+    if (_adapter == nullptr)
+    {
+        return false;
+    }
+
+    return _adapter->ok();
+}
+
+bool publish(DataServiceItem * itemlist, int count, const char *dataType)
+{
+    if (_adapter == nullptr)
+    {
+        return false;
+    }
+
+    return _adapter->publish(itemlist, count, dataType);
+}
+
+void DataService_registPublisher(const char *dataType)
+{
+    _adapter->regist_publisher(dataType);
+}
+
+void DataService_registSubscriber(const char *dataType)
+{
+    _adapter->regist_subscriber(dataType);
+}

src/dataservice/src/testAdapt.cpp

@@ -0,0 +1,41 @@
+#include "dataservice/Dataservice_lib.h"
+
+int main(int argc, char *argv[])
+{
+    initDataService(argc, argv, "test_1");
+    DataService_registPublisher(DATATYPE_COLL_LOW);
+
+    ThreadFuncType fn = [](){
+        ROS_INFO("into main thread");
+
+        DataServiceItem test;
+        test.ID = 1;
+        for (int i = 0; i < 8; i++)
+        {
+            test.char_data[i] = 0;
+        }
+        test._priority = High;
+
+        unsigned long tmp = 0;
+
+        while (DataService_ok())
+        {
+            memcpy(test.char_data, (uint8_t *)&tmp, sizeof(tmp));
+
+            publish(&test, 1, DATATYPE_COLL_LOW);
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            std::stringstream ss;
+            ss << "send: ";
+            for(int i = 0; i < 8; i++)
+            {
+                ss << (int)test.char_data[i] << " ";
+            }
+            ROS_INFO("%s", ss.str().c_str());
+
+            tmp++;
+        }
+    };
+
+    return DataService_Run(fn);
+}

src/dataservice/src/testAdapt2.cpp

@@ -0,0 +1,33 @@
+#include "dataservice/Dataservice_lib.h"
+
+int main(int argc, char *argv[])
+{
+    initDataService(argc, argv, "test_2");
+    DataService_registSubscriber(DATATYPE_COLL_LOW);
+
+    ThreadFuncType fn = [](){
+        while (!DataService_ok());
+        ROS_INFO("into main thread");
+
+        DataServiceItem test;
+
+        while (DataService_ok())
+        {
+            printf("hhh\n");
+            if (GetData(&test, DATATYPE_COLL_LOW))
+            {
+                std::stringstream ss;
+                ss << "get: ";
+                for(int i = 0; i < 8; i++)
+                {
+                    ss << (int)test.char_data[i] << " ";
+                }
+                ROS_INFO("%s", ss.str().c_str());
+            }
+
+            std::this_thread::sleep_for(std::chrono::seconds(5));
+        }
+    };
+
+    return DataService_Run(fn);
+}

src/test_topic/CMakeLists.txt

@@ -9,8 +9,10 @@ project(test_topic)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
+  rospy
   std_msgs
   std_srvs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -47,11 +49,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES 
+  Bytes.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -68,10 +69,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -103,9 +104,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS include
 #  LIBRARIES test_topic
-#  CATKIN_DEPENDS roscpp std_msgs
+  CATKIN_DEPENDS roscpp rospy message_runtime
 #  DEPENDS system_lib
 )
 
@@ -147,7 +148,7 @@ include_directories( include ${catkin_INCLUDE_DIRS})
 #   ${catkin_LIBRARIES}
 # )
 
-add_executable(talker src/minPublisher.cpp src/WzSerialportPlus.cpp)
+add_executable(talker src/minPublisher.cpp src/WzSerialportPlus.cpp src/minHandler.cpp)
 target_link_libraries(talker ${catkin_LIBRARIES})
 
 add_executable(listener src/minSubscriber.cpp src/WzSerialportPlus.cpp src/minHandler.cpp)

src/test_topic/include/test_topic/minHandler.h

@@ -13,15 +13,15 @@
  */
 
 #include <ros/ros.h>
-#include <std_msgs/String.h>
 #include <std_srvs/Empty.h>
 #include <vector>
 #include <thread>
+#include "test_topic/Bytes.h"
 
 namespace xyfc_std
 {
 
-using MsgType = std_msgs::String;
+using MsgType = test_topic::Bytes;
 using ThreadFuncType = boost::function<void (void)>;
 
 /**

src/test_topic/include/test_topic/minPublisher.h

@@ -4,16 +4,19 @@
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 #include "test_topic/WzSerialportPlus.h"
+#include "test_topic/minHandler.h"
 
-using MsgType = std_msgs::String;
-class MinPublisher: ros::NodeHandle {
+using namespace xyfc_std;
+class MinPublisher: public MinHandler {
 public:
-    MinPublisher();
+    MinPublisher(int argc, char **argv);
     ~MinPublisher();
-    void run(const char *device);
+protected:
+    virtual bool node_init(void);
 private:
     ros::Publisher _sender;
     WzSerialportPlus _serial;
+    void handle(void);
 };
 
 #endif

src/test_topic/launch/test1.launch

@@ -1,6 +1,10 @@
 <launch>
-    <arg name="port" default="/dev/ttyUSB0" />
-    <node pkg="test_topic" type="listener" name="serial_listen">
-      <param name="port" value="$(arg port)" />
+    <arg name="port_in" default="/dev/ttyUSB0" />
+    <arg name="port_out" default="/dev/ttyUSB1" />
+    <node pkg="test_topic" type="talker" name="serial_talker">
+      <param name="port" value="$(arg port_in)" />
     </node>
-  </launch>
+    <node pkg="test_topic" type="listener" name="serial_listen" output="screen">
+      <param name="port" value="$(arg port_out)" />
+    </node>
+</launch>

src/test_topic/msg/Bytes.msg

@@ -0,0 +1,5 @@
+uint8[] data
+time tick
+int64 length
+int64 id
+string type

src/test_topic/package.xml

@@ -50,14 +50,26 @@
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>std_srvs</exec_depend>
 
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+ 
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+ 
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/test_topic/src/minHandler.cpp

@@ -58,7 +58,7 @@ void MinHandler::run(int thread_num)
         return;
     }
 
-    ROS_INFO("init '%s' node", _node_name);
+    ROS_INFO("init '%s' node %d %s", _node_name, _argc, _argv[0]);
     ros::init(_argc, _argv, _node_name);
     _node = new ros::NodeHandle;
 

src/test_topic/src/minPublisher.cpp

@@ -1,23 +1,44 @@
 #include "test_topic/minPublisher.h"
 
-MinPublisher::MinPublisher()
+MinPublisher::MinPublisher(int argc, char **argv):
+    MinHandler(argc, argv, "test_pub")
 {
-    _sender = this->advertise<MsgType>("test_topic", 200);
+}
+
+bool MinPublisher::node_init(void)
+{
+    std::string port, topic;
+    int baud = 115200;
+    ros::param::param<std::string>("~port", port, "/dev/ttyUSB0");
+    ros::param::param<int>("~baud", baud, 115200);
+    ros::param::param<std::string>("~topic", topic, "test_topic");
+
+    ROS_INFO("will send '%s' tiopic", topic.c_str());
+    _sender = _node->advertise<MsgType>("test_topic", 200);
+
+    if (!_serial.open(port, baud, 1, 8, 'N'))
+    {
+        ROS_ERROR("open serial '%s' failed!", port.c_str());
+        return false;
+    }
+
+    ThreadFuncType thd = boost::bind(&MinPublisher::handle, this);
+    add_thread(thd);
+
+    return true;
 }
 
 MinPublisher::~MinPublisher()
 {
-    _serial.close();
 }
 
-void MinPublisher::run(const char *device)
+void MinPublisher::handle()
 {
-    _serial.open(device, 115200, 1, 8, 'N');
     char *read_buf = new char[500];
 
-    while (ros::ok())
+    while (ok())
     {
-        std_msgs::String msg;
+        MsgType msg;
         std::stringstream ss;
 
         memset(read_buf, 0, 500);
@@ -27,20 +48,27 @@ void MinPublisher::run(const char *device)
         {
             continue;
         }
-        ss << read_buf;
-        msg.data = ss.str();
 
-        ROS_INFO("send: '%s'", msg.data.c_str());
+        for (int i =0; i < len; i++)
+        {
+            msg.data.push_back(read_buf[i]);
+        }
+        msg.length = len;
+        msg.tick = ros::Time::now();
+
+        ROS_INFO("send: '%s'", &msg.data[0]);
         _sender.publish(msg);
     }
+
+    printf("MinPublisher: exit handle thread\n");
+    _serial.close();
 }
 
 int main(int argc, char *argv[])
 {
-    ros::init(argc, argv, "test_pub_node");
-    MinPublisher nh;
-
-    nh.run("/dev/ttyUSB0");
+    MinPublisher *nh = new MinPublisher(argc, argv);
+    nh->run(4);
+    free(nh);
 
     return 0;
 }

src/test_topic/src/minSubscriber.cpp

@@ -32,8 +32,9 @@ MinSubscriber::~MinSubscriber()
 
 void MinSubscriber::callback(const boost::shared_ptr<MsgType const> &msg)
 {
-    ROS_INFO("I heard: [%s]", msg->data.c_str());
-    _serial.send(msg->data.c_str(), strlen(msg->data.c_str()));
+    ROS_INFO("I heard: [%s]", &msg->data[0]);
+
+    _serial.send((const char *)&msg->data[0], msg->length);
 }
 
 int main(int argc, char *argv[])

src/test_topic/src/testMinHandler.cpp

@@ -17,14 +17,13 @@ protected:
 
 void testMinHandler::callback(const boost::shared_ptr<MsgType const> &msg)
 {
-    std::stringstream ss;
-
-    ss << "handled data: " << msg->data;
-    ROS_INFO("handle :%s", msg->data.c_str());
+    ROS_INFO("handle :%s", &msg->data[0]);
     std::this_thread::sleep_for(std::chrono::seconds(2));
+    std::string str = "handled :";
+    str.append((const char *)&msg->data[0]);
 
     MsgType send;
-    send.data = ss.str();
+    send.data.assign(str.begin(), str.end());
     _pub.publish(send);
 }
 

src/test_topic/src/timerPublisher.cpp

@@ -1,7 +1,7 @@
 #include <ros/ros.h>
-#include <std_msgs/String.h>
+#include "test_topic/Bytes.h"
 
-using MsgType = std_msgs::String;
+using MsgType = test_topic::Bytes;
 class TimerPublisher: ros::NodeHandle {
 public:
     TimerPublisher();
@@ -29,13 +29,19 @@ void TimerPublisher::run()
     ros::Rate loop_rate(10);
     while (ros::ok())
     {
-        std_msgs::String msg;
+        test_topic::Bytes msg;
 
         std::stringstream ss;
         ss << "hello world " << _count++ << "\n";
-        msg.data = ss.str();
+        auto str = ss.str();
 
-        ROS_INFO("%s", msg.data.c_str());
+        msg.data.assign(str.begin(), str.end());
+        msg.data.push_back(0);
+        msg.id = 1,
+        msg.length = str.size() + 1;
+        msg.tick = ros::Time::now();
+
+        ROS_INFO("%s", str.c_str());
         _sender.publish(msg);
 
         ros::spinOnce();