Feature/ppk scripts

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,6 +2,7 @@
 #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 {
 
@@ -16,13 +17,19 @@ class FileObservationLogger : public RawObservationInterface {
   FileObservationLogger() {}
   bool open(const std::string& filename);
   void close();
+
+  // Return status wetherlogger is currently running
+  bool isLogging();
+
   void insertObservation(const RawObservation& data) final;
   ~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
 

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,26 @@ class ReceiverRos : public SettingsIo {
 
   ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device);
 
+  // set up user ID
+  bool init() override;
+
+  // Set up File Logger
+  bool startFileLogger();
+  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_;
+
+  // Sender ID of device
+  uint16_t sbp_sender_id_{0x42};
 
-  // Observation callbackhandlers
+  // 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,6 +53,8 @@ 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:
   // Relaying all SBP 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 4 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/rover.launch

@@ -13,8 +13,10 @@
         <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" />
+        <param name="log_observations_to_file" value="true" />
+
+      	<!-- 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>

piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc

@@ -53,6 +53,36 @@ void CBtoRawObsConverter::observationCallback(msg_heartbeat_t msg) {
   finishMessage();
 }
 
+void CBtoRawObsConverter::observationCallback(msg_ephemeris_gps_t msg) {
+  // Repack into full SBP Message
+  startMessage();
+  sbp_send_message(&sbp_state_, SBP_MSG_EPHEMERIS_GPS, sbp_sender_id_, sizeof(msg),
+                   reinterpret_cast<uint8_t*>(&msg),
+                   &CBtoRawObsConverter::sbp_write_redirect);
+  // this triggers sbp_write_redirect
+  finishMessage();
+}
+
+void CBtoRawObsConverter::observationCallback(msg_ephemeris_glo_t msg) {
+  // Repack into full SBP Message
+  startMessage();
+  sbp_send_message(&sbp_state_, SBP_MSG_EPHEMERIS_GLO, sbp_sender_id_, sizeof(msg),
+                   reinterpret_cast<uint8_t*>(&msg),
+                   &CBtoRawObsConverter::sbp_write_redirect);
+  // this triggers sbp_write_redirect
+  finishMessage();
+}
+
+void CBtoRawObsConverter::observationCallback(msg_iono_t msg) {
+  // Repack into full SBP Message
+  startMessage();
+  sbp_send_message(&sbp_state_, SBP_MSG_IONO, sbp_sender_id_, sizeof(msg),
+                   reinterpret_cast<uint8_t*>(&msg),
+                   &CBtoRawObsConverter::sbp_write_redirect);
+  // this triggers sbp_write_redirect
+  finishMessage();
+}
+
 void CBtoRawObsConverter::startMessage() { buffer_msg_.clear(); }
 
 void CBtoRawObsConverter::finishMessage() {

piksi_multi_cpp/src/observations/file_observation_logger.cc

@@ -4,27 +4,40 @@ namespace piksi_multi_cpp {
 bool FileObservationLogger::open(const std::string& filename) {
   if (log_file_ != nullptr) return false;  // can't open if another one is open.
 
-  log_file_ = fopen(filename.c_str(), "wb");  // open binary file.
+  // If filename already exist do not overwrite but append next free number to filename
+  int file_number = 0;
+  std::string unique_filename = filename + ".sbp";
+  while ((log_file_ = fopen(unique_filename.c_str(), "r"))) {
+    file_number++;
+    unique_filename = filename + "_" + std::to_string(file_number) + ".sbp";
+    fclose(log_file_);
+  }
+
+  log_file_ = fopen(unique_filename.c_str(), "wb");  // open binary file.
   return (log_file_ == nullptr);
 }
 
 void FileObservationLogger::close() {
   if (log_file_ != nullptr) {
     fclose(log_file_);
+    log_file_ = nullptr;
   }
 }
 
+bool FileObservationLogger::isLogging() { return (log_file_ != nullptr); }
+
 void FileObservationLogger::insertObservation(
     const piksi_multi_cpp::RawObservation& data) {
-
   if (log_file_ != nullptr) {
+    file_mtx_.lock();
     // write in chunks of 1byte for now. maybe blocked write might be a bit
     // faster. But because we don't write large amounts of data, it probably
     // doesn't matter too much
     fwrite(data.data(), sizeof(uint8_t), data.size(), log_file_);
+    file_mtx_.unlock();
   }
 }
 
 FileObservationLogger::~FileObservationLogger() { close(); }
 
-}
+}  // namespace piksi_multi_cpp