[demos] custom_logging and raw_data_stream

CHANGELOG.rst

@@ -4,6 +4,7 @@ Changelog
 
 Forthcoming
 -----------
+* [core] ros_ prefixes dropped, custom_logging & raw_data_stream demos, `#40 <https://github.com/kobuki-base/kobuki_core/issues/40>`_
 * [infra] recommended firmware versions 1.1.4, 1.2.0, `#37 <https://github.com/kobuki-base/kobuki_core/issues/37>`_
 
 1.3.0 (2020-09-12)

include/kobuki_core/kobuki.hpp

@@ -26,6 +26,8 @@
 #include <ecl/exceptions/standard_exception.hpp>
 #include <ecl/geometry.hpp>
 #include "version_info.hpp"
+
+#include "logging.hpp"
 #include "parameters.hpp"
 #include "event_manager.hpp"
 #include "command.hpp"
@@ -259,7 +261,6 @@ class kobuki_PUBLIC Kobuki
   ecl::Signal<> sig_stream_data, sig_controller_info;
   ecl::Signal<const VersionInfo&> sig_version_info;
   ecl::Signal<const std::string&> sig_debug, sig_info, sig_warn, sig_error;
-  ecl::Signal<const std::vector<std::string>&> sig_named;
   ecl::Signal<Command::Buffer&> sig_raw_data_command; // should be const, but pushnpop is not fully realised yet for const args in the formatters.
   ecl::Signal<PacketFinder::BufferType&> sig_raw_data_stream; // should be const, but pushnpop is not fully realised yet for const args in the formatters.
   ecl::Signal<const std::vector<short>&> sig_raw_control_command;

src/demos/CMakeLists.txt

@@ -24,7 +24,9 @@ endmacro()
 ###############################################################################
 
 kobuki_core_add_demo(chirp)
+kobuki_core_add_demo(custom_logging)
 kobuki_core_add_demo(buttons)
-kobuki_core_add_demo(stream)
+kobuki_core_add_demo(raw_data_stream)
+kobuki_core_add_demo(data_stream)
 kobuki_core_add_demo(velocity_commands)
 kobuki_core_add_demo(simple_loop)

src/demos/custom_logging.cpp

@@ -0,0 +1,98 @@
+/**
+ * @file src/demos/logging.cpp
+ *
+ * @brief Example program for custom logging with kobuki.
+ *
+ * License: BSD
+ *   https://raw.githubusercontent.com/kobuki-base/kobuki_core/license/LICENSE
+ **/
+/*****************************************************************************
+ ** Includes
+ *****************************************************************************/
+
+#include <iostream>
+#include <string>
+
+#include <ecl/console.hpp>
+#include <ecl/sigslots.hpp>
+#include <ecl/time.hpp>
+#include <ecl/command_line.hpp>
+
+#include <kobuki_core/kobuki.hpp>
+
+/*****************************************************************************
+ ** Classes
+ *****************************************************************************/
+
+class KobukiManager
+{
+public:
+  KobukiManager(const std::string &device) :
+    slot_debug(&KobukiManager::logCustomDebug, *this),
+    slot_info(&KobukiManager::logCustomInfo, *this),
+    slot_warning(&KobukiManager::logCustomWarning, *this),
+    slot_error(&KobukiManager::logCustomError, *this)
+  {
+    kobuki::Parameters parameters;
+
+    parameters.device_port = device;
+    // Disable the default loggers
+    parameters.log_level = kobuki::LogLevel::NONE;
+
+    // Wire them up ourselves
+    slot_debug.connect(parameters.sigslots_namespace + "/debug");
+    slot_info.connect(parameters.sigslots_namespace + "/info");
+    slot_warning.connect(parameters.sigslots_namespace + "/warning");
+    slot_error.connect(parameters.sigslots_namespace + "/error");
+
+    try {
+      kobuki.init(parameters);
+    } catch (ecl::StandardException &e) {
+      std::cout << e.what();
+    }
+  }
+
+  void logCustomDebug(const std::string& message) {
+    std::cout << ecl::green << "[DEBUG_WITH_COLANDERS] " << message << ecl::reset << std::endl;
+  }
+
+  void logCustomInfo(const std::string& message) {
+    std::cout << "[INFO_WITH_COLANDERS] " << message << ecl::reset << std::endl;
+  }
+
+  void logCustomWarning(const std::string& message) {
+    std::cout << ecl::yellow << "[WARNING_WITH_COLANDERS] " << message << ecl::reset << std::endl;
+  }
+
+  void logCustomError(const std::string& message) {
+    std::cout << ecl::red << "[ERROR_WITH_COLANDERS] " << message << ecl::reset << std::endl;
+  }
+
+private:
+  kobuki::Kobuki kobuki;
+  ecl::Slot<const std::string&> slot_debug, slot_info, slot_warning, slot_error;
+};
+
+/*****************************************************************************
+ ** Main
+ *****************************************************************************/
+
+int main(int argc, char **argv)
+{
+  ecl::CmdLine cmd_line("logging", ' ', "0.3");
+  ecl::ValueArg<std::string> device_port(
+      "p", "port",
+      "Path to device file of serial port to open",
+      false,
+      "/dev/kobuki",
+      "string"
+  );
+  cmd_line.add(device_port);
+  cmd_line.parse(argc, argv);
+
+  std::cout << ecl::bold << "\nLogging Demo\n" << ecl::reset << std::endl;
+
+  KobukiManager kobuki_manager(device_port.getValue());
+  ecl::Sleep()(5);
+  return 0;
+}

