diff --git a/src/test_topic/CMakeLists.txt b/src/test_topic/CMakeLists.txt index 11f9219..49cef1a 100644 --- a/src/test_topic/CMakeLists.txt +++ b/src/test_topic/CMakeLists.txt @@ -148,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) diff --git a/src/test_topic/include/test_topic/minPublisher.h b/src/test_topic/include/test_topic/minPublisher.h index b2b61b4..77a9dbd 100644 --- a/src/test_topic/include/test_topic/minPublisher.h +++ b/src/test_topic/include/test_topic/minPublisher.h @@ -4,16 +4,19 @@ #include #include #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 \ No newline at end of file diff --git a/src/test_topic/launch/test1.launch b/src/test_topic/launch/test1.launch index ad85f00..a5639f1 100644 --- a/src/test_topic/launch/test1.launch +++ b/src/test_topic/launch/test1.launch @@ -1,6 +1,10 @@ - - - + + + + + + + diff --git a/src/test_topic/src/minPublisher.cpp b/src/test_topic/src/minPublisher.cpp index 316ce44..7ca72b8 100644 --- a/src/test_topic/src/minPublisher.cpp +++ b/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("test_topic", 200); +} + +bool MinPublisher::node_init(void) +{ + std::string port, topic; + int baud = 115200; + ros::param::param("~port", port, "/dev/ttyUSB0"); + ros::param::param("~baud", baud, 115200); + ros::param::param("~topic", topic, "test_topic"); + + ROS_INFO("will send '%s' tiopic", topic.c_str()); + _sender = _node->advertise("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; }