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>