a1012112796/test_ros
Archived
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

dataService adapter 初稿

Browse Source 
Signed-off-by: a1012112796 <1012112796@qq.com>
This commit is contained in:
a1012112796
2021-09-16 16:44:21 +08:00
parent 6b98790c1b
commit a9773a6f78
12 changed files with 750 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
src/dataservice/CMakeLists.txt Normal file
View File

@@ -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)

37
src/dataservice/include/dataservice/DataServiceItem.h Executable file
View File

@@ -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

45
src/dataservice/include/dataservice/Dataservice_lib.h Executable file
View File

@@ -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

4
src/dataservice/launch/debug.launch Normal file
View File

@@ -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>

4
src/dataservice/launch/test.launch Normal file
View File

@@ -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>

74
src/dataservice/package.xml Normal file
View File

@@ -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>

298
src/dataservice/src/dataServiceAdapter.cpp Normal file
View File

@@ -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);
}

41
src/dataservice/src/testAdapt.cpp Normal file
View File

@@ -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);
}

33
src/dataservice/src/testAdapt2.cpp Normal file
View File

@@ -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);
}

2
src/test_topic/launch/test1.launch
View File

@@ -7,4 +7,4 @@
<node pkg="test_topic" type="listener" name="serial_listen" output="screen">
<param name="port" value="$(arg port_out)" />
</node>
</launch>
</launch>

1
src/test_topic/msg/Bytes.msg
View File

@@ -2,3 +2,4 @@ uint8[] data
time tick
int64 length
int64 id
string type

2
src/test_topic/src/minHandler.cpp
View File

@@ -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;