src/demos/raw_data_stream.cpp

@@ -0,0 +1,84 @@
+/**
+ * @file src/demos/logging.cpp
+ *
+ * @brief Example program for custom logging with kobuki.
+ *
+ * License: BSD
+ *   https://raw.githubusercontent.com/kobuki-base/kobuki_core/license/LICENSE
+ **/
+/*****************************************************************************
+ ** Includes
+ *****************************************************************************/
+
+#include <iostream>
+#include <string>
+
+#include <ecl/console.hpp>
+#include <ecl/sigslots.hpp>
+#include <ecl/time.hpp>
+#include <ecl/command_line.hpp>
+
+#include <kobuki_core/kobuki.hpp>
+
+/*****************************************************************************
+ ** Classes
+ *****************************************************************************/
+
+class KobukiManager
+{
+public:
+  KobukiManager(const std::string &device) :
+    slot_raw_data_stream(&KobukiManager::logRawDataStream, *this)
+  {
+    kobuki::Parameters parameters;
+
+    parameters.device_port = device;
+
+    slot_raw_data_stream.connect(parameters.sigslots_namespace + "/raw_data_stream");
+
+    try {
+      kobuki.init(parameters);
+    } catch (ecl::StandardException &e) {
+      std::cout << e.what();
+    }
+  }
+
+  void logRawDataStream(kobuki::PacketFinder::BufferType& buffer) {
+    std::ostringstream ostream;
+    ostream << ecl::cyan << "[" << ecl::TimeStamp() << "] " << ecl::yellow;
+    ostream << std::setfill('0') << std::uppercase;
+    for (unsigned int i = 0; i < buffer.size(); i++) {
+      ostream << std::hex << std::setw(2) << static_cast<unsigned int>(buffer[i]) << " " << std::dec;
+    }
+    ostream << ecl::reset;
+    std::cout << ostream.str() << std::endl;
+  }
+
+private:
+  kobuki::Kobuki kobuki;
+  ecl::Slot<kobuki::PacketFinder::BufferType&> slot_raw_data_stream;
+};
+
+/*****************************************************************************
+ ** Main
+ *****************************************************************************/
+
+int main(int argc, char **argv)
+{
+  ecl::CmdLine cmd_line("raw_data_stream", ' ', "0.3");
+  ecl::ValueArg<std::string> device_port(
+      "p", "port",
+      "Path to device file of serial port to open",
+      false,
+      "/dev/kobuki",
+      "string"
+  );
+  cmd_line.add(device_port);
+  cmd_line.parse(argc, argv);
+
+  std::cout << ecl::bold << "\nRaw Data Stream Demo\n" << ecl::reset << std::endl;
+
+  KobukiManager kobuki_manager(device_port.getValue());
+  ecl::Sleep()(5);
+  return 0;
+}

src/driver/kobuki.cpp

@@ -75,7 +75,7 @@ Kobuki::~Kobuki()
   disable();
   shutdown_requested = true; // thread's spin() will catch this and terminate
   thread.join();
-  sig_debug.emit("Device: kobuki driver terminated.");
+  sig_debug.emit("Kobuki driver destructed.");
 }
 
 void Kobuki::init(Parameters &parameters)
@@ -102,7 +102,6 @@ void Kobuki::init(Parameters &parameters)
   sig_info.connect(sigslots_namespace + std::string("/info"));
   sig_warn.connect(sigslots_namespace + std::string("/warning"));
   sig_error.connect(sigslots_namespace + std::string("/error"));
