Feature/message logger

piksi_multi_cpp/CMakeLists.txt

@@ -38,7 +38,6 @@ cs_add_library(${PROJECT_NAME}
         src/sbp_callback_handler/ros_time_handler.cc
         src/sbp_callback_handler/sbp_callback_handler.cc
         src/sbp_callback_handler/sbp_callback_handler_factory.cc
-        src/sbp_callback_handler/sbp_observation_callback_handler.cc
         )
 target_link_libraries(${PROJECT_NAME} serialport stdc++fs)
 

piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h

@@ -10,6 +10,7 @@ namespace piksi_multi_cpp {
 /*
  * Interface for a class that can handle observation callbacks
  * from SBP.
+ * More message callbacks can be added here.
  */
 class CallbackObservationInterface {
  public:
@@ -18,6 +19,9 @@ class CallbackObservationInterface {
   virtual void observationCallback(msg_glo_biases_t msg) = 0;
   virtual void observationCallback(msg_obs_t_var msg) = 0;
   virtual void observationCallback(msg_heartbeat_t msg) = 0;
+  virtual void observationCallback(msg_ephemeris_gps_t msg) = 0;
+  virtual void observationCallback(msg_ephemeris_glo_t msg) = 0;
+  virtual void observationCallback(msg_iono_t msg) = 0;
 };
 
 }  // namespace piksi_multi_cpp

piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h

@@ -28,6 +28,9 @@ class CBtoRawObsConverter : public CallbackObservationInterface {
   void observationCallback(msg_glo_biases_t msg) final;
   void observationCallback(msg_obs_t_var msg) final;
   void observationCallback(msg_heartbeat_t msg) final;
+  void observationCallback(msg_ephemeris_gps_t msg) final;
+  void observationCallback(msg_ephemeris_glo_t msg) final;
+  void observationCallback(msg_iono_t msg) final;
 
   static std::shared_ptr<CBtoRawObsConverter> createFor(
       const RawObservationInterface::Ptr& consumer, const uint16_t sbp_sender_id) {

piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h

@@ -2,28 +2,52 @@
 #define PIKSI_MULTI_CPP_OBSERVATIONS_FILE_OBSERVATION_LOGGER_H_
 
 #include <piksi_multi_cpp/observations/cb_to_raw_obs_converter.h>
+#include <mutex>
 
 namespace piksi_multi_cpp {
 
 /*
- * Logger that writes Raw observations into file such that it can be used
- * for ppk.
- *
- * Consumes Raw Observations, thus has interface RawObservationInterface
+ * Logger that writes raw observations into file such that it can be used
+ * for PPK.
+ * Logger consumes raw observations.
  */
 class FileObservationLogger : public RawObservationInterface {
  public:
   FileObservationLogger() {}
+  /**
+   * Creates and opens log file. In case file name already exists a number is
+   * appended to filename so that old files are not overwritten.
+   */
   bool open(const std::string& filename);
+
+  /**
+   * Close log file
+   */
   void close();
+
+  /**
+   * Return status whether logger is currently running
+   */
+  bool isLogging();
+
+  /**
+   * Write observations to binary file
+   */
   void insertObservation(const RawObservation& data) final;
+
+  /**
+   * Destructor making sure log file is closed.
+   */
   ~FileObservationLogger();
 
  private:
+  // File pointer.
   FILE* log_file_{nullptr};  // not using fstream, but raw file for performance.
 
-  // File pointer etc.
+  // mutex to lock file when writing
+  std::mutex file_mtx_;
 };
+
 }  // namespace piksi_multi_cpp
 
 #endif  // PIKSI_MULTI_CPP_OBSERVATIONS_FILE_OBSERVATION_LOGGER_H_

piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_base_station.h

@@ -25,7 +25,6 @@ class ReceiverBaseStation : public ReceiverRos {
       const piksi_rtk_msgs::PositionWithCovarianceStamped::Ptr& msg);
   void setupBaseStationSampling();
 
-  uint16_t sbp_sender_id_ {0x42};
   ros::ServiceServer resample_base_position_srv_;
   ros::Subscriber ml_estimate_sub_;
   ros::Subscriber receiver_state_sub_;

piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h

@@ -2,14 +2,17 @@
 #define PIKSI_MULTI_CPP_RECEIVER_RECEIVER_ROS_H_
 
 #include <ros/ros.h>
+#include <std_srvs/SetBool.h>
 #include <memory>
 #include <string>
 #include <thread>
 #include <vector>
+#include "piksi_multi_cpp/observations/file_observation_logger.h"
 #include "piksi_multi_cpp/receiver/settings_io.h"
 #include "piksi_multi_cpp/sbp_callback_handler/geotf_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/position_sampler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler.h"
+#include "piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h"
 #include "piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h"
 
 namespace piksi_multi_cpp {
@@ -21,12 +24,33 @@ class ReceiverRos : public SettingsIo {
 
   ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device);
 
+  // set up user ID
+  bool init() override;
+
+  /**
+   * Set up File Logger.
+   * Per default observations are stored in .ros with current date & time
+   * prefixed with "<receiver_type>_", therefore multiple receivers store
+   * observations into different files.
+   */
+  bool startFileLogger();
+
+  // ROS service to start/stop logger
+  bool startStopFileLoggerCallback(std_srvs::SetBool::Request& req,
+                                   std_srvs::SetBool::Response& res);
+
  protected:
   // ROS node handle in the correct receiver namespace.
   ros::NodeHandle nh_;
+  // Service Server for starting or stopping file logger
+  ros::ServiceServer start_stop_logger_srv_;
 
-  // Observation callbackhandlers
+  // Sender ID of device
+  uint16_t sbp_sender_id_{0x42};
+
+  // Observation & Ephemeris callbackhandlers
   std::unique_ptr<SBPObservationCallbackHandler> obs_cbs_;
+  std::unique_ptr<SBPEphemerisCallbackHandler> eph_cbs_;
 
   // get vector valued string params
   std::vector<std::string> getVectorParam(
@@ -36,8 +60,12 @@ class ReceiverRos : public SettingsIo {
   GeoTfHandler::Ptr geotf_handler_;
   // Averages the position over multiple ECEF messages.
   PositionSampler::Ptr position_sampler_;
+  // File logger object
+  std::shared_ptr<FileObservationLogger> obs_logger_;
 
  private:
+  void initObsLogger();
+
   // Relaying all SBP messages. Common for all receivers.
   std::vector<SBPCallbackHandler::Ptr> sbp_relays_;
   // Relaying all ROS messages. Common for all receivers.

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h

@@ -0,0 +1,37 @@
+#ifndef PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_EPHEMERIS_CALLBACK_HANDLER_H_
+#define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_EPHEMERIS_CALLBACK_HANDLER_H_
+#include <piksi_multi_cpp/sbp_callback_handler/sbp_lambda_callback_handler.h>
+#include <piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h>
+#include <ros/ros.h>
+
+namespace piksi_multi_cpp {
+
+/*
+ * Subscribes to all 3 observation type callbacks simultaneously using
+ * LambdaCallbackHandlers and redirects these messages to all registered
+ * listeners.
+ *
+ */
+class SBPEphemerisCallbackHandler
+    : public SBPMsgTypeCallbackHandler {
+ public:
+  SBPEphemerisCallbackHandler(const ros::NodeHandle nh,
+                                const std::shared_ptr<sbp_state_t>& state)
+      : SBPMsgTypeCallbackHandler(nh, state),
+        gps_ephemeris_handler_{getCallback<msg_ephemeris_gps_t>(),
+                          SBP_MSG_EPHEMERIS_GPS, state},
+        glo_ephemeris_handler_{getCallback<msg_ephemeris_glo_t>(), SBP_MSG_EPHEMERIS_GLO,
+                          state},
+        iono_delay_handler_{getCallback<msg_iono_t>(), SBP_MSG_IONO, state} {};
+
+ private:
+  /* Set up Lambda redirect callbacks for the three message types used for
+   * converting sbp to rinex, such that this class gets any of the three callbacks and can
+   * forward them to the observation listeners */
+  SBPLambdaCallbackHandler<msg_ephemeris_gps_t> gps_ephemeris_handler_;
+  SBPLambdaCallbackHandler<msg_ephemeris_glo_t> glo_ephemeris_handler_;
+  SBPLambdaCallbackHandler<msg_iono_t> iono_delay_handler_;
+};
+}  // namespace piksi_multi_cpp
+
+#endif

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h

@@ -0,0 +1,60 @@
+#ifndef PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_OBS_TYPE_CALLBACK_HANDLER_H_
+#define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_OBS_TYPE_CALLBACK_HANDLER_H_
+#include <libsbp/sbp.h>
+#include <ros/ros.h>
+#include <piksi_multi_cpp/observations/callback_observation_interface.h>
+#include <functional>
+
+namespace piksi_multi_cpp {
+namespace s = std::placeholders;
+
+class SBPMsgTypeCallbackHandler {
+
+ public:
+  SBPMsgTypeCallbackHandler(const ros::NodeHandle nh,
+                            const std::shared_ptr<sbp_state_t>& state)
+      : state_(state){};
+
+  /*
+   * Add listeners to message callbacks.
+   */
+  void addObservationCallbackListener(const CallbackObservationInterface::Ptr& listener) {
+    if (listener.get()) {
+      listeners_.push_back(listener);
+    }
+  }
+
+ protected:
+  /*
+   * Convenience method to create a std::function for the callbackToListeners
+   * method.
+   * Implemented in header because of template.
+   */
+  template <class SBPMsgStruct>
+  std::function<void(const SBPMsgStruct&, const uint8_t len)> getCallback() {
+    return std::bind(
+        &SBPMsgTypeCallbackHandler::callbackToListeners<SBPMsgStruct>, this,
+        s::_1, s::_2);
+  }
+
+  /*
+   * Forwards messages to observation listeners.
+   * Important: Only compiles if it is used only for sbp messages that can be
+   * consumed by the listeners, i.e. that have a viable observationCallback
+   * overload
+   * Implemented in header because of template.
+   */
+  template <class SBPMsgStruct>
+  void callbackToListeners(const SBPMsgStruct& msg, const uint8_t len) {
+    // trigger callback on all listeners
+    for (const auto& listener : listeners_) {
+      listener->observationCallback(msg);
+    }
+  }
+
+  std::shared_ptr<sbp_state_t> state_;
+  std::vector<CallbackObservationInterface::Ptr> listeners_{};
+};
+}  // namespace piksi_multi_cpp
+
+#endif

piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h

@@ -1,62 +1,32 @@
 #ifndef PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_OBSERVATION_CALLBACK_HANDLER_H_
 #define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_OBSERVATION_CALLBACK_HANDLER_H_
-#include <libsbp/observation.h>
-#include <libsbp/sbp.h>
-#include <libsbp/system.h>
-#include <piksi_multi_cpp/observations/callback_observation_interface.h>
-#include <piksi_multi_cpp/sbp_callback_handler/sbp_callback_handler.h>
 #include <piksi_multi_cpp/sbp_callback_handler/sbp_lambda_callback_handler.h>
+#include <piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h>
 #include <ros/ros.h>
-#include <memory>
-#include <utility>
 
 namespace piksi_multi_cpp {
-namespace s = std::placeholders;
 
 /*
  * Subscribes to all 4 observation type callbacks simultaneously using
  * LambdaCallbackHandlers and redirects these messages to all registered
  * listeners.
  *
  */
-class SBPObservationCallbackHandler {
+class SBPObservationCallbackHandler
+    : public SBPMsgTypeCallbackHandler {
  public:
   SBPObservationCallbackHandler(const ros::NodeHandle nh,
-                                const std::shared_ptr<sbp_state_t>& state);
-
-  void addObservationCallbackListener(
-      const CallbackObservationInterface::Ptr& listener);
+                                const std::shared_ptr<sbp_state_t>& state)
+      : SBPMsgTypeCallbackHandler(nh, state),
+        base_pos_handler_{getCallback<msg_base_pos_ecef_t>(),
+                          SBP_MSG_BASE_POS_ECEF, state},
+        glo_bias_handler_{getCallback<msg_glo_biases_t>(), SBP_MSG_GLO_BIASES,
+                          state},
+        obs_handler_{getCallback<msg_obs_t_var>(), SBP_MSG_OBS, state},
+        heartbeat_handler_{getCallback<msg_heartbeat_t>(), SBP_MSG_HEARTBEAT,
+                           state} {};
 
  private:
-  /*
-   * Convenience method to create a std::function for the callbackToListeners
-   * method.
-   * Implemented in header because of template.
-   */
-  template <class SBPMsgStruct>
-  std::function<void(const SBPMsgStruct&, const uint8_t len)> getCallback() {
-    return std::bind(
-        &SBPObservationCallbackHandler::callbackToListeners<SBPMsgStruct>, this,
-        s::_1, s::_2);
-  }
-
-  /*
-   * Forwards messages to observation listeners.
-   * Important: Only compiles if it is used only for sbp messages that can be
-   * consumed by the listeners, i.e. that have a viable observationCallback
-   * overload
-   * Implemented in header because of template.
-   */
-  template <class SBPMsgStruct>
-  void callbackToListeners(const SBPMsgStruct& msg, const uint8_t len) {
-    // trigger callback on all listeners
-    for (const auto& listener : listeners_) {
-      listener->observationCallback(msg);
-    }
-  }
-
-  std::shared_ptr<sbp_state_t> state_;
-  std::vector<CallbackObservationInterface::Ptr> listeners_{};
   /* Set up Lambda redirect callbacks for the four message types used for
    * corrections, such that this class gets any of the four callbacks and can
    * forward them to the observation listeners */

piksi_multi_cpp/launch/base.launch

@@ -5,6 +5,9 @@
       <param name="use_gps_time" value="true" />
       <param name="autostart_base_sampling" value="false" />
       <param name="num_desired_fixes" value="2000" />
+      <param name="log_observations_to_file" value="false" />
+      <!--param name="log_dir" value="/tmp" /-->
+      <!--param name="observation_filename" value="raw_observations" /-->
 
       <param name="base_station_receiver_0/udp_port"
              value="26078"/> <!-- Port on which UDP broadcasts are sent -->

piksi_multi_cpp/launch/rover.launch

@@ -4,17 +4,20 @@
 
   <!-- The next line allows you to set a predefined ENU frame. -->
   <!--rosparam file="$(find piksi_multi_cpp)/cfg/enu_origin.yaml" /-->
-  
+
   <group ns="$(arg ns)">
     <node name="piksi" pkg="piksi_multi_cpp" type="piksi_multi" output="log" required="true">
         <param name="device_ids" value="$(arg device_ids)" />
         <param name="use_gps_time" value="true" />
+        <param name="log_observations_to_file" value="false" />
+        <!--param name="log_dir" value="/tmp" /-->
+        <!--param name="observation_filename" value="piksi.obs" /-->
 
         <param name="udp_observation_port"
                value="26078"/> <!-- Port on which UDP broadcasts are received -->
 
-        <!-- Autoset ENU origin to startup position. Otherwise it waits for base station position. -->
-        <param name="set_enu_origin_from_current_pos" value="false" />
+      	<!-- Autoset ENU origin to startup position. Otherwise it waits for base station position. -->
+      	<param name="set_enu_origin_from_current_pos" value="false" />
     </node>
   </group>
 </launch>