From a1edefc884bb72cff2f69cb8ed8a045e8f0e6daf Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 17 Apr 2024 00:50:00 +0900 Subject: [PATCH 01/10] Fix handling of multiple responses in one read --- include/scip2/protocol.h | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index e975ee71..d5c38c41 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -41,25 +41,26 @@ class Protocol const boost::posix_time::ptime& time_read) { std::istream stream(&buf); - std::string echo_back; - if (!std::getline(stream, echo_back)) + while (true) { - logger::error() << "Failed to get echo back" << std::endl; - return; - } - std::string status; - if (!std::getline(stream, status)) - { - logger::error() << "Failed to get status" << std::endl; - return; - } - status.pop_back(); // remove checksum + std::string echo_back; + if (!std::getline(stream, echo_back)) + { + return; + } + if (echo_back == "") + { + continue; + } + std::string status; + if (!std::getline(stream, status)) + { + logger::error() << "Failed to get status" << std::endl; + return; + } + status.pop_back(); // remove checksum - response_processor_(time_read, echo_back, status, stream); - - std::string line; - while (std::getline(stream, line)) - { + response_processor_(time_read, echo_back, status, stream); } } From 750cbf925031c10548ab77fb7795dd38fe346966 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 19 Apr 2024 13:13:23 +0900 Subject: [PATCH 02/10] Fix unknown command handling --- include/scip2/connection.h | 1 + include/scip2/response.h | 4 ++ test/CMakeLists.txt | 6 +++ test/src/test_scip2.cpp | 91 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 102 insertions(+) create mode 100644 test/src/test_scip2.cpp diff --git a/include/scip2/connection.h b/include/scip2/connection.h index 709b2a1a..b35a8b6e 100644 --- a/include/scip2/connection.h +++ b/include/scip2/connection.h @@ -22,6 +22,7 @@ #include #include +#include #include diff --git a/include/scip2/response.h b/include/scip2/response.h index 3900550f..8a159f5f 100644 --- a/include/scip2/response.h +++ b/include/scip2/response.h @@ -67,6 +67,10 @@ class ResponseProcessor if (response == responses_.end()) { logger::debug() << "Unknown response " << command_code << std::endl; + std::string line; + while (std::getline(stream, line) && line.size() > 0) + { + } return; } (*(response->second))(time_read, echo_back, status, stream); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f0b40695..d4c9536a 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -13,6 +13,12 @@ target_link_libraries(test_timestamp_moving_average ${catkin_LIBRARIES} ${Boost_ catkin_add_gtest(test_timestamp_outlier_remover src/test_timestamp_outlier_remover.cpp) target_link_libraries(test_timestamp_outlier_remover ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +catkin_add_gtest(test_scip2 + src/test_scip2.cpp + ../src/scip2/logger.cpp +) +target_link_libraries(test_scip2 ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + catkin_add_gtest(test_walltime src/test_walltime.cpp ../src/scip2/logger.cpp) target_link_libraries(test_walltime ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/test/src/test_scip2.cpp b/test/src/test_scip2.cpp new file mode 100644 index 00000000..f466840e --- /dev/null +++ b/test/src/test_scip2.cpp @@ -0,0 +1,91 @@ +/* + * Copyright 2024 The urg_stamped Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +class ConnectionDummy : public scip2::Connection +{ +public: + void spin() final + { + } + void stop() final + { + } + void send(const std::string&, CallbackSend = CallbackSend()) final + { + } + void startWatchdog(const boost::posix_time::time_duration&) final + { + } + void feed(const std::string& data, const boost::posix_time::ptime& time_read) + { + boost::asio::streambuf buf; + std::ostream os(&buf); + os << data; + receive(buf, time_read); + } +}; + +TEST(SCIP2, MultipleResponses) +{ + std::shared_ptr dev(new ConnectionDummy()); + scip2::Protocol p(dev); + + const auto now = boost::posix_time::microsec_clock::universal_time(); + int num_receive = 0; + + const auto cb = + [&num_receive, now]( + const boost::posix_time::ptime& time_read, + const std::string& echo_back, + const std::string& status) + { + num_receive++; + EXPECT_EQ(now, time_read); + }; + p.registerCallback(cb); + + /* + Input data: + + QT // First QT command + + FOO // Unknown command + BAR + QT // This line must be ignored as a part of unknown command + + QT // Second QT command + + */ + dev->feed("QT\n\nFOO\nBAR\nQT\n\nQT\n\n", now); + ASSERT_EQ(2, num_receive); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} From d8e7b8aee7e1cd4f1cfd24e4e3f9999f2dfe6c2f Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 19 Apr 2024 16:09:09 +0900 Subject: [PATCH 03/10] Ignore when status line is empty --- include/scip2/protocol.h | 5 +++++ test/src/test_scip2.cpp | 13 +++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index d5c38c41..5e9ba6f3 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -58,6 +58,11 @@ class Protocol logger::error() << "Failed to get status" << std::endl; return; } + if (status == "") + { + logger::debug() << "Ignoring response without status" << std::endl; + continue; + } status.pop_back(); // remove checksum response_processor_(time_read, echo_back, status, stream); diff --git a/test/src/test_scip2.cpp b/test/src/test_scip2.cpp index f466840e..792c8cbe 100644 --- a/test/src/test_scip2.cpp +++ b/test/src/test_scip2.cpp @@ -64,22 +64,27 @@ TEST(SCIP2, MultipleResponses) { num_receive++; EXPECT_EQ(now, time_read); + EXPECT_EQ("00", status); }; p.registerCallback(cb); /* Input data: - QT // First QT command + QT // First QT response + 00P - FOO // Unknown command + FOO // Unknown response BAR QT // This line must be ignored as a part of unknown command - QT // Second QT command + QT // This must be ignored as it doesn't have correct status line + + QT // Second QT response + 00P */ - dev->feed("QT\n\nFOO\nBAR\nQT\n\nQT\n\n", now); + dev->feed("QT\n00P\n\nFOO\nBAR\nQT\n\nQT\n\nQT\n00P\n\n", now); ASSERT_EQ(2, num_receive); } From 2808e8376955449734a84abd7b82ba9340ff01ce Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 19 Apr 2024 16:52:33 +0900 Subject: [PATCH 04/10] Fix response processing on non-aligned read --- include/scip2/protocol.h | 47 +++++++++++++++--------------- include/scip2/response/abstract.h | 8 +++++ include/scip2/response/quit.h | 3 ++ include/scip2/response/reboot.h | 3 ++ include/scip2/response/reset.h | 6 ++++ include/scip2/response/stream.h | 7 +++++ include/scip2/response/time_sync.h | 9 ++++++ test/src/test_scip2.cpp | 29 ++++++++++++++---- 8 files changed, 84 insertions(+), 28 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index 5e9ba6f3..d7a04ac7 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -41,32 +41,33 @@ class Protocol const boost::posix_time::ptime& time_read) { std::istream stream(&buf); - while (true) + const auto pos = stream.tellg(); + + std::string echo_back; + if (!std::getline(stream, echo_back)) + { + return; + } + if (echo_back == "") { - std::string echo_back; - if (!std::getline(stream, echo_back)) - { - return; - } - if (echo_back == "") - { - continue; - } - std::string status; - if (!std::getline(stream, status)) - { - logger::error() << "Failed to get status" << std::endl; - return; - } - if (status == "") - { - logger::debug() << "Ignoring response without status" << std::endl; - continue; - } - status.pop_back(); // remove checksum + logger::debug() << "Empty response echo back" << std::endl; + return; + } - response_processor_(time_read, echo_back, status, stream); + std::string status; + if (!std::getline(stream, status)) + { + stream.seekg(pos); + return; + } + if (status == "") + { + logger::debug() << "Empty response status" << std::endl; + return; } + status.pop_back(); // remove checksum + + response_processor_(time_read, echo_back, status, stream); } public: diff --git a/include/scip2/response/abstract.h b/include/scip2/response/abstract.h index b5b3777e..3594dddf 100644 --- a/include/scip2/response/abstract.h +++ b/include/scip2/response/abstract.h @@ -36,6 +36,14 @@ class Response std::istream&) = 0; }; +inline void readUntilEnd(std::istream& stream) +{ + std::string line; + while (std::getline(stream, line) && line.size() > 0) + { + } +} + } // namespace scip2 #endif // SCIP2_RESPONSE_ABSTRACT_H diff --git a/include/scip2/response/quit.h b/include/scip2/response/quit.h index 6e4460f1..5118ecce 100644 --- a/include/scip2/response/quit.h +++ b/include/scip2/response/quit.h @@ -48,7 +48,10 @@ class ResponseQT : public Response std::istream& stream) { if (cb_) + { cb_(time_read, echo_back, status); + } + readUntilEnd(stream); } void registerCallback(Callback cb) { diff --git a/include/scip2/response/reboot.h b/include/scip2/response/reboot.h index fc588070..4196d375 100644 --- a/include/scip2/response/reboot.h +++ b/include/scip2/response/reboot.h @@ -48,7 +48,10 @@ class ResponseRB : public Response std::istream& stream) { if (cb_) + { cb_(time_read, echo_back, status); + } + readUntilEnd(stream); } void registerCallback(Callback cb) { diff --git a/include/scip2/response/reset.h b/include/scip2/response/reset.h index de59d511..e57d8db7 100644 --- a/include/scip2/response/reset.h +++ b/include/scip2/response/reset.h @@ -48,7 +48,10 @@ class ResponseRS : public Response std::istream& stream) { if (cb_) + { cb_(time_read, echo_back, status); + } + readUntilEnd(stream); } void registerCallback(Callback cb) { @@ -79,7 +82,10 @@ class ResponseRT : public Response std::istream& stream) { if (cb_) + { cb_(time_read, echo_back, status); + } + readUntilEnd(stream); } void registerCallback(Callback cb) { diff --git a/include/scip2/response/stream.h b/include/scip2/response/stream.h index cdb68f97..62612ce1 100644 --- a/include/scip2/response/stream.h +++ b/include/scip2/response/stream.h @@ -115,7 +115,10 @@ class ResponseMD : public ResponseStream if (!readTimestamp(time_read, echo_back, status, stream, scan)) { if (cb_) + { cb_(time_read, echo_back, status, scan); + } + readUntilEnd(stream); return; } scan.ranges_.reserve(512); @@ -149,7 +152,9 @@ class ResponseMD : public ResponseStream } } if (cb_) + { cb_(time_read, echo_back, status, scan); + } } }; @@ -206,7 +211,9 @@ class ResponseME : public ResponseStream } } if (cb_) + { cb_(time_read, echo_back, status, scan); + } } }; diff --git a/include/scip2/response/time_sync.h b/include/scip2/response/time_sync.h index cd3f7654..7535fb0d 100644 --- a/include/scip2/response/time_sync.h +++ b/include/scip2/response/time_sync.h @@ -65,7 +65,10 @@ class ResponseTM : public Response if (status != "00") { if (cb_) + { cb_(time_read, echo_back, status, timestamp); + } + readUntilEnd(stream); return; } if (echo_back[2] == '1') @@ -74,6 +77,7 @@ class ResponseTM : public Response if (!std::getline(stream, stamp)) { logger::error() << "Failed to get timestamp" << std::endl; + readUntilEnd(stream); return; } const uint8_t checksum = stamp.back(); @@ -81,6 +85,7 @@ class ResponseTM : public Response if (stamp.size() < 4) { logger::error() << "Wrong timestamp format" << std::endl; + readUntilEnd(stream); return; } @@ -90,11 +95,15 @@ class ResponseTM : public Response if ((dec.getChecksum() & 0x3F) + 0x30 != checksum) { logger::error() << "Checksum mismatch" << std::endl; + readUntilEnd(stream); return; } } if (cb_) + { cb_(time_read, echo_back, status, timestamp); + } + readUntilEnd(stream); } void registerCallback(Callback cb) { diff --git a/test/src/test_scip2.cpp b/test/src/test_scip2.cpp index 792c8cbe..49e0c2b2 100644 --- a/test/src/test_scip2.cpp +++ b/test/src/test_scip2.cpp @@ -39,11 +39,8 @@ class ConnectionDummy : public scip2::Connection void startWatchdog(const boost::posix_time::time_duration&) final { } - void feed(const std::string& data, const boost::posix_time::ptime& time_read) + void feed(boost::asio::streambuf& buf, const boost::posix_time::ptime& time_read) { - boost::asio::streambuf buf; - std::ostream os(&buf); - os << data; receive(buf, time_read); } }; @@ -84,7 +81,29 @@ TEST(SCIP2, MultipleResponses) 00P */ - dev->feed("QT\n00P\n\nFOO\nBAR\nQT\n\nQT\n\nQT\n00P\n\n", now); + boost::asio::streambuf buf; + std::ostream os(&buf); + os << "QT\n00P\n\nF"; + dev->feed(buf, now); + ASSERT_EQ(1, num_receive); + + os << "OO\nBAR\nQT\n\n"; + dev->feed(buf, now); + ASSERT_EQ(1, num_receive); + + os << "QT\n\nQT\n"; + dev->feed(buf, now); + ASSERT_EQ(1, num_receive); + + os << "00P\n\n"; + dev->feed(buf, now); + ASSERT_EQ(2, num_receive); + + os << "\n\n"; + dev->feed(buf, now); + ASSERT_EQ(2, num_receive); + + dev->feed(buf, now); ASSERT_EQ(2, num_receive); } From 30e43e932c79b6c87c91e29b2970bb1b4b33e064 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 19 Apr 2024 16:54:51 +0900 Subject: [PATCH 05/10] Add comment --- include/scip2/protocol.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index d7a04ac7..ff642f58 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -57,6 +57,7 @@ class Protocol std::string status; if (!std::getline(stream, status)) { + // Re-read from beginning on the next callback stream.seekg(pos); return; } From a4423e2a83b59a80ef7b30591966573e8f8abd55 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Fri, 19 Apr 2024 20:02:41 +0900 Subject: [PATCH 06/10] Fix multiple responses in one read --- include/scip2/protocol.h | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index ff642f58..b6cc23cd 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -43,24 +43,26 @@ class Protocol std::istream stream(&buf); const auto pos = stream.tellg(); - std::string echo_back; - if (!std::getline(stream, echo_back)) + if (stream.eof()) { return; } + std::string echo_back; + std::getline(stream, echo_back); if (echo_back == "") { logger::debug() << "Empty response echo back" << std::endl; return; } - std::string status; - if (!std::getline(stream, status)) + if (stream.eof()) { - // Re-read from beginning on the next callback + // Re-read from the beginning on the next callback stream.seekg(pos); return; } + std::string status; + std::getline(stream, status); if (status == "") { logger::debug() << "Empty response status" << std::endl; From f2a4e5d3f3c2b0f30da2db764c028eba65c30c61 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 24 Apr 2024 09:13:55 +0900 Subject: [PATCH 07/10] Unuse eof which is not available --- include/scip2/protocol.h | 18 ++++++------------ test/src/test_scip2.cpp | 3 --- 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index b6cc23cd..e2b17391 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -28,6 +28,11 @@ #include #include +namespace +{ + +} // namespace + namespace scip2 { class Protocol @@ -41,26 +46,15 @@ class Protocol const boost::posix_time::ptime& time_read) { std::istream stream(&buf); - const auto pos = stream.tellg(); - if (stream.eof()) - { - return; - } std::string echo_back; std::getline(stream, echo_back); if (echo_back == "") { - logger::debug() << "Empty response echo back" << std::endl; + logger::debug() << "Empty response echo back " << std::endl; return; } - if (stream.eof()) - { - // Re-read from the beginning on the next callback - stream.seekg(pos); - return; - } std::string status; std::getline(stream, status); if (status == "") diff --git a/test/src/test_scip2.cpp b/test/src/test_scip2.cpp index 49e0c2b2..8c4c1c05 100644 --- a/test/src/test_scip2.cpp +++ b/test/src/test_scip2.cpp @@ -102,9 +102,6 @@ TEST(SCIP2, MultipleResponses) os << "\n\n"; dev->feed(buf, now); ASSERT_EQ(2, num_receive); - - dev->feed(buf, now); - ASSERT_EQ(2, num_receive); } int main(int argc, char** argv) From 658cc5791a0306280e8615f4c6c04412a1c16042 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 24 Apr 2024 09:17:04 +0900 Subject: [PATCH 08/10] Remove unused block --- include/scip2/protocol.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index e2b17391..23f5fcc2 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -28,11 +28,6 @@ #include #include -namespace -{ - -} // namespace - namespace scip2 { class Protocol From 4f076676ec9355379848f482fbb8999e88da3245 Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 24 Apr 2024 09:22:00 +0900 Subject: [PATCH 09/10] Remove duplicated code --- include/scip2/response.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/include/scip2/response.h b/include/scip2/response.h index 8a159f5f..a54b6c24 100644 --- a/include/scip2/response.h +++ b/include/scip2/response.h @@ -67,10 +67,7 @@ class ResponseProcessor if (response == responses_.end()) { logger::debug() << "Unknown response " << command_code << std::endl; - std::string line; - while (std::getline(stream, line) && line.size() > 0) - { - } + readUntilEnd(stream); return; } (*(response->second))(time_read, echo_back, status, stream); From d655009cc1ad6ab15f5f85b371742bdf4005703a Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Wed, 24 Apr 2024 09:28:26 +0900 Subject: [PATCH 10/10] Handle getline error as well --- include/scip2/protocol.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/include/scip2/protocol.h b/include/scip2/protocol.h index 23f5fcc2..f0d21d70 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -43,7 +43,11 @@ class Protocol std::istream stream(&buf); std::string echo_back; - std::getline(stream, echo_back); + if (!std::getline(stream, echo_back)) + { + logger::error() << "Failed to get echo back" << std::endl; + return; + } if (echo_back == "") { logger::debug() << "Empty response echo back " << std::endl; @@ -51,7 +55,11 @@ class Protocol } std::string status; - std::getline(stream, status); + if (!std::getline(stream, status)) + { + logger::error() << "Failed to get status" << std::endl; + return; + } if (status == "") { logger::debug() << "Empty response status" << std::endl;