-  sig_named.connect(sigslots_namespace + std::string("/named"));
 
   switch(this->parameters.log_level) {
     case LogLevel::DEBUG:
@@ -123,6 +122,7 @@ void Kobuki::init(Parameters &parameters)
 
   try {
     serial.open(parameters.device_port, ecl::BaudRate_115200, ecl::DataBits_8, ecl::StopBits_1, ecl::NoParity);  // this will throw exceptions - NotFoundError, OpenError
+    sig_debug.emit("Serial connection opened.");
     is_connected = true;
     serial.block(4000); // blocks by default, but just to be clear!
   }
@@ -249,19 +249,18 @@ void Kobuki::spin()
         is_alive = false;
         version_info_reminder = 10;
         controller_info_reminder = 10;
-        sig_debug.emit("Timed out while waiting for incoming bytes.");
+        sig_warn.emit("Timed out while waiting for incoming bytes.");
       }
       event_manager.update(is_connected, is_alive);
       continue;
     }
     else
     {
-      std::ostringstream ostream;
-      ostream << "kobuki_node : serial_read(" << n << ")"
-        << ", packet_finder.numberOfDataToRead(" << packet_finder.numberOfDataToRead() << ")";
-      //sig_debug.emit(ostream.str());
-      sig_named.emit(log("debug", "serial", ostream.str()));
-      // might be useful to send this to a topic if there is subscribers
+      // too much noise, even for debug
+      // std::ostringstream ostream;
+      // ostream << "serial_read(" << n << ")"
+      //   << ", packet_finder.numberOfDataToRead(" << packet_finder.numberOfDataToRead() << ")";
+      // sig_debug.emit(ostream.str());
     }
 
     if (packet_finder.update(buf, n)) // this clears packet finder's buffer and transfers important bytes into it
@@ -387,7 +386,10 @@ void Kobuki::spin()
       //std::cout << "---" << std::endl;
       unlockDataAccess();
 
-      is_alive = true;
+      if ( !is_alive ) {
+        sig_debug.emit("First data received.");
+        is_alive = true;
+      }
       event_manager.update(is_connected, is_alive);
       last_signal_time.stamp();
       sig_stream_data.emit();
@@ -410,7 +412,7 @@ void Kobuki::spin()
 void Kobuki::fixPayload(ecl::PushAndPop<unsigned char> & byteStream)
 {
   if (byteStream.size() < 3 ) { /* minimum size of sub-payload is 3; header_id, length, data */
-    sig_named.emit(log("error", "packet", "too small sub-payload detected."));
+    sig_error.emit("too small sub-payload detected.");
     byteStream.clear();
   } else {
     std::stringstream ostream;
@@ -436,8 +438,8 @@ void Kobuki::fixPayload(ecl::PushAndPop<unsigned char> & byteStream)
     }
     ostream << "]";
 
-    if (remains < length) sig_named.emit(log("error", "packet", "malformed sub-payload detected. "  + ostream.str()));
-    else                  sig_named.emit(log("debug", "packet", "unknown sub-payload detected. " + ostream.str()));
+    if (remains < length) sig_error.emit("Malformed sub-payload detected. "  + ostream.str());
+    else                  sig_debug.emit("Unknown sub-payload detected. " + ostream.str());
   }
 }
 
@@ -597,9 +599,8 @@ void Kobuki::sendCommand(Command command)
 {
   if( !is_alive || !is_connected ) {
     //need to do something
-    sig_debug.emit("Device state is not ready yet.");
-    if( !is_alive     ) sig_debug.emit(" - Device is not alive.");
-    if( !is_connected ) sig_debug.emit(" - Device is not connected.");
+    if( !is_alive     ) sig_debug.emit("Serial connection opened, but not yet receiving data.");
+    if( !is_connected ) sig_debug.emit("Serial connection not open.");
     //std::cout << is_enabled << ", " << is_alive << ", " << is_connected << std::endl;
     return;
   }

src/tools/simple_keyop.cpp

@@ -353,7 +353,6 @@ int main(int argc, char** argv)
       std::cout << ecl::green;
       std::cout << "current pose: [x: " << pose[0] << ", y: " << pose[1] << ", heading: " << pose[2] << "]" << std::endl;
       std::cout << ecl::reset;
-      std::cout << std::endl;
     }
   } catch ( ecl::StandardException &e ) {
     std::cout << e.what();