Fix handling of multiple responses in one read

include/scip2/connection.h

@@ -22,6 +22,7 @@
 
 #include <boost/asio.hpp>
 #include <boost/bind/bind.hpp>
+#include <boost/function.hpp>
 
 #include <scip2/logger.h>
 

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:

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);

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

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)
   {

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)
   {

include/scip2/response/reset.h

@@ -48,7 +48,10 @@
       std::istream& stream)
   {
     if (cb_)
+    {
       cb_(time_read, echo_back, status);
+    }
+    readUntilEnd(stream);
   }
   void registerCallback(Callback cb)
   {
@@ -79,7 +82,10 @@
       std::istream& stream)
   {
     if (cb_)
+    {
       cb_(time_read, echo_back, status);
+    }
+    readUntilEnd(stream);
   }
   void registerCallback(Callback cb)
   {

include/scip2/response/stream.h

@@ -115,7 +115,10 @@
     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 @@
       }
     }
     if (cb_)
+    {
       cb_(time_read, echo_back, status, scan);
+    }
   }
 };
 
@@ -206,7 +211,9 @@
       }
     }
     if (cb_)
+    {
       cb_(time_read, echo_back, status, scan);
+    }
   }
 };
 

include/scip2/response/time_sync.h

@@ -65,7 +65,10 @@
     if (status != "00")
     {
       if (cb_)
+      {
         cb_(time_read, echo_back, status, timestamp);
+      }
+      readUntilEnd(stream);
       return;
     }
     if (echo_back[2] == '1')
@@ -74,13 +77,15 @@
       if (!std::getline(stream, stamp))
       {
         logger::error() << "Failed to get timestamp" << std::endl;
+        readUntilEnd(stream);
         return;
       }
       const uint8_t checksum = stamp.back();
       stamp.pop_back();  // remove checksum
       if (stamp.size() < 4)
       {
         logger::error() << "Wrong timestamp format" << std::endl;
+        readUntilEnd(stream);
         return;
       }
 
@@ -90,11 +95,15 @@
       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)
   {

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})
 

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 <gtest/gtest.h>
+
+#include <memory>
+#include <ostream>
+#include <string>
+#include <vector>
+
+#include <scip2/protocol.h>
+#include <scip2/connection.h>
+
+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<ConnectionDummy> 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<scip2::ResponseQT>(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();
+}