From a9773a6f78467a69175b4101ac19f7f67563c34f Mon Sep 17 00:00:00 2001 From: a1012112796 <1012112796@qq.com> Date: Thu, 16 Sep 2021 16:44:21 +0800 Subject: [PATCH] =?UTF-8?q?dataService=20adapter=20=E5=88=9D=E7=A8=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: a1012112796 <1012112796@qq.com> --- src/dataservice/CMakeLists.txt | 211 +++++++++++++ .../include/dataservice/DataServiceItem.h | 37 +++ .../include/dataservice/Dataservice_lib.h | 45 +++ src/dataservice/launch/debug.launch | 4 + src/dataservice/launch/test.launch | 4 + src/dataservice/package.xml | 74 +++++ src/dataservice/src/dataServiceAdapter.cpp | 298 ++++++++++++++++++ src/dataservice/src/testAdapt.cpp | 41 +++ src/dataservice/src/testAdapt2.cpp | 33 ++ src/test_topic/launch/test1.launch | 2 +- src/test_topic/msg/Bytes.msg | 1 + src/test_topic/src/minHandler.cpp | 2 +- 12 files changed, 750 insertions(+), 2 deletions(-) create mode 100644 src/dataservice/CMakeLists.txt create mode 100755 src/dataservice/include/dataservice/DataServiceItem.h create mode 100755 src/dataservice/include/dataservice/Dataservice_lib.h create mode 100644 src/dataservice/launch/debug.launch create mode 100644 src/dataservice/launch/test.launch create mode 100644 src/dataservice/package.xml create mode 100644 src/dataservice/src/dataServiceAdapter.cpp create mode 100644 src/dataservice/src/testAdapt.cpp create mode 100644 src/dataservice/src/testAdapt2.cpp diff --git a/src/dataservice/CMakeLists.txt b/src/dataservice/CMakeLists.txt new file mode 100644 index 0000000..1db7fb0 --- /dev/null +++ b/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) diff --git a/src/dataservice/include/dataservice/DataServiceItem.h b/src/dataservice/include/dataservice/DataServiceItem.h new file mode 100755 index 0000000..a8fae93 --- /dev/null +++ b/src/dataservice/include/dataservice/DataServiceItem.h @@ -0,0 +1,37 @@ +#ifndef _DATASERVICE_ITEM_H +#define _DATASERVICE_ITEM_H + +#include +#include +#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 diff --git a/src/dataservice/include/dataservice/Dataservice_lib.h b/src/dataservice/include/dataservice/Dataservice_lib.h new file mode 100755 index 0000000..abf133d --- /dev/null +++ b/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 diff --git a/src/dataservice/launch/debug.launch b/src/dataservice/launch/debug.launch new file mode 100644 index 0000000..89d3505 --- /dev/null +++ b/src/dataservice/launch/debug.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/src/dataservice/launch/test.launch b/src/dataservice/launch/test.launch new file mode 100644 index 0000000..2c6026e --- /dev/null +++ b/src/dataservice/launch/test.launch @@ -0,0 +1,4 @@ + + + + diff --git a/src/dataservice/package.xml b/src/dataservice/package.xml new file mode 100644 index 0000000..605268c --- /dev/null +++ b/src/dataservice/package.xml @@ -0,0 +1,74 @@ + + + dataservice + 0.0.0 + The dataservice package + + + + + zzc + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + std_srvs + test_topic + roscpp + rospy + std_msgs + std_srvs + test_topic + roscpp + rospy + std_msgs + std_srvs + test_topic + + + + + + + + diff --git a/src/dataservice/src/dataServiceAdapter.cpp b/src/dataservice/src/dataServiceAdapter.cpp new file mode 100644 index 0000000..acfe95e --- /dev/null +++ b/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 +#include +#include + +#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 &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 &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 _pubs; + void close_all_pubs(void); + std::vector _pub_names; + + std::vector _sub_names; + std::vector _subs; + std::map _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(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(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); +} diff --git a/src/dataservice/src/testAdapt.cpp b/src/dataservice/src/testAdapt.cpp new file mode 100644 index 0000000..f29463a --- /dev/null +++ b/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); +} diff --git a/src/dataservice/src/testAdapt2.cpp b/src/dataservice/src/testAdapt2.cpp new file mode 100644 index 0000000..1a8f537 --- /dev/null +++ b/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); +} diff --git a/src/test_topic/launch/test1.launch b/src/test_topic/launch/test1.launch index a5639f1..aebc8ad 100644 --- a/src/test_topic/launch/test1.launch +++ b/src/test_topic/launch/test1.launch @@ -7,4 +7,4 @@ - + diff --git a/src/test_topic/msg/Bytes.msg b/src/test_topic/msg/Bytes.msg index 236ddc5..6e88f2c 100644 --- a/src/test_topic/msg/Bytes.msg +++ b/src/test_topic/msg/Bytes.msg @@ -2,3 +2,4 @@ uint8[] data time tick int64 length int64 id +string type diff --git a/src/test_topic/src/minHandler.cpp b/src/test_topic/src/minHandler.cpp index 1ef33cf..1ff58f4 100644 --- a/src/test_topic/src/minHandler.cpp +++ b/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;