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/protocol.h b/include/scip2/protocol.h index e975ee71..f0d21d70 100644 --- a/include/scip2/protocol.h +++ b/include/scip2/protocol.h @@ -41,26 +41,33 @@ class Protocol const boost::posix_time::ptime& time_read) { std::istream stream(&buf); + std::string 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; + return; + } + std::string status; if (!std::getline(stream, status)) { logger::error() << "Failed to get status" << std::endl; return; } + if (status == "") + { + logger::debug() << "Empty response 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)) - { - } } public: diff --git a/include/scip2/response.h b/include/scip2/response.h index 3900550f..a54b6c24 100644 --- a/include/scip2/response.h +++ b/include/scip2/response.h @@ -67,6 +67,7 @@ class ResponseProcessor if (response == responses_.end()) { logger::debug() << "Unknown response " << command_code << std::endl; + readUntilEnd(stream); return; } (*(response->second))(time_read, echo_back, status, stream); 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/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..8c4c1c05 --- /dev/null +++ b/test/src/test_scip2.cpp @@ -0,0 +1,112 @@ +/* + * 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(boost::asio::streambuf& buf, const boost::posix_time::ptime& time_read) + { + 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); + EXPECT_EQ("00", status); + }; + p.registerCallback(cb); + + /* + Input data: + + QT // First QT response + 00P + + FOO // Unknown response + BAR + QT // This line must be ignored as a part of unknown command + + QT // This must be ignored as it doesn't have correct status line + + QT // Second QT response + 00P + + */ + 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); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +}