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

代码初稿

Browse Source 
Signed-off-by: a1012112796 <1012112796@qq.com>
This commit is contained in:
a1012112796
2021-09-03 18:11:12 +08:00
commit a542762244
12 changed files with 801 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.catkin_workspace Normal file
View File

@@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

4
.gitignore vendored Normal file
View File

@@ -0,0 +1,4 @@
.vscode/
build/
devel/
*.o

1
src/CMakeLists.txt Symbolic link
View File

@@ -0,0 +1 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

212
src/test_topic/CMakeLists.txt Normal file
View File

@@ -0,0 +1,212 @@
cmake_minimum_required(VERSION 3.0.2)
project(test_topic)
## 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
std_msgs
)
## 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 test_topic
# CATKIN_DEPENDS roscpp std_msgs
# 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}/test_topic.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/test_topic_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(talker src/minPublisher.cpp src/WzSerialportPlus.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_executable(listener src/minSubscriber.cpp src/WzSerialportPlus.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_executable(time_tacker src/timerPublisher.cpp)
target_link_libraries(time_tacker ${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
talker
listener
time_tacker
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_test_topic.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)

71
src/test_topic/include/test_topic/WzSerialportPlus.h Executable file
View File

@@ -0,0 +1,71 @@
/**
* @file WzSerialportPlus.h
* @author ouyangwei
* @brief library for serial port in linux
* @version 0.1
* @date 2020-06-15
* @copyright Copyright (c) 2020
*/
#ifndef __WZSERIALPORTPLUS_H__
#define __WZSERIALPORTPLUS_H__
#include <iostream>
#include <string>
#include <thread>
#include <functional>
using ReceiveCallback = std::function<void (char*,int)>;
class WzSerialportPlus
{
public:
WzSerialportPlus();
/**
* @param name: serialport name , such as: /dev/ttyS1
* @param baudrate: baud rate , valid: 2400,4800,9600,19200,38400,57600,115200,230400
* @param stopbit: stop bit , valid: 1,2
* @param databit: data bit , valid: 7,8
* @param paritybit: parity bit , valid: 'o'/'O' for odd,'e'/'E' for even,'n'/'N' for none
*/
WzSerialportPlus(const std::string& name,
const int& baudrate,
const int& stopbit,
const int& databit,
const int& paritybit);
~WzSerialportPlus();
bool open();
/**
* @brief parameters same as constructor
*/
bool open(const std::string& name,
const int& baudrate,
const int& stopbit,
const int& databit,
const int& paritybit);
void close();
int send(const char* data,int length);
void setReceiveCalback(ReceiveCallback receiveCallback);
protected:
virtual void onReceive(char* data,int length);
private:
int serialportFd;
std::string name;
int baudrate;
int stopbit;
int databit;
int paritybit;
bool receivable;
int receiveMaxlength;
int receiveTimeout; /* unit: ms */
ReceiveCallback receiveCallback;
};
#endif

21
src/test_topic/include/test_topic/minPublisher.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef __MIN_PUBLISHER_H__
#define __MIN_PUBLISHER_H__
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <queue>
#include "test_topic/WzSerialportPlus.h"
using MsgType = std_msgs::String;
class MinPublisher: ros::NodeHandle {
public:
MinPublisher();
~MinPublisher();
void run(const char *device);
private:
ros::Publisher _sender;
std::queue<MsgType> _queue;
WzSerialportPlus _serial;
};
#endif

21
src/test_topic/include/test_topic/minSubscriber.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef __MIN_SUBSCRIBER_H__
#define __MIN_SUBSCRIBER_H__
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <queue>
#include "test_topic/WzSerialportPlus.h"
using MsgType = std_msgs::String;
class MinSubscriber: ros::NodeHandle {
public:
MinSubscriber();
~MinSubscriber();
void run(const char *device);
private:
ros::Subscriber _sub;
WzSerialportPlus _serial;
void callback(const boost::shared_ptr<MsgType const> &msg);
};
#endif

65
src/test_topic/package.xml Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package format="2">
<name>test_topic</name>
<version>0.0.0</version>
<description>The test_topic 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/test_topic</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>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

269
src/test_topic/src/WzSerialportPlus.cpp Normal file
View File

@@ -0,0 +1,269 @@
#include "test_topic/WzSerialportPlus.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>
#include <getopt.h>
#include <string.h>
#include <time.h>
#include <sys/select.h>
WzSerialportPlus::WzSerialportPlus()
: serialportFd(-1),
name(""),
baudrate(9600),
stopbit(1),
databit(8),
paritybit('n'),
receivable(false),
receiveMaxlength(2048),
receiveTimeout(5000),
receiveCallback(nullptr)
{
}
WzSerialportPlus::WzSerialportPlus(const std::string& name,
const int& baudrate,
const int& stopbit,
const int& databit,
const int& paritybit):
serialportFd(-1),
name(name),
baudrate(baudrate),
stopbit(stopbit),
databit(databit),
paritybit(paritybit),
receivable(false),
receiveMaxlength(2048),
receiveTimeout(5000),
receiveCallback(nullptr)
{
}
WzSerialportPlus::~WzSerialportPlus()
{
close();
while(receivable)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
printf("[WzSerialportPlus::~WzSerialportPlus()]: destructed...\n");
}
bool WzSerialportPlus::open()
{
serialportFd = ::open(name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if(serialportFd < 0)
{
printf("[WzSerialportPlus::open()]: open failed with serialportFd is %d , maybe permission denied or this serialport is opened!\n",serialportFd);
return false;
}
if (fcntl(serialportFd, F_SETFL, 0) < 0)
{
printf("[WzSerialportPlus::open()]: open failed with fcntl failed!\n");
return false;
}
if (isatty(serialportFd) == 0)
{
printf("[WzSerialportPlus::open()]: open failed with standard input is not a terminal device!\n");
::close(serialportFd);
return false;
}
struct termios newtio, oldtio;
if (tcgetattr(serialportFd, &oldtio) != 0)
{
printf("[WzSerialportPlus::open()]: open failed with tcgetattr failed!\n");
return false;
}
bzero(&newtio, sizeof(newtio));
newtio.c_cflag |= CLOCAL | CREAD;
newtio.c_cflag &= ~CSIZE;
/* databit init */
switch (databit)
{
case 7:
newtio.c_cflag |= CS7;
break;
case 8:
newtio.c_cflag |= CS8;
break;
default:
printf("[WzSerialportPlus::open()]: open failed with invalid databit %d!\n",databit);
return false;
}
/* paritybit init */
switch (paritybit)
{
case 'o':
case 'O':
newtio.c_cflag |= PARENB;
newtio.c_cflag |= PARODD;
newtio.c_iflag |= (INPCK | ISTRIP);
break;
case 'e':
case 'E':
newtio.c_iflag |= (INPCK | ISTRIP);
newtio.c_cflag |= PARENB;
newtio.c_cflag &= ~PARODD;
break;
case 'n':
case 'N':
newtio.c_cflag &= ~PARENB;
break;
default:
printf("[WzSerialportPlus::open()]: open failed with invalid paritybit %d!\n",paritybit);
return false;
}
/* stopbit init */
switch (stopbit)
{
case 1:
newtio.c_cflag &= ~CSTOPB;
break;
case 2:
newtio.c_cflag |= CSTOPB;
break;
default:
printf("[WzSerialportPlus::open()]: open failed with invalid stopbit %d!\n",stopbit);
return false;
}
/* baudrate init */
switch (baudrate)
{
case 2400:
cfsetispeed(&newtio, B2400);
cfsetospeed(&newtio, B2400);
break;
case 4800:
cfsetispeed(&newtio, B4800);
cfsetospeed(&newtio, B4800);
break;
case 9600:
cfsetispeed(&newtio, B9600);
cfsetospeed(&newtio, B9600);
break;
case 19200:
cfsetispeed(&newtio, B19200);
cfsetospeed(&newtio, B19200);
break;
case 38400:
cfsetispeed(&newtio, B38400);
cfsetospeed(&newtio, B38400);
break;
case 57600:
cfsetispeed(&newtio, B57600);
cfsetospeed(&newtio, B57600);
break;
case 115200:
cfsetispeed(&newtio, B115200);
cfsetospeed(&newtio, B115200);
break;
case 230400:
cfsetispeed(&newtio, B230400);
cfsetospeed(&newtio, B230400);
break;
default:
printf("[WzSerialportPlus::open()]: open failed with invalid baudrate %d!\n",baudrate);
return false;
}
newtio.c_cc[VTIME] = 1;
newtio.c_cc[VMIN] = 1;
tcflush(serialportFd,TCIFLUSH);
if (tcsetattr(serialportFd, TCSANOW, &newtio) != 0)
{
printf("[WzSerialportPlus::open()]: open failed with tcgetattr failed!\n");
return false;
}
tcflush(serialportFd, TCIOFLUSH);
fcntl(serialportFd, F_SETFL, 0);
receivable = true;
std::thread([&]{
char* receiveData = new char[receiveMaxlength];
int receivedLength = 0;
int selectResult = -1;
while (receivable)
{
memset(receiveData,0,receiveMaxlength);
receivedLength = read(serialportFd, receiveData, receiveMaxlength); /* block util data received */
if(receivedLength > 0)
{
onReceive(receiveData,receivedLength);
if(nullptr != receiveCallback)
{
receiveCallback(receiveData,receivedLength);
}
}
receivedLength = 0;
}
delete[] receiveData;
receiveData = nullptr;
}).detach();
printf("[WzSerialportPlus::open()]: open success.\n");
return true;
}
bool WzSerialportPlus::open(const std::string& name,
const int& baudrate,
const int& stopbit,
const int& databit,
const int& paritybit)
{
this->name = name;
this->baudrate = baudrate;
this->stopbit = stopbit;
this->databit = databit;
this->paritybit = paritybit;
return open();
}
void WzSerialportPlus::close()
{
if(receivable)
{
receivable = false;
}
if(serialportFd >= 0)
{
::close(serialportFd);
serialportFd = -1;
}
}
int WzSerialportPlus::send(const char* data,int length)
{
int lengthSent = 0;
lengthSent = write(serialportFd, data, length);
return lengthSent;
}
void WzSerialportPlus::setReceiveCalback(ReceiveCallback receiveCallback)
{
this->receiveCallback = receiveCallback;
}
void WzSerialportPlus::onReceive(char* data,int length)
{
}

49
src/test_topic/src/minPublisher.cpp Normal file
View File

@@ -0,0 +1,49 @@
#include "test_topic/minPublisher.h"
MinPublisher::MinPublisher()
{
_sender = this->advertise<MsgType>("test_topic", 200);
}
MinPublisher::~MinPublisher()
{
_serial.close();
}
void MinPublisher::run(const char *device)
{
_serial.setReceiveCalback([this](char* str,int length){
MsgType msg;
std::stringstream ss;
ss << str;
msg.data = ss.str();
this->_queue.push(msg);
});
_serial.open(device, 115200, 1, 8, 'N');
while (ros::ok())
{
while(_queue.size() > 0)
{
MsgType msg = _queue.front();
ROS_INFO("send: '%s'", msg.data.c_str());
_sender.publish(msg);
_queue.pop();
}
usleep(50);
}
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "test_pub_node");
MinPublisher nh;
nh.run("/dev/ttyUSB0");
return 0;
}

33
src/test_topic/src/minSubscriber.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include "test_topic/minSubscriber.h"
MinSubscriber::MinSubscriber()
{
_sub = this->subscribe<MsgType>("test_topic", 200, boost::bind(&MinSubscriber::callback, this, _1));
}
MinSubscriber::~MinSubscriber()
{
_serial.close();
}
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()));
}
void MinSubscriber::run(const char *device)
{
_serial.open(device, 115200, 1, 8, 'N');
ros::spin();
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "test_sub_node");
MinSubscriber nh;
nh.run("/dev/ttyUSB0");
return 0;
}

54
src/test_topic/src/timerPublisher.cpp Normal file
View File

@@ -0,0 +1,54 @@
#include <ros/ros.h>
#include <std_msgs/String.h>
using MsgType = std_msgs::String;
class TimerPublisher: ros::NodeHandle {
public:
TimerPublisher();
~TimerPublisher();
void run();
private:
ros::Publisher _sender;
int _count = 0;
};
TimerPublisher::TimerPublisher()
{
_sender = this->advertise<MsgType>("test_topic", 200);
_count = 0;
}
TimerPublisher::~TimerPublisher()
{
}
void TimerPublisher::run()
{
ros::Rate loop_rate(10);
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "hello world " << _count++;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
_sender.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "timer_pub_node");
TimerPublisher nh;
nh.run();
return 0;
}