From 28e0be6d0efee92abb247a7aeca166f9b06e7a76 Mon Sep 17 00:00:00 2001 From: Robin Vanhove <1r0b1n0@gmail.com> Date: Sun, 19 Nov 2017 23:36:04 +0100 Subject: [PATCH 1/8] modified the examples and update of README.md --- .gitignore | 2 + README.md | 19 +- launch/rosserial_python_server.launch | 2 + test/.gitignore | 1 + test/Basic/Basic.pro | 31 ++- test/Basic/Basic.pro.user | 16 +- test/Basic/Basic.pro_ | 36 ++++ test/LoopbackTest/Basic.pro.user | 276 ------------------------ test/LoopbackTest/LoopbackTest.pro | 31 ++- test/LoopbackTest/LoopbackTest.pro.user | 16 +- test/LoopbackTest/LoopbackTest.pro_ | 36 ++++ 11 files changed, 136 insertions(+), 330 deletions(-) create mode 100644 test/Basic/Basic.pro_ delete mode 100644 test/LoopbackTest/Basic.pro.user create mode 100644 test/LoopbackTest/LoopbackTest.pro_ diff --git a/.gitignore b/.gitignore index cf25ac6..46efdb3 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ CMakeLists.txt.user* +/ros_lib + diff --git a/README.md b/README.md index 56a0033..fcc4348 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # rosserial_qt Client implementation for rosserial using the multi-platform Qt library. -For the moment it only supports TCP/IP, but it should be easily adapted to UDP (using QUdpSocket) or serial port (using QSerialPort) communications. +For the moment it only supports TCP/IP, but it could be easily adapted for UDP (using QUdpSocket) or serial port (using QSerialPort) communications. ## Usage @@ -9,25 +9,32 @@ For the moment it only supports TCP/IP, but it should be easily adapted to UDP ( 2. Compile your client application using the library. You should add the roslib library to you include path, and add the .cpp files to your source files. Remarks: -* Unlike other clients of rosserial, the *NodeHandle* uses a dynamically sized buffer. -* There is no need to call spinOnce() manually : the *NodeHandle* will call spinOnce() periodically and on reception of new data +* C++11 support is required +* Unlike other client implementations of rosserial, the *NodeHandle* uses a dynamically sized buffer. +* There is no need to call spinOnce() manually : the *NodeHandle* will call spinOnce() periodically and on reception of new data, using the Qt event loopŝ * Unlike rosserial_arduino using C-style arrays, the message generation script will convert arrays to std::vector and std::array ## Examples Some simple examples of applications using the library are in the *test* directory +To use the examples you should first generate the *ros_lib* directory, for example like this : +``` +roscd rosserial_qt +rosrun rosserial_qt make_library.py . +``` ## Performance -CPU usage is quite close to native roscpp nodes : +CPU usage seems to be quite close to native roscpp nodes. +(TODO : add some data) ## Supported platforms -This library should work on all platforms supported by Qt (Linux, Windows, macos, ios, android ...) +This library should work on all platforms supported by Qt (Linux, Windows, Android, ios...) I have succesfully tested this library on : * Linux * Windows ## Server-side configuration -The current implementation needs the message length to be coded on 4 bytes (rosserial's default is 2 bytes). This has been implemented in rosserial-server, but has to be configured with the *msg_length_bytes* parameter. +The current implementation needs the message length to be coded on 4 bytes (rosserial's default is 2 bytes). This has been implemented in rosserial-server (modified version available here : [https://github.com/1r0b1n0/rosserial](https://github.com/1r0b1n0/rosserial) ), but has to be configured with the *msg_length_bytes* parameter. You should also configure the receive buffer size depending of your application. The rosserial-python server implementation will not work, as it doesn't support 4 bytes coding of the message length. diff --git a/launch/rosserial_python_server.launch b/launch/rosserial_python_server.launch index c5ba357..79ff830 100644 --- a/launch/rosserial_python_server.launch +++ b/launch/rosserial_python_server.launch @@ -1,4 +1,6 @@ + + diff --git a/test/.gitignore b/test/.gitignore index e2f7bf4..b4331f2 100644 --- a/test/.gitignore +++ b/test/.gitignore @@ -1,2 +1,3 @@ /ros_lib /build*/ +*pro.user* diff --git a/test/Basic/Basic.pro b/test/Basic/Basic.pro index 683b3ff..1414622 100644 --- a/test/Basic/Basic.pro +++ b/test/Basic/Basic.pro @@ -10,27 +10,26 @@ CONFIG -= app_bundle TEMPLATE = app -INCLUDEPATH += ../../src/ros_lib -INCLUDEPATH += ../ros_lib +INCLUDEPATH += ../../ros_lib SOURCES += main.cpp \ - ../../src/ros_lib/duration.cpp \ - ../../src/ros_lib/time.cpp \ - ../../src/ros_lib/RosQtSocket.cpp \ - ../../src/ros_lib/node_handle.cpp \ + ../../ros_lib/duration.cpp \ + ../../ros_lib/time.cpp \ + ../../ros_lib/RosQtSocket.cpp \ + ../../ros_lib/node_handle.cpp \ node.cpp DEFINES += QT_DEPRECATED_WARNINGS HEADERS += \ - ../../src/ros_lib/ros/duration.h \ - ../../src/ros_lib/ros/msg.h \ - ../../src/ros_lib/ros/node_handle.h \ - ../../src/ros_lib/ros/publisher.h \ - ../../src/ros_lib/ros/ros.h \ - ../../src/ros_lib/ros/service_client.h \ - ../../src/ros_lib/ros/service_server.h \ - ../../src/ros_lib/ros/subscriber.h \ - ../../src/ros_lib/ros/time.h \ - ../../src/ros_lib/ros/RosQtSocket.h \ + ../../ros_lib/ros/duration.h \ + ../../ros_lib/ros/msg.h \ + ../../ros_lib/ros/node_handle.h \ + ../../ros_lib/ros/publisher.h \ + ../../ros_lib/ros/ros.h \ + ../../ros_lib/ros/service_client.h \ + ../../ros_lib/ros/service_server.h \ + ../../ros_lib/ros/subscriber.h \ + ../../ros_lib/ros/time.h \ + ../../ros_lib/ros/RosQtSocket.h \ node.h diff --git a/test/Basic/Basic.pro.user b/test/Basic/Basic.pro.user index 6d71bbc..27040ff 100644 --- a/test/Basic/Basic.pro.user +++ b/test/Basic/Basic.pro.user @@ -1,10 +1,10 @@ - + EnvironmentId - {b4f30ced-7693-4a4b-b90f-54df2b2dcced} + {879e81b6-cba8-47ee-9e58-98b9c532cb3d} ProjectExplorer.Project.ActiveTarget @@ -61,19 +61,19 @@ Desktop Desktop - {95f7913d-7084-4d5d-9839-c0c5e82f5474} + {4dd4c15a-e3fc-4f28-ad62-08eacb8bfbf3} 0 0 0 - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-Basic-Desktop-Debug + /home/robin/rosserial/ws/src/rosserial_qt/test/build-Basic-Desktop-Debug true qmake QtProjectManager.QMakeBuildStep - true + false false false @@ -126,7 +126,7 @@ true - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-Basic-Desktop-Release + /home/robin/rosserial/ws/src/rosserial_qt/test/build-Basic-Desktop-Release true @@ -244,13 +244,13 @@ Basic - Qt4ProjectManager.Qt4RunConfiguration:/home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/Basic/Basic.pro + Qt4ProjectManager.Qt4RunConfiguration:/home/robin/rosserial/ws/src/rosserial_qt/test/Basic/Basic.pro true Basic.pro false - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-Basic-Desktop-Debug + /home/robin/rosserial/ws/src/rosserial_qt/test/build-Basic-Desktop-Debug 3768 false true diff --git a/test/Basic/Basic.pro_ b/test/Basic/Basic.pro_ new file mode 100644 index 0000000..683b3ff --- /dev/null +++ b/test/Basic/Basic.pro_ @@ -0,0 +1,36 @@ +QT += core +QT -= gui +QT += network + +CONFIG += c++11 + +TARGET = Basic +CONFIG += console +CONFIG -= app_bundle + +TEMPLATE = app + +INCLUDEPATH += ../../src/ros_lib +INCLUDEPATH += ../ros_lib + +SOURCES += main.cpp \ + ../../src/ros_lib/duration.cpp \ + ../../src/ros_lib/time.cpp \ + ../../src/ros_lib/RosQtSocket.cpp \ + ../../src/ros_lib/node_handle.cpp \ + node.cpp + +DEFINES += QT_DEPRECATED_WARNINGS + +HEADERS += \ + ../../src/ros_lib/ros/duration.h \ + ../../src/ros_lib/ros/msg.h \ + ../../src/ros_lib/ros/node_handle.h \ + ../../src/ros_lib/ros/publisher.h \ + ../../src/ros_lib/ros/ros.h \ + ../../src/ros_lib/ros/service_client.h \ + ../../src/ros_lib/ros/service_server.h \ + ../../src/ros_lib/ros/subscriber.h \ + ../../src/ros_lib/ros/time.h \ + ../../src/ros_lib/ros/RosQtSocket.h \ + node.h diff --git a/test/LoopbackTest/Basic.pro.user b/test/LoopbackTest/Basic.pro.user deleted file mode 100644 index c0dd247..0000000 --- a/test/LoopbackTest/Basic.pro.user +++ /dev/null @@ -1,276 +0,0 @@ - - - - - - EnvironmentId - {b4f30ced-7693-4a4b-b90f-54df2b2dcced} - - - ProjectExplorer.Project.ActiveTarget - 0 - - - ProjectExplorer.Project.EditorSettings - - true - false - true - - Cpp - - CppGlobal - - - - QmlJS - - QmlJSGlobal - - - 2 - UTF-8 - false - 4 - false - 80 - true - true - 1 - true - false - 0 - true - true - 0 - 8 - true - 1 - true - true - true - false - - - - ProjectExplorer.Project.PluginSettings - - - - ProjectExplorer.Project.Target.0 - - Desktop - Desktop - {95f7913d-7084-4d5d-9839-c0c5e82f5474} - 0 - 0 - 0 - - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-Basic-Desktop-Debug - - - true - qmake - - QtProjectManager.QMakeBuildStep - true - - false - false - false - - - true - Make - - Qt4ProjectManager.MakeStep - - -w - -r - - false - - - - 2 - Build - - ProjectExplorer.BuildSteps.Build - - - - true - Make - - Qt4ProjectManager.MakeStep - - -w - -r - - true - clean - - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Debug - - Qt4ProjectManager.Qt4BuildConfiguration - 2 - true - - - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-Basic-Desktop-Release - - - true - qmake - - QtProjectManager.QMakeBuildStep - false - - false - false - false - - - true - Make - - Qt4ProjectManager.MakeStep - - -w - -r - - false - - - - 2 - Build - - ProjectExplorer.BuildSteps.Build - - - - true - Make - - Qt4ProjectManager.MakeStep - - -w - -r - - true - clean - - - 1 - Clean - - ProjectExplorer.BuildSteps.Clean - - 2 - false - - Release - - Qt4ProjectManager.Qt4BuildConfiguration - 0 - true - - 2 - - - 0 - Deploy - - ProjectExplorer.BuildSteps.Deploy - - 1 - Deploy locally - - ProjectExplorer.DefaultDeployConfiguration - - 1 - - - false - false - 1000 - - true - - false - false - false - false - true - 0.01 - 10 - true - 1 - 25 - - 1 - true - false - true - valgrind - - 0 - 1 - 2 - 3 - 4 - 5 - 6 - 7 - 8 - 9 - 10 - 11 - 12 - 13 - 14 - - 2 - - Basic - - Qt4ProjectManager.Qt4RunConfiguration:/home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/Basic/Basic.pro - true - - Basic.pro - false - - - 3768 - false - true - false - false - true - - 1 - - - - ProjectExplorer.Project.TargetCount - 1 - - - ProjectExplorer.Project.Updater.FileVersion - 18 - - - Version - 18 - - diff --git a/test/LoopbackTest/LoopbackTest.pro b/test/LoopbackTest/LoopbackTest.pro index 990bfec..86486ea 100644 --- a/test/LoopbackTest/LoopbackTest.pro +++ b/test/LoopbackTest/LoopbackTest.pro @@ -10,27 +10,26 @@ CONFIG -= app_bundle TEMPLATE = app -INCLUDEPATH += ../../src/ros_lib -INCLUDEPATH += ../ros_lib +INCLUDEPATH += ../../ros_lib SOURCES += main.cpp \ - ../../src/ros_lib/duration.cpp \ - ../../src/ros_lib/time.cpp \ - ../../src/ros_lib/RosQtSocket.cpp \ - ../../src/ros_lib/node_handle.cpp \ + ../../ros_lib/duration.cpp \ + ../../ros_lib/time.cpp \ + ../../ros_lib/RosQtSocket.cpp \ + ../../ros_lib/node_handle.cpp \ node.cpp DEFINES += QT_DEPRECATED_WARNINGS HEADERS += \ - ../../src/ros_lib/ros/duration.h \ - ../../src/ros_lib/ros/msg.h \ - ../../src/ros_lib/ros/node_handle.h \ - ../../src/ros_lib/ros/publisher.h \ - ../../src/ros_lib/ros/ros.h \ - ../../src/ros_lib/ros/service_client.h \ - ../../src/ros_lib/ros/service_server.h \ - ../../src/ros_lib/ros/subscriber.h \ - ../../src/ros_lib/ros/time.h \ - ../../src/ros_lib/ros/RosQtSocket.h \ + ../../ros_lib/ros/duration.h \ + ../../ros_lib/ros/msg.h \ + ../../ros_lib/ros/node_handle.h \ + ../../ros_lib/ros/publisher.h \ + ../../ros_lib/ros/ros.h \ + ../../ros_lib/ros/service_client.h \ + ../../ros_lib/ros/service_server.h \ + ../../ros_lib/ros/subscriber.h \ + ../../ros_lib/ros/time.h \ + ../../ros_lib/ros/RosQtSocket.h \ node.h diff --git a/test/LoopbackTest/LoopbackTest.pro.user b/test/LoopbackTest/LoopbackTest.pro.user index 041c5b4..2f0b63e 100644 --- a/test/LoopbackTest/LoopbackTest.pro.user +++ b/test/LoopbackTest/LoopbackTest.pro.user @@ -1,10 +1,10 @@ - + EnvironmentId - {b4f30ced-7693-4a4b-b90f-54df2b2dcced} + {879e81b6-cba8-47ee-9e58-98b9c532cb3d} ProjectExplorer.Project.ActiveTarget @@ -61,12 +61,12 @@ Desktop Desktop - {95f7913d-7084-4d5d-9839-c0c5e82f5474} + {4dd4c15a-e3fc-4f28-ad62-08eacb8bfbf3} 0 0 0 - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-LoopbackTest-Desktop-Debug + /home/robin/rosserial/ws/src/rosserial_qt/test/build-LoopbackTest-Desktop-Debug true @@ -126,7 +126,7 @@ true - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-LoopbackTest-Desktop-Release + /home/robin/rosserial/ws/src/rosserial_qt/test/build-LoopbackTest-Desktop-Release true @@ -186,7 +186,7 @@ true - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-LoopbackTest-Desktop-Profile + /home/robin/rosserial/ws/src/rosserial_qt/test/build-LoopbackTest-Desktop-Profile true @@ -304,13 +304,13 @@ LoopbackTest - Qt4ProjectManager.Qt4RunConfiguration:/home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/LoopbackTest/LoopbackTest.pro + Qt4ProjectManager.Qt4RunConfiguration:/home/robin/rosserial/ws/src/rosserial_qt/test/LoopbackTest/LoopbackTest.pro true LoopbackTest.pro false - /home/robopec/Projects/Phenomobile/ws_phenofield/src/rosserial_qt/test/build-LoopbackTest-Desktop-Debug + /home/robin/rosserial/ws/src/rosserial_qt/test/build-LoopbackTest-Desktop-Debug 3768 false true diff --git a/test/LoopbackTest/LoopbackTest.pro_ b/test/LoopbackTest/LoopbackTest.pro_ new file mode 100644 index 0000000..990bfec --- /dev/null +++ b/test/LoopbackTest/LoopbackTest.pro_ @@ -0,0 +1,36 @@ +QT += core +QT -= gui +QT += network + +CONFIG += c++11 + +TARGET = LoopbackTest +CONFIG += console +CONFIG -= app_bundle + +TEMPLATE = app + +INCLUDEPATH += ../../src/ros_lib +INCLUDEPATH += ../ros_lib + +SOURCES += main.cpp \ + ../../src/ros_lib/duration.cpp \ + ../../src/ros_lib/time.cpp \ + ../../src/ros_lib/RosQtSocket.cpp \ + ../../src/ros_lib/node_handle.cpp \ + node.cpp + +DEFINES += QT_DEPRECATED_WARNINGS + +HEADERS += \ + ../../src/ros_lib/ros/duration.h \ + ../../src/ros_lib/ros/msg.h \ + ../../src/ros_lib/ros/node_handle.h \ + ../../src/ros_lib/ros/publisher.h \ + ../../src/ros_lib/ros/ros.h \ + ../../src/ros_lib/ros/service_client.h \ + ../../src/ros_lib/ros/service_server.h \ + ../../src/ros_lib/ros/subscriber.h \ + ../../src/ros_lib/ros/time.h \ + ../../src/ros_lib/ros/RosQtSocket.h \ + node.h From ad056995d14f1adbb354d55af64aa37ae0bfcfc4 Mon Sep 17 00:00:00 2001 From: Robin Vanhove <1r0b1n0@gmail.com> Date: Sun, 19 Nov 2017 23:37:54 +0100 Subject: [PATCH 2/8] type --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index fcc4348..cbfb0d7 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ For the moment it only supports TCP/IP, but it could be easily adapted for UDP ( Remarks: * C++11 support is required * Unlike other client implementations of rosserial, the *NodeHandle* uses a dynamically sized buffer. -* There is no need to call spinOnce() manually : the *NodeHandle* will call spinOnce() periodically and on reception of new data, using the Qt event loopŝ +* There is no need to call spinOnce() manually : the *NodeHandle* will call spinOnce() periodically and on reception of new data, using the Qt event loop * Unlike rosserial_arduino using C-style arrays, the message generation script will convert arrays to std::vector and std::array ## Examples From 71bb14a3a9ae9e2fb8942064ab5e360823caa7f3 Mon Sep 17 00:00:00 2001 From: Robin Vanhove <1r0b1n0@gmail.com> Date: Sun, 19 Nov 2017 23:49:58 +0100 Subject: [PATCH 3/8] update basic test --- test/Basic/node.cpp | 21 +++++++++++---------- test/Basic/node.h | 1 + 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/test/Basic/node.cpp b/test/Basic/node.cpp index e5044a8..bfb46a0 100644 --- a/test/Basic/node.cpp +++ b/test/Basic/node.cpp @@ -1,17 +1,9 @@ #include "node.h" #include -void estimated_pose_callback (const geometry_msgs::PoseWithCovarianceStamped & pose) -{ - printf ("Received pose %f, %f, %f\n", pose.pose.pose.position.x, - pose.pose.pose.position.y, pose.pose.pose.position.z); - - std::cout << "Received pose : " << QJsonDocument(pose.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl; -} - Node::Node(): chatter("chatter"), - poseSub("estimated_pose", &estimated_pose_callback), + poseSub("estimated_pose", std::bind(&Node::estimated_pose_callback, this, std::placeholders::_1)), serviceClient("/rosout/get_loggers"), serviceServer("add_two_ints", std::bind(&Node::addTwoInts, this, std::placeholders::_1, std::placeholders::_2)) { @@ -29,10 +21,19 @@ Node::Node(): nh.serviceClient(serviceClient); - // not supported by rosserial_server + // not yet supported by rosserial_server //nh.advertiseService(serviceServer); } +void Node::estimated_pose_callback (const geometry_msgs::PoseWithCovarianceStamped & pose) +{ + printf ("Received pose %f, %f, %f\n", pose.pose.pose.position.x, + pose.pose.pose.position.y, pose.pose.pose.position.z); + + std::cout << "Received pose : " << QJsonDocument(pose.serializeAsJson()).toJson(QJsonDocument::Indented).toStdString() << std::endl; +} + + void Node::onTimer() { static int i=1; diff --git a/test/Basic/node.h b/test/Basic/node.h index cfa9aab..1b69d34 100644 --- a/test/Basic/node.h +++ b/test/Basic/node.h @@ -21,6 +21,7 @@ public slots: private: void addTwoInts(const roscpp_tutorials::TwoIntsRequest &req, roscpp_tutorials::TwoIntsResponse &res); + void estimated_pose_callback (const geometry_msgs::PoseWithCovarianceStamped & pose); ros::NodeHandle nh; QTimer *m_timer; From fdc5efdf6942248553fae0c7809834c7145fc8c9 Mon Sep 17 00:00:00 2001 From: Romain Reignier Date: Thu, 4 Jan 2018 09:39:40 +0100 Subject: [PATCH 4/8] Add a latency property to get the latency of the connection --- src/ros_lib/node_handle.cpp | 20 +++++++++++++++++++- src/ros_lib/ros/node_handle.h | 5 +++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/ros_lib/node_handle.cpp b/src/ros_lib/node_handle.cpp index a7299ae..d987aab 100644 --- a/src/ros_lib/node_handle.cpp +++ b/src/ros_lib/node_handle.cpp @@ -46,7 +46,8 @@ ros::NodeHandle::NodeHandle(uint32_t input_size, uint32_t output_size, QObject * INPUT_SIZE(input_size), OUTPUT_SIZE(output_size), configured_(false), - isActive_(false) + isActive_(false), + latency_(NAN) { message_in.resize(INPUT_SIZE); message_out.resize(OUTPUT_SIZE); @@ -233,6 +234,8 @@ void ros::NodeHandle::requestSyncTime() void ros::NodeHandle::syncTime(uint8_t *data) { + uint32_t latency_ms = hardware_.time() - rt_time; + std_msgs::Time t; uint32_t offset = hardware_.time() - rt_time; @@ -242,6 +245,10 @@ void ros::NodeHandle::syncTime(uint8_t *data) this->setNow(t.data); last_sync_receive_time = hardware_.time(); + + // update latency property + latency_ = latency_ms/1000.; + emit latencyChanged(latency_); } ros::Time ros::NodeHandle::now() @@ -427,6 +434,11 @@ bool ros::NodeHandle::getParam(const std::string &name, std::string *param, unsi return false; } +float NodeHandle::latency() const +{ + return latency_; +} + void NodeHandle::setConfigured(bool isConnected) { if (configured_ == isConnected) @@ -434,4 +446,10 @@ void NodeHandle::setConfigured(bool isConnected) configured_ = isConnected; emit isConnectedChanged(configured_); + + if(!isConnected) + { + latency_ = NAN; + emit latencyChanged(latency_); + } } diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h index 80e9f3c..ba2fbeb 100644 --- a/src/ros_lib/ros/node_handle.h +++ b/src/ros_lib/ros/node_handle.h @@ -47,6 +47,7 @@ namespace ros { class NodeHandleBase_ : public QObject{ + Q_OBJECT public: NodeHandleBase_(QObject *parent=0) : QObject(parent){} @@ -72,6 +73,7 @@ class NodeHandle : public NodeHandleBase_ Q_OBJECT Q_PROPERTY(bool isConnected READ connected NOTIFY isConnectedChanged) + Q_PROPERTY(float latency READ latency NOTIFY latencyChanged) protected: RosQtSocket hardware_; @@ -115,6 +117,7 @@ class NodeHandle : public NodeHandleBase_ bool configured_; bool isActive_; // true if we should try to connect + float latency_; /* used for syncing the time */ uint32_t last_sync_time; @@ -228,9 +231,11 @@ private slots: bool getParam(const std::string &name, float* param, unsigned int length=1, int timeout = 1000); bool getParam(const std::string &name, std::string *param, unsigned int length=1, int timeout = 1000); bool isConnected() const; + float latency() const; signals: void isConnectedChanged(bool isConnected); + void latencyChanged(float latency); }; } From ea5966c9b2c1cec12994a462ef22b2518582b8bd Mon Sep 17 00:00:00 2001 From: Romain Reignier Date: Thu, 4 Jan 2018 09:40:05 +0100 Subject: [PATCH 5/8] Add install statements in CMakeLists.txt --- CMakeLists.txt | 8 ++++++++ src/ros_lib/ros/RosQtSocket.h | 6 +++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1157b5e..7c2fddd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,4 +7,12 @@ find_package(catkin REQUIRED COMPONENTS roscpp rosserial_msgs rosserial_python r catkin_package() +install( + DIRECTORY src/ros_lib + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src +) +catkin_install_python( + PROGRAMS scripts/make_library.py + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/src/ros_lib/ros/RosQtSocket.h b/src/ros_lib/ros/RosQtSocket.h index f2b405d..4dfb26e 100644 --- a/src/ros_lib/ros/RosQtSocket.h +++ b/src/ros_lib/ros/RosQtSocket.h @@ -1,9 +1,9 @@ /** Software License Agreement (BSD) -\file WindowsSocket.h -\authors Kareem Shehata -\copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. +\file RosQtSocket.h +\authors Robin Vanhove +\copyright Copyright (c) 2017, Robopec, All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: From ab72c3612fb68a14a008e2f6c0017361cf0146b5 Mon Sep 17 00:00:00 2001 From: Romain Reignier Date: Thu, 4 Jan 2018 09:45:03 +0100 Subject: [PATCH 6/8] Fix Code Style --- src/ros_lib/node_handle.cpp | 2 +- src/ros_lib/ros/node_handle.h | 30 +++++++++++++++--------------- src/ros_lib/ros/publisher.h | 4 ++-- src/ros_lib/ros/service_client.h | 8 ++++---- src/ros_lib/ros/service_server.h | 4 ++-- 5 files changed, 24 insertions(+), 24 deletions(-) diff --git a/src/ros_lib/node_handle.cpp b/src/ros_lib/node_handle.cpp index d987aab..aabf486 100644 --- a/src/ros_lib/node_handle.cpp +++ b/src/ros_lib/node_handle.cpp @@ -2,7 +2,7 @@ #ifdef ERROR // ach, windows.h polluting everything again, - // clashes with autogenerated rosgraph_msgs/Log.h + // clashes with autogenerated rosgraph_msgs/Log.h #undef ERROR #endif #include "rosserial_msgs/Log.h" diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h index ba2fbeb..11bfd87 100644 --- a/src/ros_lib/ros/node_handle.h +++ b/src/ros_lib/ros/node_handle.h @@ -94,12 +94,12 @@ class NodeHandle : public NodeHandleBase_ uint32_t OUTPUT_SIZE; /* - * Setup Functions - */ + * Setup Functions + */ public: NodeHandle(uint32_t input_size=10000, uint32_t output_size=10000, QObject *parent=0); - void timerEvent(QTimerEvent *event); + void timerEvent(QTimerEvent *event) override; /* Start a named port, which may be network server IP, initialize buffers */ void open(const std::string &hostName, uint16_t port = 11411); @@ -126,19 +126,19 @@ class NodeHandle : public NodeHandleBase_ public: /* This function goes in your loop() function, it handles - * serial input and callbacks for subscribers. - */ + * serial input and callbacks for subscribers. + */ - virtual int spinOnce(); + virtual int spinOnce() override; /* Are we connected to the PC? */ bool connected() const override; /******************************************************************** - * Time functions - */ + * Time functions + */ void requestSyncTime(); @@ -149,8 +149,8 @@ class NodeHandle : public NodeHandleBase_ void setNow( Time & new_now ); /******************************************************************** - * Topic Management - */ + * Topic Management + */ /* Register a new publisher */ template @@ -194,7 +194,7 @@ class NodeHandle : public NodeHandleBase_ void negotiateTopics(); - virtual int publish(int id, const Msg * msg); + virtual int publish(int id, const Msg * msg) override; public slots: void setConfigured(bool isConnected); @@ -203,8 +203,8 @@ private slots: void onReadyRead(); /******************************************************************** - * Logging - */ + * Logging + */ private: void log(char byte, const char * msg); @@ -217,8 +217,8 @@ private slots: void logfatal(const char*msg); /******************************************************************** - * Parameters - */ + * Parameters + */ private: bool param_recieved; diff --git a/src/ros_lib/ros/publisher.h b/src/ros_lib/ros/publisher.h index fe37710..d408ab3 100644 --- a/src/ros_lib/ros/publisher.h +++ b/src/ros_lib/ros/publisher.h @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef _ROS_PUBLISHER_H_ -#define _ROS_PUBLISHER_H_ +#ifndef ROS_PUBLISHER_H_ +#define ROS_PUBLISHER_H_ #include #include "rosserial_msgs/TopicInfo.h" diff --git a/src/ros_lib/ros/service_client.h b/src/ros_lib/ros/service_client.h index f70f3cf..2b578fd 100644 --- a/src/ros_lib/ros/service_client.h +++ b/src/ros_lib/ros/service_client.h @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef _ROS_SERVICE_CLIENT_H_ -#define _ROS_SERVICE_CLIENT_H_ +#ifndef ROS_SERVICE_CLIENT_H_ +#define ROS_SERVICE_CLIENT_H_ #include #include "rosserial_msgs/TopicInfo.h" @@ -70,7 +70,7 @@ namespace ros { const std::string & getMsgMD5() override { return MRes::getMD5(); } int getEndpointType() override{ return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER; } - private: + private: // these refer to the subscriber void callback(unsigned char *data) override{ MRes ret; @@ -86,7 +86,7 @@ namespace ros { bool waiting; CallbackT cb_; - public: + public: Publisher pub; }; diff --git a/src/ros_lib/ros/service_server.h b/src/ros_lib/ros/service_server.h index 3e9ee40..e60c0d5 100644 --- a/src/ros_lib/ros/service_server.h +++ b/src/ros_lib/ros/service_server.h @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef _ROS_SERVICE_SERVER_H_ -#define _ROS_SERVICE_SERVER_H_ +#ifndef ROS_SERVICE_SERVER_H_ +#define ROS_SERVICE_SERVER_H_ #include "rosserial_msgs/TopicInfo.h" From a898fcd42e6374ab03444a3f42e1d368790ff524 Mon Sep 17 00:00:00 2001 From: Romain Reignier Date: Thu, 4 Jan 2018 09:45:19 +0100 Subject: [PATCH 7/8] Add a basic action client --- src/ros_lib/ros/action_client.h | 190 ++++++++++++++++++++++++++++++++ src/ros_lib/ros/node_handle.h | 12 ++ 2 files changed, 202 insertions(+) create mode 100644 src/ros_lib/ros/action_client.h diff --git a/src/ros_lib/ros/action_client.h b/src/ros_lib/ros/action_client.h new file mode 100644 index 0000000..c331776 --- /dev/null +++ b/src/ros_lib/ros/action_client.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Robopec + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote prducts derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROS_ACTION_CLIENT_H_ +#define ROS_ACTION_CLIENT_H_ + +#include +#include +#include "rosserial_msgs/TopicInfo.h" + +#include "ros/publisher.h" +#include "ros/subscriber.h" +#include "../actionlib_msgs/GoalID.h" +#include "../actionlib_msgs/GoalStatusArray.h" + +namespace ros { + +#define ACTION_DEFINITION(ActionSpec) \ + typedef typename ActionSpec::_action_goal_type ActionGoal; \ + typedef typename ActionGoal::_goal_type Goal; \ + typedef typename ActionSpec::_action_result_type ActionResult; \ + typedef typename ActionResult::_result_type Result; \ + typedef typename ActionSpec::_action_feedback_type ActionFeedback; \ + typedef typename ActionFeedback::_feedback_type Feedback; + + template + class ActionClient { + ACTION_DEFINITION(ActionType) + public: + typedef std::function FeedbackCallbackT; + typedef std::function ResultCallbackT; + typedef std::function StateChangedCallbackT; + + enum StateEnum + { + PENDING, + ACTIVE, + PREEMPTED, + SUCCEEDED, + ABORTED, + REJECTED, + PREEMPTING, + RECALLING, + RECALLED, + LOST, + INACTIVE + }; + + ActionClient(const std::string& action_name) : + goal_pub(action_name + "/goal"), + cancel_pub(action_name + "/cancel"), + status_sub(action_name + "/status", std::bind(&ActionClient::status_cb_internal, this, std::placeholders::_1)), + feedback_sub(action_name + "/feedback", std::bind(&ActionClient::feedback_cb_internal, this, std::placeholders::_1)), + result_sub(action_name + "/result", std::bind(&ActionClient::result_cb_internal, this, std::placeholders::_1)) + { + resetState(); + } + + void sendGoal(const Goal& goal, FeedbackCallbackT feedback_cb = FeedbackCallbackT(), ResultCallbackT result_cb = ResultCallbackT()) + { + if(!goal_pub.nh_->connected()) return; + + feedback_cb_ = feedback_cb; + result_cb_ = result_cb; + + ActionGoal action_goal_msg; + // no goal id and stamp -> will be generated by the action server + // see: http://wiki.ros.org/actionlib/DetailedDescription + action_goal_msg.goal = goal; + goal_pub.publish(action_goal_msg); + } + + void cancelGoal() + { + actionlib_msgs::GoalID msg; + // no goal id and stamp -> cancel all goals + // see: http://wiki.ros.org/actionlib/DetailedDescription + cancel_pub.publish(msg); + } + + void registerStateCallback(StateChangedCallbackT state_cb) + { + state_cb_ = state_cb; + resetState(); + } + + void unregisterStateCallback() + { + state_cb_ = StateChangedCallbackT(); + } + + uint8_t getState() const + { + return status_.status; + } + + std::string getStateText() const + { + return status_.text; + } + + private: + void status_cb_internal(const actionlib_msgs::GoalStatusArray& msg) + { + if(!msg.status_list.empty()) + { + if(state_cb_ && (msg.status_list.back().status != status_.status)) + { + state_cb_(msg.status_list.back().status); + } + + status_ = msg.status_list.back(); + } + else + { + resetState(); + } + } + + void feedback_cb_internal(const ActionFeedback& msg) + { + if(feedback_cb_) + { + feedback_cb_(msg.feedback); + } + } + + void result_cb_internal(const ActionResult& msg) + { + if(result_cb_) + { + result_cb_(msg.result); + } + } + + void resetState() + { + status_.goal_id = actionlib_msgs::GoalID(); + status_.text = "inactive"; + status_.status = INACTIVE; + } + + FeedbackCallbackT feedback_cb_; + ResultCallbackT result_cb_; + StateChangedCallbackT state_cb_; + size_t goal_count_= 0; + actionlib_msgs::GoalStatus status_; + + public: + Publisher goal_pub; + Publisher cancel_pub; + Subscriber status_sub; + Subscriber feedback_sub; + Subscriber result_sub; + }; + +} + +#endif // ROS_ACTION_CLIENT_H_ diff --git a/src/ros_lib/ros/node_handle.h b/src/ros_lib/ros/node_handle.h index 11bfd87..8c87087 100644 --- a/src/ros_lib/ros/node_handle.h +++ b/src/ros_lib/ros/node_handle.h @@ -62,6 +62,7 @@ class NodeHandleBase_ : public QObject{ #include "ros/subscriber.h" #include "ros/service_server.h" #include "ros/service_client.h" +#include "ros/action_client.h" namespace ros { @@ -192,6 +193,17 @@ class NodeHandle : public NodeHandleBase_ return v; } + /* Register a new Action Client */ + template + bool actionClient(ActionClient& ac){ + bool v = advertise(ac.goal_pub); + v |= advertise(ac.cancel_pub); + v |= subscribe(ac.status_sub); + v |= subscribe(ac.feedback_sub); + v |= subscribe(ac.result_sub); + return v; + } + void negotiateTopics(); virtual int publish(int id, const Msg * msg) override; From 052525e0d86d9db7f5894055fcb8069d8f48bfcc Mon Sep 17 00:00:00 2001 From: Romain Reignier Date: Thu, 4 Jan 2018 09:45:49 +0100 Subject: [PATCH 8/8] Modify service client to retrieve if the service call has succeeded --- src/ros_lib/ros/service_client.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/ros_lib/ros/service_client.h b/src/ros_lib/ros/service_client.h index 2b578fd..b6169ee 100644 --- a/src/ros_lib/ros/service_client.h +++ b/src/ros_lib/ros/service_client.h @@ -49,7 +49,7 @@ namespace ros { public: typedef typename MType::Request MReq; typedef typename MType::Response MRes; - typedef std::function CallbackT; + typedef std::function CallbackT; ServiceClient(const char* topic_name) : pub(topic_name, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER) @@ -74,11 +74,18 @@ namespace ros { // these refer to the subscriber void callback(unsigned char *data) override{ MRes ret; - ret.deserialize(data); + // First byte give us the success status + bool success = data[0]; + + if(success) + { + // Access from the second byte + ret.deserialize(data+1); + } waiting = false; if(cb_) { - cb_(ret); + cb_(ret, success); } cb_ = nullptr; }