From 14c31beff07a316f2c8eb47d668e74d0a72b6295 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 4 Jun 2020 15:46:41 +0200 Subject: [PATCH 01/17] uncommented existing logger and hardcoded a fix sender id for now --- piksi_multi_cpp/src/receiver/receiver_ros.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 930763a3..067b7dc8 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -29,9 +29,9 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) if (1) { auto logger = std::make_shared(); - /*ROS_WARN_STREAM(logger->open("/tmp/tempfile.sbp")); + ROS_WARN_STREAM(logger->open("/tmp/tempfile.sbp")); obs_cbs_->addObservationCallbackListener( - CBtoRawObsConverter::createFor(logger));*/ + CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); // Hardcoded sender Id for now, as it porbably does not matter } } From 950c78407b9de9195a4eedb9dc3798299a29fb45 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 11 Jun 2020 15:14:59 +0200 Subject: [PATCH 02/17] Refactoring of observation callback handler to handle any message type New base class was set up handling the listener subscriptions to message callbacks. Object can inherit from this class and set up callbacks for any group of SBP Message (observation, emphemeris, ...) . --- piksi_multi_cpp/CMakeLists.txt | 1 - ...n_interface.h => callback_msg_interface.h} | 15 ++--- .../observations/cb_to_raw_obs_converter.h | 12 ++-- .../sbp_msg_type_callback_handler.h | 62 +++++++++++++++++++ .../sbp_observation_callback_handler.h | 48 ++++---------- .../observations/cb_to_raw_obs_converter.cc | 8 +-- .../src/receiver/receiver_base_station.cc | 4 +- piksi_multi_cpp/src/receiver/receiver_ros.cc | 4 +- .../sbp_observation_callback_handler.cc | 23 ------- 9 files changed, 97 insertions(+), 80 deletions(-) rename piksi_multi_cpp/include/piksi_multi_cpp/observations/{callback_observation_interface.h => callback_msg_interface.h} (53%) create mode 100644 piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h delete mode 100644 piksi_multi_cpp/src/sbp_callback_handler/sbp_observation_callback_handler.cc diff --git a/piksi_multi_cpp/CMakeLists.txt b/piksi_multi_cpp/CMakeLists.txt index 113883f3..7bd9f125 100644 --- a/piksi_multi_cpp/CMakeLists.txt +++ b/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) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h similarity index 53% rename from piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h rename to piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h index 33621cd7..26a3ff8e 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h @@ -8,16 +8,17 @@ namespace piksi_multi_cpp { /* - * Interface for a class that can handle observation callbacks + * Interface for a class that can handle message callbacks * from SBP. + * More message callbacks can be added here. */ -class CallbackObservationInterface { +class CallbackMsgInterface { public: - typedef std::shared_ptr Ptr; - virtual void observationCallback(msg_base_pos_ecef_t msg) = 0; - 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; + typedef std::shared_ptr Ptr; + virtual void messageCallback(msg_base_pos_ecef_t msg) = 0; + virtual void messageCallback(msg_glo_biases_t msg) = 0; + virtual void messageCallback(msg_obs_t_var msg) = 0; + virtual void messageCallback(msg_heartbeat_t msg) = 0; }; } // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h index ab1e1cbe..29d14751 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h @@ -1,6 +1,6 @@ #ifndef PIKSI_MULTI_CPP_OBSERVATIONS_CB_TO_RAW_OBS_CONVERTER_H_ #define PIKSI_MULTI_CPP_OBSERVATIONS_CB_TO_RAW_OBS_CONVERTER_H_ -#include +#include #include #include @@ -20,14 +20,14 @@ namespace piksi_multi_cpp { * the RawObservationInterface * */ -class CBtoRawObsConverter : public CallbackObservationInterface { +class CBtoRawObsConverter : public CallbackMsgInterface { public: typename std::shared_ptr Ptr; CBtoRawObsConverter(); - void observationCallback(msg_base_pos_ecef_t msg) final; - void observationCallback(msg_glo_biases_t msg) final; - void observationCallback(msg_obs_t_var msg) final; - void observationCallback(msg_heartbeat_t msg) final; + void messageCallback(msg_base_pos_ecef_t msg) final; + void messageCallback(msg_glo_biases_t msg) final; + void messageCallback(msg_obs_t_var msg) final; + void messageCallback(msg_heartbeat_t msg) final; static std::shared_ptr createFor( const RawObservationInterface::Ptr& consumer, const uint16_t sbp_sender_id) { diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h new file mode 100644 index 00000000..3b04f286 --- /dev/null +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h @@ -0,0 +1,62 @@ +#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 +#include +#include + +#include +#include + +namespace piksi_multi_cpp { +namespace s = std::placeholders; + +class SBPMsgTypeCallbackHandler { + + public: + SBPMsgTypeCallbackHandler(const ros::NodeHandle nh, + const std::shared_ptr& state) + : state_(state){}; + + /* + * Add listeners to message callbacks specified in interface. + */ + void addMsgCallbackListener(const CallbackMsgInterface::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 + std::function getCallback() { + return std::bind( + &SBPMsgTypeCallbackHandler::callbackToListeners, 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 messageCallback + * overload + * Implemented in header because of template. + */ + template + void callbackToListeners(const SBPMsgStruct& msg, const uint8_t len) { + // trigger callback on all listeners + for (const auto& listener : listeners_) { + listener->messageCallback(msg); + } + } + + std::shared_ptr state_; + std::vector listeners_{}; +}; +} // namespace piksi_multi_cpp + +#endif \ No newline at end of file diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h index 43ce7dbf..7df704d2 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h @@ -3,9 +3,10 @@ #include #include #include -#include +#include #include #include +#include #include #include #include @@ -19,44 +20,21 @@ namespace s = std::placeholders; * listeners. * */ -class SBPObservationCallbackHandler { +class SBPObservationCallbackHandler + : public SBPMsgTypeCallbackHandler { public: SBPObservationCallbackHandler(const ros::NodeHandle nh, - const std::shared_ptr& state); - - void addObservationCallbackListener( - const CallbackObservationInterface::Ptr& listener); + const std::shared_ptr& state) + : SBPMsgTypeCallbackHandler(nh, state), + base_pos_handler_{getCallback(), + SBP_MSG_BASE_POS_ECEF, state}, + glo_bias_handler_{getCallback(), SBP_MSG_GLO_BIASES, + state}, + obs_handler_{getCallback(), SBP_MSG_OBS, state}, + heartbeat_handler_{getCallback(), SBP_MSG_HEARTBEAT, + state} {}; private: - /* - * Convenience method to create a std::function for the callbackToListeners - * method. - * Implemented in header because of template. - */ - template - std::function getCallback() { - return std::bind( - &SBPObservationCallbackHandler::callbackToListeners, 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 - 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 state_; - std::vector 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 */ diff --git a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc index 1ea63eaa..2ec7b8ba 100644 --- a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc +++ b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc @@ -6,7 +6,7 @@ CBtoRawObsConverter::CBtoRawObsConverter() : sbp_state_{} { sbp_state_set_io_context(&sbp_state_, this); } -void CBtoRawObsConverter::observationCallback(msg_base_pos_ecef_t msg) { +void CBtoRawObsConverter::messageCallback(msg_base_pos_ecef_t msg) { startMessage(); // Repack into full SBP Message sbp_send_message(&sbp_state_, SBP_MSG_BASE_POS_ECEF, sbp_sender_id_, @@ -16,7 +16,7 @@ void CBtoRawObsConverter::observationCallback(msg_base_pos_ecef_t msg) { finishMessage(); } -void CBtoRawObsConverter::observationCallback(msg_glo_biases_t msg) { +void CBtoRawObsConverter::messageCallback(msg_glo_biases_t msg) { // Repack into full SBP Message startMessage(); sbp_send_message(&sbp_state_, SBP_MSG_GLO_BIASES, sbp_sender_id_, sizeof(msg), @@ -26,7 +26,7 @@ void CBtoRawObsConverter::observationCallback(msg_glo_biases_t msg) { finishMessage(); } -void CBtoRawObsConverter::observationCallback(msg_obs_t_var msg) { +void CBtoRawObsConverter::messageCallback(msg_obs_t_var msg) { // reconstruct raw buffer std::vector observation_buffer; observation_buffer.resize(msg.length()); @@ -43,7 +43,7 @@ void CBtoRawObsConverter::observationCallback(msg_obs_t_var msg) { finishMessage(); } -void CBtoRawObsConverter::observationCallback(msg_heartbeat_t msg) { +void CBtoRawObsConverter::messageCallback(msg_heartbeat_t msg) { // Repack into full SBP Message startMessage(); sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index 7f87c110..ca139ca4 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -67,7 +67,7 @@ void ReceiverBaseStation::setupUDPSenders() { if (udp_sender_) { udp_sender_->open(); // Register with observation callbacks - obs_cbs_->addObservationCallbackListener( + obs_cbs_->addMsgCallbackListener( CBtoRawObsConverter::createFor(udp_sender_, sbp_sender_id_)); } } @@ -81,7 +81,7 @@ void ReceiverBaseStation::setupUDPSenders() { udp_sender_->open(); // Register with observation callbacks - obs_cbs_->addObservationCallbackListener( + obs_cbs_->addMsgCallbackListener( CBtoRawObsConverter::createFor(udp_sender_, sbp_sender_id_)); } } diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 067b7dc8..fb7f3a6e 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -30,8 +30,8 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) if (1) { auto logger = std::make_shared(); ROS_WARN_STREAM(logger->open("/tmp/tempfile.sbp")); - obs_cbs_->addObservationCallbackListener( - CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); // Hardcoded sender Id for now, as it porbably does not matter + obs_cbs_->addMsgCallbackListener( + CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); // Hardcoded sender Id for now, as it probably does not matter } } diff --git a/piksi_multi_cpp/src/sbp_callback_handler/sbp_observation_callback_handler.cc b/piksi_multi_cpp/src/sbp_callback_handler/sbp_observation_callback_handler.cc deleted file mode 100644 index 824cc6db..00000000 --- a/piksi_multi_cpp/src/sbp_callback_handler/sbp_observation_callback_handler.cc +++ /dev/null @@ -1,23 +0,0 @@ -#include - -namespace piksi_multi_cpp { - -SBPObservationCallbackHandler::SBPObservationCallbackHandler( - const ros::NodeHandle nh, const std::shared_ptr& state) - : state_(state), - base_pos_handler_{getCallback(), - SBP_MSG_BASE_POS_ECEF, state}, - glo_bias_handler_{getCallback(), SBP_MSG_GLO_BIASES, - state}, - obs_handler_{getCallback(), SBP_MSG_OBS, state}, - heartbeat_handler_{getCallback(), SBP_MSG_HEARTBEAT, - state} {} - -void SBPObservationCallbackHandler::addObservationCallbackListener( - const CallbackObservationInterface::Ptr& listener) { - if (listener.get()) { - listeners_.push_back(listener); - } -} - -} // namespace piksi_multi_cpp From 0402c132204988941a31ddfc7258962639fc2962 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Fri, 12 Jun 2020 23:53:12 +0200 Subject: [PATCH 03/17] Created new MsgCallbackHandler for ephemeris and ionospheric delay Adds 3 lambda functions. Corresponding methods have been added to cb_to_raw_obs_converter as well. --- .../observations/callback_msg_interface.h | 3 ++ .../observations/cb_to_raw_obs_converter.h | 3 ++ .../sbp_ephemeris_callback_handler.h | 37 +++++++++++++++++++ .../sbp_msg_type_callback_handler.h | 2 - .../sbp_observation_callback_handler.h | 8 ---- .../observations/cb_to_raw_obs_converter.cc | 30 +++++++++++++++ 6 files changed, 73 insertions(+), 10 deletions(-) create mode 100644 piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h index 26a3ff8e..0d28d735 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h @@ -19,6 +19,9 @@ class CallbackMsgInterface { virtual void messageCallback(msg_glo_biases_t msg) = 0; virtual void messageCallback(msg_obs_t_var msg) = 0; virtual void messageCallback(msg_heartbeat_t msg) = 0; + virtual void messageCallback(msg_ephemeris_gps_t msg) = 0; + virtual void messageCallback(msg_ephemeris_glo_t msg) = 0; + virtual void messageCallback(msg_iono_t msg) = 0; }; } // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h index 29d14751..2497d1ab 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h @@ -28,6 +28,9 @@ class CBtoRawObsConverter : public CallbackMsgInterface { void messageCallback(msg_glo_biases_t msg) final; void messageCallback(msg_obs_t_var msg) final; void messageCallback(msg_heartbeat_t msg) final; + void messageCallback(msg_ephemeris_gps_t msg) final; + void messageCallback(msg_ephemeris_glo_t msg) final; + void messageCallback(msg_iono_t msg) final; static std::shared_ptr createFor( const RawObservationInterface::Ptr& consumer, const uint16_t sbp_sender_id) { diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h new file mode 100644 index 00000000..ede3f270 --- /dev/null +++ b/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 +#include +#include + +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& state) + : SBPMsgTypeCallbackHandler(nh, state), + gps_ephemeris_handler_{getCallback(), + SBP_MSG_EPHEMERIS_GPS, state}, + glo_ephemeris_handler_{getCallback(), SBP_MSG_EPHEMERIS_GLO, + state}, + iono_delay_handler_{getCallback(), 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 gps_ephemeris_handler_; + SBPLambdaCallbackHandler glo_ephemeris_handler_; + SBPLambdaCallbackHandler iono_delay_handler_; +}; +} // namespace piksi_multi_cpp + +#endif diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h index 3b04f286..1e923fd5 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h @@ -1,9 +1,7 @@ #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 -#include #include - #include #include diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h index 7df704d2..c7a135a9 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_observation_callback_handler.h @@ -1,18 +1,10 @@ #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 -#include -#include -#include -#include #include #include #include -#include -#include namespace piksi_multi_cpp { -namespace s = std::placeholders; /* * Subscribes to all 4 observation type callbacks simultaneously using diff --git a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc index 2ec7b8ba..3b1682f3 100644 --- a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc +++ b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc @@ -53,6 +53,36 @@ void CBtoRawObsConverter::messageCallback(msg_heartbeat_t msg) { finishMessage(); } +void CBtoRawObsConverter::messageCallback(msg_ephemeris_gps_t msg) { + // Repack into full SBP Message + startMessage(); + sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + reinterpret_cast(&msg), + &CBtoRawObsConverter::sbp_write_redirect); + // this triggers sbp_write_redirect + finishMessage(); +} + +void CBtoRawObsConverter::messageCallback(msg_ephemeris_glo_t msg) { + // Repack into full SBP Message + startMessage(); + sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + reinterpret_cast(&msg), + &CBtoRawObsConverter::sbp_write_redirect); + // this triggers sbp_write_redirect + finishMessage(); +} + +void CBtoRawObsConverter::messageCallback(msg_iono_t msg) { + // Repack into full SBP Message + startMessage(); + sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + reinterpret_cast(&msg), + &CBtoRawObsConverter::sbp_write_redirect); + // this triggers sbp_write_redirect + finishMessage(); +} + void CBtoRawObsConverter::startMessage() { buffer_msg_.clear(); } void CBtoRawObsConverter::finishMessage() { From 71f9ee0c31df0e1f197405d9aa43a29850ac0f78 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Sat, 13 Jun 2020 14:03:05 +0200 Subject: [PATCH 04/17] Added ephemeris callback handler instance to ros receiver. --- .../include/piksi_multi_cpp/receiver/receiver_ros.h | 4 +++- .../sbp_callback_handler/sbp_msg_type_callback_handler.h | 2 +- piksi_multi_cpp/src/receiver/receiver_ros.cc | 4 +++- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h index 599d95f6..444baeb8 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h @@ -11,6 +11,7 @@ #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_observation_callback_handler.h" +#include "piksi_multi_cpp/sbp_callback_handler/sbp_ephemeris_callback_handler.h" namespace piksi_multi_cpp { @@ -25,8 +26,9 @@ class ReceiverRos : public SettingsIo { // ROS node handle in the correct receiver namespace. ros::NodeHandle nh_; - // Observation callbackhandlers + // Observation & Ephemeris callbackhandlers std::unique_ptr obs_cbs_; + std::unique_ptr eph_cbs_; // get vector valued string params std::vector getVectorParam( diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h index 1e923fd5..134e2199 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h @@ -16,7 +16,7 @@ class SBPMsgTypeCallbackHandler { : state_(state){}; /* - * Add listeners to message callbacks specified in interface. + * Add listeners to message callbacks. */ void addMsgCallbackListener(const CallbackMsgInterface::Ptr& listener) { if (listener.get()) { diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index fb7f3a6e..8db2ccc4 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -24,14 +24,16 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) position_sampler_ = std::make_shared( nh, state_, ros_time_handler, geotf_handler_); - // Create observation callbacks + // Create observation & ephemeris callbacks obs_cbs_ = std::make_unique(nh, state_); + eph_cbs_ = std::make_unique(nh, state_); if (1) { auto logger = std::make_shared(); ROS_WARN_STREAM(logger->open("/tmp/tempfile.sbp")); obs_cbs_->addMsgCallbackListener( CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); // Hardcoded sender Id for now, as it probably does not matter + eph_cbs_->addMsgCallbackListener(CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); } } From e684d387411b328829d46c7eb9b8114f75b59d0b Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Sat, 13 Jun 2020 14:05:20 +0200 Subject: [PATCH 05/17] Lock on file, if for some reason two callbacks would try to write at same time --- .../piksi_multi_cpp/observations/file_observation_logger.h | 7 ++++++- .../src/observations/file_observation_logger.cc | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h index 1d2dd149..546602f2 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h +++ b/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 +#include namespace piksi_multi_cpp { @@ -20,9 +21,13 @@ class FileObservationLogger : public RawObservationInterface { ~FileObservationLogger(); private: + // File pointer. FILE* log_file_{nullptr}; // not using fstream, but raw file for performance. + + // mutex to lock file when writing + std::mutex file_mtx_; + - // File pointer etc. }; } // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/src/observations/file_observation_logger.cc b/piksi_multi_cpp/src/observations/file_observation_logger.cc index 19f222bb..9a751ba5 100644 --- a/piksi_multi_cpp/src/observations/file_observation_logger.cc +++ b/piksi_multi_cpp/src/observations/file_observation_logger.cc @@ -18,10 +18,12 @@ 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(); } } From 32c54cc7bfc08e162b362dccb77f64a052d329bf Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Sat, 13 Jun 2020 19:48:32 +0200 Subject: [PATCH 06/17] Tiny bug fix in cb_to_raw_obs_converter Wrong message type was copy pasted to sbp_send_message method --- piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc index 3b1682f3..c4dfa6fc 100644 --- a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc +++ b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc @@ -56,7 +56,7 @@ void CBtoRawObsConverter::messageCallback(msg_heartbeat_t msg) { void CBtoRawObsConverter::messageCallback(msg_ephemeris_gps_t msg) { // Repack into full SBP Message startMessage(); - sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + sbp_send_message(&sbp_state_, SBP_MSG_EPHEMERIS_GPS, sbp_sender_id_, sizeof(msg), reinterpret_cast(&msg), &CBtoRawObsConverter::sbp_write_redirect); // this triggers sbp_write_redirect @@ -66,7 +66,7 @@ void CBtoRawObsConverter::messageCallback(msg_ephemeris_gps_t msg) { void CBtoRawObsConverter::messageCallback(msg_ephemeris_glo_t msg) { // Repack into full SBP Message startMessage(); - sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + sbp_send_message(&sbp_state_, SBP_MSG_EPHEMERIS_GLO, sbp_sender_id_, sizeof(msg), reinterpret_cast(&msg), &CBtoRawObsConverter::sbp_write_redirect); // this triggers sbp_write_redirect @@ -76,7 +76,7 @@ void CBtoRawObsConverter::messageCallback(msg_ephemeris_glo_t msg) { void CBtoRawObsConverter::messageCallback(msg_iono_t msg) { // Repack into full SBP Message startMessage(); - sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), + sbp_send_message(&sbp_state_, SBP_MSG_IONO, sbp_sender_id_, sizeof(msg), reinterpret_cast(&msg), &CBtoRawObsConverter::sbp_write_redirect); // this triggers sbp_write_redirect From 47351a06a6058b77683cbf181f22108886fe2c12 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Sat, 13 Jun 2020 19:52:08 +0200 Subject: [PATCH 07/17] Cleaned up FileLogger initialization - File logger is initialized in own method if in launch file logging is set to true. - Per default logger is off. Default binary directory is: /tmp/tmp_observations.sbp - Sender id was moved down to receiver_ros, so that every receiver stores its own ID, and not only the base station (sender id is needed for logging) --- .../receiver/receiver_base_station.h | 1 - .../piksi_multi_cpp/receiver/receiver_ros.h | 11 +++++- piksi_multi_cpp/launch/rover.launch | 2 ++ .../src/receiver/receiver_base_station.cc | 14 +++++--- .../src/receiver/receiver_position.cc | 8 +++++ piksi_multi_cpp/src/receiver/receiver_ros.cc | 36 ++++++++++++++----- 6 files changed, 57 insertions(+), 15 deletions(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_base_station.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_base_station.h index d12d5371..3f2b704d 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_base_station.h +++ b/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_; diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h index 444baeb8..531c6b7d 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h @@ -10,8 +10,8 @@ #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_observation_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 { @@ -22,10 +22,19 @@ class ReceiverRos : public SettingsIo { ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device); + // set up user ID + bool init() override; + + void startFileLogger( + const std::string& log_file_dir); + protected: // ROS node handle in the correct receiver namespace. ros::NodeHandle nh_; + // Sender ID of device + uint16_t sbp_sender_id_{0x42}; + // Observation & Ephemeris callbackhandlers std::unique_ptr obs_cbs_; std::unique_ptr eph_cbs_; diff --git a/piksi_multi_cpp/launch/rover.launch b/piksi_multi_cpp/launch/rover.launch index 37aa53f1..84578ea1 100644 --- a/piksi_multi_cpp/launch/rover.launch +++ b/piksi_multi_cpp/launch/rover.launch @@ -13,6 +13,8 @@ + + diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index ca139ca4..f6e7f5f8 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -1,9 +1,9 @@ +#include "piksi_multi_cpp/receiver/receiver_base_station.h" #include #include #include #include #include -#include "piksi_multi_cpp/receiver/receiver_base_station.h" namespace piksi_multi_cpp { @@ -20,12 +20,18 @@ bool ReceiverBaseStation::init() { } // Setup UDP senders. - while (!readSetting("system_info", "sbp_sender_id")) { - } - sbp_sender_id_ = static_cast(std::stoul(getValue(), nullptr, 16)); ROS_INFO("UDP corrections sender ID: 0x%.4X", sbp_sender_id_); setupUDPSenders(); + // Start file logger if requested + ros::NodeHandle nh_private("~"); + auto log_to_file = nh_private.param("log_observations_to_file", false); + if (log_to_file) { + auto log_file_dir = + nh_private.param("log_file_dir", "/tmp/tmp_observations.sbp"); + ReceiverRos::startFileLogger(log_file_dir); + } + return true; } diff --git a/piksi_multi_cpp/src/receiver/receiver_position.cc b/piksi_multi_cpp/src/receiver/receiver_position.cc index 87210eec..d2007fd8 100644 --- a/piksi_multi_cpp/src/receiver/receiver_position.cc +++ b/piksi_multi_cpp/src/receiver/receiver_position.cc @@ -19,6 +19,14 @@ bool ReceiverPosition::init() { ros::NodeHandle nh_private("~"); udp_receiver_.start(nh_private.param("udp_observation_port", 26078)); + // Start file logger if requested + auto log_to_file = nh_private.param("log_observations_to_file", false); + if (log_to_file) { + auto log_file_dir = + nh_private.param("log_file_dir", "/tmp/tmp_observations.sbp"); + ReceiverRos::startFileLogger(log_file_dir); + } + return true; } diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 8db2ccc4..2cc2ae1e 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -24,17 +24,35 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) position_sampler_ = std::make_shared( nh, state_, ros_time_handler, geotf_handler_); - // Create observation & ephemeris callbacks + // Create observation callbacks obs_cbs_ = std::make_unique(nh, state_); - eph_cbs_ = std::make_unique(nh, state_); - - if (1) { - auto logger = std::make_shared(); - ROS_WARN_STREAM(logger->open("/tmp/tempfile.sbp")); - obs_cbs_->addMsgCallbackListener( - CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); // Hardcoded sender Id for now, as it probably does not matter - eph_cbs_->addMsgCallbackListener(CBtoRawObsConverter::createFor(logger, uint16_t(0x42))); +} + +bool ReceiverRos::init() { + if (!Receiver::init()) { + return false; } + + // Get and store ID of device + while (!readSetting("system_info", "sbp_sender_id")) { + } + sbp_sender_id_ = static_cast(std::stoul(getValue(), nullptr, 16)); + + return true; +} + +void ReceiverRos::startFileLogger(const std::string& log_file_dir) { + // Create ephemeris callbacks + eph_cbs_ = std::make_unique(nh_, state_); + + auto logger = std::make_shared(); + ROS_WARN_STREAM(logger->open(log_file_dir)); + + // Add logger as listener to callbacks + obs_cbs_->addMsgCallbackListener(CBtoRawObsConverter::createFor( + logger, sbp_sender_id_)); + eph_cbs_->addMsgCallbackListener( + CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); } std::vector ReceiverRos::getVectorParam( From 7d98f7d5f82bca5d7ac5c05684a17d7958f23138 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 15 Jun 2020 10:07:23 +0200 Subject: [PATCH 08/17] Some renaming Changed back class names to "observation". Ephemeris can also be considered an observation --- .../observations/callback_msg_interface.h | 28 ------------------- .../callback_observation_interface.h | 28 +++++++++++++++++++ .../observations/cb_to_raw_obs_converter.h | 18 ++++++------ .../sbp_msg_type_callback_handler.h | 10 +++---- .../observations/cb_to_raw_obs_converter.cc | 14 +++++----- .../src/receiver/receiver_base_station.cc | 4 +-- piksi_multi_cpp/src/receiver/receiver_ros.cc | 4 +-- 7 files changed, 53 insertions(+), 53 deletions(-) delete mode 100644 piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h create mode 100644 piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h deleted file mode 100644 index 0d28d735..00000000 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_msg_interface.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ -#define PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ -#include -#include -#include -#include -#include - -namespace piksi_multi_cpp { -/* - * Interface for a class that can handle message callbacks - * from SBP. - * More message callbacks can be added here. - */ -class CallbackMsgInterface { - public: - typedef std::shared_ptr Ptr; - virtual void messageCallback(msg_base_pos_ecef_t msg) = 0; - virtual void messageCallback(msg_glo_biases_t msg) = 0; - virtual void messageCallback(msg_obs_t_var msg) = 0; - virtual void messageCallback(msg_heartbeat_t msg) = 0; - virtual void messageCallback(msg_ephemeris_gps_t msg) = 0; - virtual void messageCallback(msg_ephemeris_glo_t msg) = 0; - virtual void messageCallback(msg_iono_t msg) = 0; -}; - -} // namespace piksi_multi_cpp -#endif // PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h new file mode 100644 index 00000000..21f21cce --- /dev/null +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/callback_observation_interface.h @@ -0,0 +1,28 @@ +#ifndef PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ +#define PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ +#include +#include +#include +#include +#include + +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: + typedef std::shared_ptr Ptr; + virtual void observationCallback(msg_base_pos_ecef_t msg) = 0; + 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 +#endif // PIKSI_MULTI_CPP_OBSERVATIONS_CALLBACK_OBSERVATION_INTERFACE_H_ diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h index 2497d1ab..c88d2689 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/cb_to_raw_obs_converter.h @@ -1,6 +1,6 @@ #ifndef PIKSI_MULTI_CPP_OBSERVATIONS_CB_TO_RAW_OBS_CONVERTER_H_ #define PIKSI_MULTI_CPP_OBSERVATIONS_CB_TO_RAW_OBS_CONVERTER_H_ -#include +#include #include #include @@ -20,17 +20,17 @@ namespace piksi_multi_cpp { * the RawObservationInterface * */ -class CBtoRawObsConverter : public CallbackMsgInterface { +class CBtoRawObsConverter : public CallbackObservationInterface { public: typename std::shared_ptr Ptr; CBtoRawObsConverter(); - void messageCallback(msg_base_pos_ecef_t msg) final; - void messageCallback(msg_glo_biases_t msg) final; - void messageCallback(msg_obs_t_var msg) final; - void messageCallback(msg_heartbeat_t msg) final; - void messageCallback(msg_ephemeris_gps_t msg) final; - void messageCallback(msg_ephemeris_glo_t msg) final; - void messageCallback(msg_iono_t msg) final; + void observationCallback(msg_base_pos_ecef_t msg) final; + 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 createFor( const RawObservationInterface::Ptr& consumer, const uint16_t sbp_sender_id) { diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h index 134e2199..6fd898cc 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/sbp_callback_handler/sbp_msg_type_callback_handler.h @@ -2,7 +2,7 @@ #define PIKSI_MULTI_CPP_SBP_CALLBACK_HANDLER_SBP_OBS_TYPE_CALLBACK_HANDLER_H_ #include #include -#include +#include #include namespace piksi_multi_cpp { @@ -18,7 +18,7 @@ class SBPMsgTypeCallbackHandler { /* * Add listeners to message callbacks. */ - void addMsgCallbackListener(const CallbackMsgInterface::Ptr& listener) { + void addObservationCallbackListener(const CallbackObservationInterface::Ptr& listener) { if (listener.get()) { listeners_.push_back(listener); } @@ -40,7 +40,7 @@ class SBPMsgTypeCallbackHandler { /* * 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 messageCallback + * consumed by the listeners, i.e. that have a viable observationCallback * overload * Implemented in header because of template. */ @@ -48,12 +48,12 @@ class SBPMsgTypeCallbackHandler { void callbackToListeners(const SBPMsgStruct& msg, const uint8_t len) { // trigger callback on all listeners for (const auto& listener : listeners_) { - listener->messageCallback(msg); + listener->observationCallback(msg); } } std::shared_ptr state_; - std::vector listeners_{}; + std::vector listeners_{}; }; } // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc index c4dfa6fc..a032d343 100644 --- a/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc +++ b/piksi_multi_cpp/src/observations/cb_to_raw_obs_converter.cc @@ -6,7 +6,7 @@ CBtoRawObsConverter::CBtoRawObsConverter() : sbp_state_{} { sbp_state_set_io_context(&sbp_state_, this); } -void CBtoRawObsConverter::messageCallback(msg_base_pos_ecef_t msg) { +void CBtoRawObsConverter::observationCallback(msg_base_pos_ecef_t msg) { startMessage(); // Repack into full SBP Message sbp_send_message(&sbp_state_, SBP_MSG_BASE_POS_ECEF, sbp_sender_id_, @@ -16,7 +16,7 @@ void CBtoRawObsConverter::messageCallback(msg_base_pos_ecef_t msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_glo_biases_t msg) { +void CBtoRawObsConverter::observationCallback(msg_glo_biases_t msg) { // Repack into full SBP Message startMessage(); sbp_send_message(&sbp_state_, SBP_MSG_GLO_BIASES, sbp_sender_id_, sizeof(msg), @@ -26,7 +26,7 @@ void CBtoRawObsConverter::messageCallback(msg_glo_biases_t msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_obs_t_var msg) { +void CBtoRawObsConverter::observationCallback(msg_obs_t_var msg) { // reconstruct raw buffer std::vector observation_buffer; observation_buffer.resize(msg.length()); @@ -43,7 +43,7 @@ void CBtoRawObsConverter::messageCallback(msg_obs_t_var msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_heartbeat_t msg) { +void CBtoRawObsConverter::observationCallback(msg_heartbeat_t msg) { // Repack into full SBP Message startMessage(); sbp_send_message(&sbp_state_, SBP_MSG_HEARTBEAT, sbp_sender_id_, sizeof(msg), @@ -53,7 +53,7 @@ void CBtoRawObsConverter::messageCallback(msg_heartbeat_t msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_ephemeris_gps_t msg) { +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), @@ -63,7 +63,7 @@ void CBtoRawObsConverter::messageCallback(msg_ephemeris_gps_t msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_ephemeris_glo_t msg) { +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), @@ -73,7 +73,7 @@ void CBtoRawObsConverter::messageCallback(msg_ephemeris_glo_t msg) { finishMessage(); } -void CBtoRawObsConverter::messageCallback(msg_iono_t msg) { +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), diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index f6e7f5f8..c5a536b1 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -73,7 +73,7 @@ void ReceiverBaseStation::setupUDPSenders() { if (udp_sender_) { udp_sender_->open(); // Register with observation callbacks - obs_cbs_->addMsgCallbackListener( + obs_cbs_->addObservationCallbackListener( CBtoRawObsConverter::createFor(udp_sender_, sbp_sender_id_)); } } @@ -87,7 +87,7 @@ void ReceiverBaseStation::setupUDPSenders() { udp_sender_->open(); // Register with observation callbacks - obs_cbs_->addMsgCallbackListener( + obs_cbs_->addObservationCallbackListener( CBtoRawObsConverter::createFor(udp_sender_, sbp_sender_id_)); } } diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 2cc2ae1e..0430be3d 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -49,9 +49,9 @@ void ReceiverRos::startFileLogger(const std::string& log_file_dir) { ROS_WARN_STREAM(logger->open(log_file_dir)); // Add logger as listener to callbacks - obs_cbs_->addMsgCallbackListener(CBtoRawObsConverter::createFor( + obs_cbs_->addObservationCallbackListener(CBtoRawObsConverter::createFor( logger, sbp_sender_id_)); - eph_cbs_->addMsgCallbackListener( + eph_cbs_->addObservationCallbackListener( CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); } From a416ac90e5df256e724532a02cccc231c1b7a136 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Mon, 29 Jun 2020 18:24:50 +0200 Subject: [PATCH 09/17] Add time-date filename to the log. --- .../src/receiver/receiver_position.cc | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/piksi_multi_cpp/src/receiver/receiver_position.cc b/piksi_multi_cpp/src/receiver/receiver_position.cc index d2007fd8..8f698bfd 100644 --- a/piksi_multi_cpp/src/receiver/receiver_position.cc +++ b/piksi_multi_cpp/src/receiver/receiver_position.cc @@ -1,3 +1,7 @@ +#include +#include +#include + #include "piksi_multi_cpp/receiver/receiver_position.h" namespace piksi_multi_cpp { @@ -22,9 +26,20 @@ bool ReceiverPosition::init() { // Start file logger if requested auto log_to_file = nh_private.param("log_observations_to_file", false); if (log_to_file) { - auto log_file_dir = - nh_private.param("log_file_dir", "/tmp/tmp_observations.sbp"); - ReceiverRos::startFileLogger(log_file_dir); + std::string log_file_dir = + nh_private.param("log_file_dir", "./"); + bool use_date_time_as_filename = true; + nh_private.param("use_date_time_as_filename", use_date_time_as_filename, + use_date_time_as_filename); + std::string filename = "observations.sbp"; + if (use_date_time_as_filename) { + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + std::stringstream time_ss; + time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; + filename = time_ss.str(); + } + ReceiverRos::startFileLogger(log_file_dir + '/' + filename); } return true; From 331d2b1eafaef478b09cc5a900ac3faaa5767cf4 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Tue, 30 Jun 2020 13:59:43 +0200 Subject: [PATCH 10/17] Base station corrections too. --- .../src/receiver/receiver_base_station.cc | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index c5a536b1..b7d311f0 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -27,9 +27,20 @@ bool ReceiverBaseStation::init() { ros::NodeHandle nh_private("~"); auto log_to_file = nh_private.param("log_observations_to_file", false); if (log_to_file) { - auto log_file_dir = - nh_private.param("log_file_dir", "/tmp/tmp_observations.sbp"); - ReceiverRos::startFileLogger(log_file_dir); + std::string log_file_dir = + nh_private.param("log_file_dir", "./"); + bool use_date_time_as_filename = true; + nh_private.param("use_date_time_as_filename", use_date_time_as_filename, + use_date_time_as_filename); + std::string filename = "observations.sbp"; + if (use_date_time_as_filename) { + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + std::stringstream time_ss; + time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; + filename = time_ss.str(); + } + ReceiverRos::startFileLogger(log_file_dir + '/' + filename); } return true; @@ -41,8 +52,8 @@ bool ReceiverBaseStation::init() { // The number of desired fixes is determined through the parameter // `num_desired_fixes` when autosampling or defined in the service call. void ReceiverBaseStation::setupBaseStationSampling() { - // Subscribe to maximum likelihood estimate and advertise service to overwrite - // current base station position. + // Subscribe to maximum likelihood estimate and advertise service to + // overwrite current base station position. resample_base_position_srv_ = nh_.advertiseService( "resample_base_position", &ReceiverBaseStation::resampleBasePositionCallback, this); From 7528c26a0f2535ba6435214854a6f56561af87f8 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 14 Sep 2020 11:39:08 +0200 Subject: [PATCH 11/17] Moved initialization of file logger into receiver_ros Also added prefix to observation file with the type of receiver, hopefully this allows to store observations from multiple connected receivers into different files. --- .../piksi_multi_cpp/receiver/receiver_ros.h | 3 +- .../src/receiver/receiver_base_station.cc | 15 +------ .../src/receiver/receiver_position.cc | 15 +------ piksi_multi_cpp/src/receiver/receiver_ros.cc | 39 +++++++++++++++++-- 4 files changed, 38 insertions(+), 34 deletions(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h index 531c6b7d..30a9999a 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h @@ -25,8 +25,7 @@ class ReceiverRos : public SettingsIo { // set up user ID bool init() override; - void startFileLogger( - const std::string& log_file_dir); + bool startFileLogger(); protected: // ROS node handle in the correct receiver namespace. diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index 83f73ccf..7c1e6954 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -27,20 +27,7 @@ bool ReceiverBaseStation::init() { ros::NodeHandle nh_private("~"); auto log_to_file = nh_private.param("log_observations_to_file", false); if (log_to_file) { - std::string log_file_dir = - nh_private.param("log_file_dir", "./"); - bool use_date_time_as_filename = true; - nh_private.param("use_date_time_as_filename", use_date_time_as_filename, - use_date_time_as_filename); - std::string filename = "observations.sbp"; - if (use_date_time_as_filename) { - std::time_t t = std::time(nullptr); - std::tm tm = *std::localtime(&t); - std::stringstream time_ss; - time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; - filename = time_ss.str(); - } - ReceiverRos::startFileLogger(log_file_dir + '/' + filename); + ReceiverRos::startFileLogger(); } return true; diff --git a/piksi_multi_cpp/src/receiver/receiver_position.cc b/piksi_multi_cpp/src/receiver/receiver_position.cc index 8f698bfd..d910c332 100644 --- a/piksi_multi_cpp/src/receiver/receiver_position.cc +++ b/piksi_multi_cpp/src/receiver/receiver_position.cc @@ -26,20 +26,7 @@ bool ReceiverPosition::init() { // Start file logger if requested auto log_to_file = nh_private.param("log_observations_to_file", false); if (log_to_file) { - std::string log_file_dir = - nh_private.param("log_file_dir", "./"); - bool use_date_time_as_filename = true; - nh_private.param("use_date_time_as_filename", use_date_time_as_filename, - use_date_time_as_filename); - std::string filename = "observations.sbp"; - if (use_date_time_as_filename) { - std::time_t t = std::time(nullptr); - std::tm tm = *std::localtime(&t); - std::stringstream time_ss; - time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; - filename = time_ss.str(); - } - ReceiverRos::startFileLogger(log_file_dir + '/' + filename); + ReceiverRos::startFileLogger(); } return true; diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 0430be3d..df1be452 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -41,18 +41,49 @@ bool ReceiverRos::init() { return true; } -void ReceiverRos::startFileLogger(const std::string& log_file_dir) { +bool ReceiverRos::startFileLogger() { // Create ephemeris callbacks eph_cbs_ = std::make_unique(nh_, state_); + ros::NodeHandle nh_private("~"); + // Per default observations are stored in .ros with current date & time + // prefixed with "_" so that observations are stored in + // different files if multiple receivers connected + auto obs_dir = nh_private.param("log_dir", "./"); + std::string obs_filename; + std::size_t receiver_type_pos = nh_.getNamespace().find_last_of("/"); + std::string obs_prefix = + nh_.getNamespace().substr(receiver_type_pos + 1) + "_"; + + bool use_custom_filename = nh_private.hasParam("observation_filename"); + if (!use_custom_filename) { + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + std::stringstream time_ss; + time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; + obs_filename = obs_prefix + time_ss.str(); + } else { + // Get filename if custom name set + nh_private.getParam("observation_filename", obs_filename); + obs_filename = obs_prefix + obs_filename; + } + auto logger = std::make_shared(); - ROS_WARN_STREAM(logger->open(log_file_dir)); + if (!logger->open(obs_dir + "/" + obs_filename)) { + ROS_WARN_STREAM( + "Could not open logger file. Observation are not being stored!"); + return false; + } // Add logger as listener to callbacks - obs_cbs_->addObservationCallbackListener(CBtoRawObsConverter::createFor( - logger, sbp_sender_id_)); + obs_cbs_->addObservationCallbackListener( + CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); eph_cbs_->addObservationCallbackListener( CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); + + return true; + + // TODO: add service for start stopping logger here!! } std::vector ReceiverRos::getVectorParam( From e9a0fdfc7f07afe3506eed36d88d8261ba81a4ac Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 14 Sep 2020 12:25:18 +0200 Subject: [PATCH 12/17] Fixed attitude receiver not logging sbp messages --- .../include/piksi_multi_cpp/receiver/receiver_ros.h | 2 +- piksi_multi_cpp/src/receiver/receiver_base_station.cc | 7 ------- piksi_multi_cpp/src/receiver/receiver_position.cc | 6 ------ piksi_multi_cpp/src/receiver/receiver_ros.cc | 10 ++++++++-- 4 files changed, 9 insertions(+), 16 deletions(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h index 30a9999a..dad62f37 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h @@ -25,7 +25,7 @@ class ReceiverRos : public SettingsIo { // set up user ID bool init() override; - bool startFileLogger(); + bool startFileLogger(const ros::NodeHandle &nh_private); protected: // ROS node handle in the correct receiver namespace. diff --git a/piksi_multi_cpp/src/receiver/receiver_base_station.cc b/piksi_multi_cpp/src/receiver/receiver_base_station.cc index 7c1e6954..20196ab5 100644 --- a/piksi_multi_cpp/src/receiver/receiver_base_station.cc +++ b/piksi_multi_cpp/src/receiver/receiver_base_station.cc @@ -23,13 +23,6 @@ bool ReceiverBaseStation::init() { ROS_INFO("UDP corrections sender ID: 0x%.4X", sbp_sender_id_); setupUDPSenders(); - // Start file logger if requested - ros::NodeHandle nh_private("~"); - auto log_to_file = nh_private.param("log_observations_to_file", false); - if (log_to_file) { - ReceiverRos::startFileLogger(); - } - return true; } diff --git a/piksi_multi_cpp/src/receiver/receiver_position.cc b/piksi_multi_cpp/src/receiver/receiver_position.cc index d910c332..ebc35205 100644 --- a/piksi_multi_cpp/src/receiver/receiver_position.cc +++ b/piksi_multi_cpp/src/receiver/receiver_position.cc @@ -23,12 +23,6 @@ bool ReceiverPosition::init() { ros::NodeHandle nh_private("~"); udp_receiver_.start(nh_private.param("udp_observation_port", 26078)); - // Start file logger if requested - auto log_to_file = nh_private.param("log_observations_to_file", false); - if (log_to_file) { - ReceiverRos::startFileLogger(); - } - return true; } diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index df1be452..16f4603a 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -26,6 +26,13 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) // Create observation callbacks obs_cbs_ = std::make_unique(nh, state_); + + // Start file logger if requested + ros::NodeHandle nh_private("~"); + auto log_to_file = nh_private.param("log_observations_to_file", false); + if (log_to_file) { + startFileLogger(nh_private); + } } bool ReceiverRos::init() { @@ -41,11 +48,10 @@ bool ReceiverRos::init() { return true; } -bool ReceiverRos::startFileLogger() { +bool ReceiverRos::startFileLogger(const ros::NodeHandle &nh_private) { // Create ephemeris callbacks eph_cbs_ = std::make_unique(nh_, state_); - ros::NodeHandle nh_private("~"); // Per default observations are stored in .ros with current date & time // prefixed with "_" so that observations are stored in // different files if multiple receivers connected From 2f966e59f5c66e49da7a901aada4bba40f908c8f Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 14 Sep 2020 14:59:44 +0200 Subject: [PATCH 13/17] Added stop and start service server for observation logger Can start and stop logger for attitude and position receiver independently. If a custom name is set, the file is not overwritten when logging restarted but rather a number is added as appendix preventing accidental loss of observation files. --- .../observations/file_observation_logger.h | 8 ++- .../piksi_multi_cpp/receiver/receiver_ros.h | 11 +++- .../observations/file_observation_logger.cc | 17 ++++- piksi_multi_cpp/src/receiver/receiver_ros.cc | 63 ++++++++++++++----- 4 files changed, 75 insertions(+), 24 deletions(-) diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h b/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h index 546602f2..7feb4ea2 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/observations/file_observation_logger.h @@ -17,17 +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. - + // mutex to lock file when writing std::mutex file_mtx_; - - }; } // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h index dad62f37..73eb5da6 100644 --- a/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h +++ b/piksi_multi_cpp/include/piksi_multi_cpp/receiver/receiver_ros.h @@ -2,10 +2,12 @@ #define PIKSI_MULTI_CPP_RECEIVER_RECEIVER_ROS_H_ #include +#include #include #include #include #include +#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" @@ -25,11 +27,16 @@ class ReceiverRos : public SettingsIo { // set up user ID bool init() override; - bool startFileLogger(const ros::NodeHandle &nh_private); + // 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}; @@ -46,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 obs_logger_; private: // Relaying all SBP messages. Common for all receivers. diff --git a/piksi_multi_cpp/src/observations/file_observation_logger.cc b/piksi_multi_cpp/src/observations/file_observation_logger.cc index 9a751ba5..be4b092a 100644 --- a/piksi_multi_cpp/src/observations/file_observation_logger.cc +++ b/piksi_multi_cpp/src/observations/file_observation_logger.cc @@ -4,19 +4,30 @@ 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 @@ -29,4 +40,4 @@ void FileObservationLogger::insertObservation( FileObservationLogger::~FileObservationLogger() { close(); } -} \ No newline at end of file +} // namespace piksi_multi_cpp diff --git a/piksi_multi_cpp/src/receiver/receiver_ros.cc b/piksi_multi_cpp/src/receiver/receiver_ros.cc index 16f4603a..8335e23f 100644 --- a/piksi_multi_cpp/src/receiver/receiver_ros.cc +++ b/piksi_multi_cpp/src/receiver/receiver_ros.cc @@ -3,7 +3,6 @@ #include // SBP message definitions. -#include #include #include #include "piksi_multi_cpp/sbp_callback_handler/position_sampler.h" @@ -31,7 +30,10 @@ ReceiverRos::ReceiverRos(const ros::NodeHandle& nh, const Device::Ptr& device) ros::NodeHandle nh_private("~"); auto log_to_file = nh_private.param("log_observations_to_file", false); if (log_to_file) { - startFileLogger(nh_private); + startFileLogger(); + start_stop_logger_srv_ = + nh_.advertiseService("start_stop_obs_logger", + &ReceiverRos::startStopFileLoggerCallback, this); } } @@ -48,14 +50,12 @@ bool ReceiverRos::init() { return true; } -bool ReceiverRos::startFileLogger(const ros::NodeHandle &nh_private) { - // Create ephemeris callbacks - eph_cbs_ = std::make_unique(nh_, state_); - +bool ReceiverRos::startFileLogger() { // Per default observations are stored in .ros with current date & time // prefixed with "_" so that observations are stored in // different files if multiple receivers connected - auto obs_dir = nh_private.param("log_dir", "./"); + ros::NodeHandle nh_private("~"); + auto obs_dir = nh_private.param("log_dir", "."); std::string obs_filename; std::size_t receiver_type_pos = nh_.getNamespace().find_last_of("/"); std::string obs_prefix = @@ -66,30 +66,59 @@ bool ReceiverRos::startFileLogger(const ros::NodeHandle &nh_private) { std::time_t t = std::time(nullptr); std::tm tm = *std::localtime(&t); std::stringstream time_ss; - time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S") << ".sbp"; + time_ss << std::put_time(&tm, "%Y_%d_%m_%H_%M_%S"); obs_filename = obs_prefix + time_ss.str(); } else { // Get filename if custom name set nh_private.getParam("observation_filename", obs_filename); obs_filename = obs_prefix + obs_filename; } + ROS_INFO_STREAM("Logging observations to file: \n\"" << obs_dir << "/" << obs_filename << ".sbp\"."); + + // Initialize logger if not already done + if (!obs_logger_) { + obs_logger_ = std::make_shared(); + // Create ephemeris callbacks and add listeners to logger + eph_cbs_ = std::make_unique(nh_, state_); + // Add logger as listener to callbacks + obs_cbs_->addObservationCallbackListener( + CBtoRawObsConverter::createFor(obs_logger_, sbp_sender_id_)); + eph_cbs_->addObservationCallbackListener( + CBtoRawObsConverter::createFor(obs_logger_, sbp_sender_id_)); + } - auto logger = std::make_shared(); - if (!logger->open(obs_dir + "/" + obs_filename)) { + // start logging + if (obs_logger_->open(obs_dir + "/" + obs_filename) != 0) { ROS_WARN_STREAM( "Could not open logger file. Observation are not being stored!"); return false; } - // Add logger as listener to callbacks - obs_cbs_->addObservationCallbackListener( - CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); - eph_cbs_->addObservationCallbackListener( - CBtoRawObsConverter::createFor(logger, sbp_sender_id_)); - return true; +} - // TODO: add service for start stopping logger here!! +bool ReceiverRos::startStopFileLoggerCallback( + std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { + + if (req.data && !obs_logger_->isLogging()) { + // Start logger if not running + if (startFileLogger()) { + res.success = true; + res.message = "Logger succesfully started"; + } else { + res.success = false; + res.message = "Failed to start logger."; + } + } else if (!req.data && obs_logger_->isLogging()) { + // Stop logger if runnning + obs_logger_->close(); + res.success = true; + res.message = "Stopped logging."; + } else { + res.success = false; + res.message = "Service call failed."; + } + return true; } std::vector ReceiverRos::getVectorParam( From d5b3e86a05c2fc33cebbedfa6d83345f41ca1def Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 14 Sep 2020 17:13:20 +0200 Subject: [PATCH 14/17] Added script to convert from sbp binary file to rinex ppk_solution. Also added python script which takes the solution and adds them to an existing Rosbag. --- utils/piksi_rtklib_postp/pos2bag.py | 225 ++++++++++++++++++++++++++++ utils/piksi_rtklib_postp/sbp2pos.sh | 98 ++++++++++++ 2 files changed, 323 insertions(+) create mode 100644 utils/piksi_rtklib_postp/pos2bag.py create mode 100755 utils/piksi_rtklib_postp/sbp2pos.sh diff --git a/utils/piksi_rtklib_postp/pos2bag.py b/utils/piksi_rtklib_postp/pos2bag.py new file mode 100644 index 00000000..9d5e8741 --- /dev/null +++ b/utils/piksi_rtklib_postp/pos2bag.py @@ -0,0 +1,225 @@ +import rosbag +import rospy +import sys +import csv + +from datetime import datetime +import time +import calendar +import numpy as np +import matplotlib.pyplot as plt +from geometry_msgs.msg import PointStamped + +import geotf + + +class pos2bag(object): + def __init__(self, observed_pos_dir, bag_dir): + self.bag_dir = bag_dir + self.observed_pos_dir = observed_pos_dir + self.crop_ppk_solution_to_bag_ = False + self.pos_ecef_topic = "/kiwi/piksi/position_receiver_0/ros/pos_ecef_cov" + self.ppk_solution_topic = "/kiwi/ppk_position" + self.reference_pos = np.array([47.234219638, 7.682116020, 477.5765]) + + self.global_frame = "llh_wgs84" + self.base_frame = "enu_base" + self.obs_timestamps = [] + self.ros_msg_time = [] + self.obs_position = np.empty((0, 3), float) + self.gc = geotf.GeodeticConverter() + + self.load_pos_file() + self.initGeodeticConverter() + + def load_pos_file(self): + # takes file from observed pos and loads and stores it in a somewhat smart manner + with open(self.observed_pos_dir, "r") as file: + reader = csv.reader(file, delimiter=" ", skipinitialspace=True) + + # Run through header + in_header = True + observation_time = None + while in_header: + header_line = next(reader) + # Store which time is being used + if len(header_line) > 1 and header_line[1] == "obs": + observation_time = header_line[6] + if header_line[0] != "%": + in_header = False + + # Abort if not UTC time used + if observation_time != "UTC": + print( + "ERROR: Observation time has to be in UTC. Anything else is not supported." + ) + sys.exit(1) + + # load content + time_format_str = "%Y/%m/%d %H:%M:%S.%f-%Z" + for row in reader: + self.obs_timestamps.append( + datetime.strptime(row[0] + " " + row[1] + "-UTC", time_format_str) + ) + self.obs_position = np.append( + self.obs_position, + np.array([[float(row[2]), float(row[3]), float(row[4])]]), + axis=0, + ) + + def initGeodeticConverter(self): + if not self.gc.addFrameByENUOrigin( + self.base_frame, + self.reference_pos[0], + self.reference_pos[1], + self.reference_pos[2], + ): + print("Error: Could not add Frame: " + self.base_frame + " by ENU origin.") + sys.exit(1) + + if not self.gc.addFrameByGCSCode(self.global_frame, "WGS84"): + print("Error: Could not add Frame: " + self.global_frame + " by GCSCode.") + sys.exit(1) + + if self.gc.canConvert(self.global_frame, self.base_frame): + return True + else: + print( + "Error: Loading of frames failed. Cannot convert between global and local frame" + ) + sys.exit(1) + + def add_observations_to_bag(self, bag_suffix, outbag_dir=None): + if bag_suffix == "": + bag_suffix = "_with_observations" + if outbag_dir == None: + outbag_dir = self.bag_dir.split(".")[0] + bag_suffix + else: + inbag_name = self.bag_dir.split("/")[-1].split(".")[0] + outbag_dir = outbag_dir + "/" + inbag_name + bag_suffix + + # Opening dem bags + inbag = rosbag.Bag(self.bag_dir) + outbag = rosbag.Bag(outbag_dir + ".bag", "w") + + print("Computing time difference") + time_diff = np.empty(0, float) + for topic, msg, t in inbag.read_messages(topics=[self.pos_ecef_topic]): + time_diff = np.append(time_diff, msg.header.stamp.to_sec() - t.to_sec()) + + if time_diff.size == 0: + print("Error: No time difference received from bag.") + sys.exit(1) + # Skip first value because its akwardly huge and skews mean + time_diff = time_diff[1:] + mean_time_offset = np.mean(time_diff) + + # Convert timestamps to UTC + obs_ros_time = [] + for dt_time in self.obs_timestamps: + # Convert to utc and add bag offset + dt_time_utc_offset = ( + calendar.timegm(dt_time.timetuple()) + + dt_time.microsecond / 1e6 + + mean_time_offset + ) + # Make it ros time object + obs_ros_time.append(rospy.Time.from_sec(dt_time_utc_offset)) + + # Crop observations to bag start and stop time + if self.crop_ppk_solution_to_bag_: + # Store start & end time of bag + inbag_t_start = inbag.get_start_time() + inbag_t_end = inbag.get_end_time() + obs_ros_time_cropped = [] + obs_position_cropped = np.empty((0, 3), float) + + # Cropping info.. + print("Cropping Observations to bag start and end time.") + print( + "Bag start/end time:\t" + str(inbag_t_start) + "\t" + str(inbag_t_end) + ) + print( + "Obs start/end time:\t" + + str(obs_ros_time[0]) + + "\t" + + str(obs_ros_time[-1]) + ) + # # Get first and last timestamp of observation + # obs_t_start = ( + # calendar.timegm(self.obs_timestamps[0].timetuple()) + # + self.obs_timestamps[0].microsecond / 1e6 + # + mean_time_offset + # ) + # obs_t_end = ( + # calendar.timegm(self.obs_timestamps[-1].timetuple()) + # + self.obs_timestamps[-1].microsecond / 1e6 + # + mean_time_offset + # ) + + # Crop messages + for timestamp, g_position in zip(obs_ros_time, self.obs_position): + if timestamp.to_sec() >= inbag_t_start and timestamp.to_sec() <= inbag_t_end: + obs_ros_time_cropped.append(timestamp) + obs_position_cropped = np.append( + obs_position_cropped, [g_position], axis=0 + ) + + if len(obs_ros_time_cropped) == 0 or len(obs_position_cropped) == 0: + print("ERROR: Observations seem not to overlap with bag times.") + sys.exit(1) + else: + # Don't change size of observations + print( + "WARNING: Observation messages are not being cropped to rosbag start & end time." + ) + obs_ros_time_cropped = obs_ros_time + obs_position_cropped = self.obs_position + + # Writing the topic from inbag + print("Copying all topics to new bag") + for topic, msg, t in inbag.read_messages(): + outbag.write(topic, msg, t) + + print("Adding robot positions to new bag") + msg_idx = 0 + for obs_timestamp, g_position in zip( + obs_ros_time_cropped, obs_position_cropped + ): + # Convert from global to base frame + b_position = self.gc.convert(self.global_frame, g_position, self.base_frame) + + msg = PointStamped() + msg.header.frame_id = self.base_frame + msg.header.seq = msg_idx + + msg.header.stamp = obs_timestamp + + msg.point.x = b_position[0] + msg.point.y = b_position[1] + msg.point.z = b_position[2] + outbag.write(self.ppk_solution_topic, msg, obs_timestamp) + msg_idx += 1 + + outbag.close() + + plt.plot( + np.arange(0, len(time_diff), 1), + time_diff, + "b", + np.arange(0, len(time_diff), 1), + np.ones(len(time_diff)) * mean_time_offset, + "r", + ) + plt.grid(True) + plt.show() + + +if __name__ == "__main__": + dir_arg = sys.argv[1] + sol_arg = sys.argv[2] + pos2bag = pos2bag(sol_arg, dir_arg) + if len(sys.argv) == 4: + pos2bag.add_observations_to_bag("_with_ppk_obs", outbag_dir=sys.argv[3]) + else: + pos2bag.add_observations_to_bag("_with_ppk_obs") diff --git a/utils/piksi_rtklib_postp/sbp2pos.sh b/utils/piksi_rtklib_postp/sbp2pos.sh new file mode 100755 index 00000000..51728fed --- /dev/null +++ b/utils/piksi_rtklib_postp/sbp2pos.sh @@ -0,0 +1,98 @@ +#!/bin/bash + +# Dependencies: +# - RINEX (ideally newest version for best results) +# - SBP2RINEX (from swift repos) +# - pos2bag python script +# [[- GDAL with newest version PROJ (python) +# - sqlite3 +# --> https://github.com/OSGeo/GDAL.git ; https://github.com/OSGeo/PROJ/tree/master ; https://gis.stackexchange.com/questions/317109/build-gdal-with-proj-version-6 ; +# ./configure --with-python --with-proj +# https://proj.org/install.html ]]] +# - geotf_python , numpy_eigen, catkin_boost_python_buildtool + +# Script needs as argument directory with rosbag, base station correction and rover correction as sbp binaries and corresponding configuration file +# Base station correction file needs to have the prefix: "base_" + +# Script: +# 1. converts sbp binaries to rinex and then generates a PPK solution using the base and rover observation +# 2. generates KML file which can be uploaded to swisstopo maps [https://map.geo.admin.ch/] to check for results (markers are color coded based on solution type) +# (Green - Fix ; Yellow - Float ; Blue - No DGPS) +# 4. Adds ppk solution to new rosbag (has appendix _with_obs). The ppk solution can be converted to a reference frame if one is specified in script + +# Configuration info: +# - needs to be in xyz --> pos2bag converts solution to enu later on. + +# Make script fail if one of the steps fail +set -e + +# TODO: Check if all files exist in dir and naming is correct!!!!! + +# Font colors +BLUE='\033[0;36m' +MAGENTA='\033[0;35m' +NC='\033[0m' # No Color + +# File and directory variables +OBSERVATION_DIR=${1%/} +SOLUTION_DIR=$OBSERVATION_DIR/ppk_solution +RNX_DIR=$SOLUTION_DIR/rnx_files +DATA_NAME="" +SOL_FILE_NAME="" +COMP_SOLUTION=true + +# Create solution directories if they don't exist +if [[ ! -d $SOLUTION_DIR ]]; then + mkdir $SOLUTION_DIR +fi +if [[ ! -d $RNX_DIR ]]; then + mkdir $RNX_DIR +fi + +# Extract name of dataset from directory name +DATA_NAME="$(basename $OBSERVATION_DIR)" +SOL_FILE_NAME=ppk_$DATA_NAME + +for csv_file in $SOLUTION_DIR/*.csv; do + if [[ -f "${csv_file}" ]]; then + echo -e "${MAGENTA}\nFound csv file in solution directory. \nRecompute solution? (previous files will be lost) [Y/n] ${NC}" + read user_input + if [[ $user_input == "n" || $user_input == "N" ]]; then + COMP_SOLUTION=false + fi + fi +done + +if $COMP_SOLUTION; then + echo -e "${BLUE}\n=== Converting SBP Binaries to RINEX === \n ${NC}" + # Convert from SBP to RINEX + for binaries in "$OBSERVATION_DIR"/*.sbp; do + sbp2rinex $binaries -d $RNX_DIR + done + + echo -e "${BLUE}\n=== Creating PPK Solution === \n ${NC}" + rnx2rtkp $RNX_DIR/$DATA_NAME.obs $RNX_DIR/$DATA_NAME.nav $RNX_DIR/base_$DATA_NAME.obs $RNX_DIR/base_$DATA_NAME.nav -k $OBSERVATION_DIR/*.conf -o $SOLUTION_DIR/$SOL_FILE_NAME.csv + + # CREATE KML AND CHANGE TO CORRECT MARKERS! + echo -e "${BLUE}\n=== Creating KML File from PPK Solution === \n ${NC}" + pos2kml $SOLUTION_DIR/$SOL_FILE_NAME.csv + # Change solution to correct markers as changing the color does not work + # Marker size + perl -i -pe's{.*?}{++$n == 1 ? "0.5" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml + perl -i -pe's{.*?}{++$n == 2 ? "0.4" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml + perl -i -pe's{.*?}{++$n == 3 ? "0.4" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml + # Make no DGPS (blue) bigger so that it stands out in map + perl -i -pe's{.*?}{++$n == 4 ? "0.7" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml + # Set correct marker type + perl -i -pe's{pal2(.*?)png}{++$n == 2 ? "paddle/grn-blank-lv.png" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml # Fixed - Green + perl -i -pe's{pal2(.*?)png}{++$n == 2 ? "paddle/ylw-blank-lv.png" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml # Float - Yellow + perl -i -pe's{pal2(.*?)png}{++$n == 2 ? "paddle/blu-blank-lv.png" : $&}ge' $SOLUTION_DIR/$SOL_FILE_NAME.kml # No DGPS (single) - Blue +fi + +echo -e "${MAGENTA}\nWould you like to add PPK Solution to ROSbag? [Y/n] ${NC}" +read user_input +if [[ $user_input == "Y" || $user_input == "y" ]]; then + echo -e "${BLUE}\n=== Starting Python script to add ppk solution to ROS Bag === \n ${NC}" + ROSBAG=$OBSERVATION_DIR/$DATA_NAME.bag + python pos2bag.py $ROSBAG $SOLUTION_DIR/$SOL_FILE_NAME.csv $SOLUTION_DIR +fi From eccb6f0873f3e9c513ae8ff44af5bfe6e894985d Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Mon, 14 Sep 2020 18:47:38 +0200 Subject: [PATCH 15/17] Added fallback to Swisspos observations if base_*.sbp not found Can be used if trying to get correct position for base station for example. --- utils/piksi_rtklib_postp/pos2bag.py | 1 + utils/piksi_rtklib_postp/sbp2pos.sh | 31 ++++++++++++++++++++++++++--- 2 files changed, 29 insertions(+), 3 deletions(-) diff --git a/utils/piksi_rtklib_postp/pos2bag.py b/utils/piksi_rtklib_postp/pos2bag.py index 9d5e8741..f550697b 100644 --- a/utils/piksi_rtklib_postp/pos2bag.py +++ b/utils/piksi_rtklib_postp/pos2bag.py @@ -103,6 +103,7 @@ def add_observations_to_bag(self, bag_suffix, outbag_dir=None): outbag = rosbag.Bag(outbag_dir + ".bag", "w") print("Computing time difference") + # TODO(clanegge): Add option to add binaries in rosbag time and not utc time time_diff = np.empty(0, float) for topic, msg, t in inbag.read_messages(topics=[self.pos_ecef_topic]): time_diff = np.append(time_diff, msg.header.stamp.to_sec() - t.to_sec()) diff --git a/utils/piksi_rtklib_postp/sbp2pos.sh b/utils/piksi_rtklib_postp/sbp2pos.sh index 51728fed..4486241c 100755 --- a/utils/piksi_rtklib_postp/sbp2pos.sh +++ b/utils/piksi_rtklib_postp/sbp2pos.sh @@ -31,9 +31,10 @@ set -e # Font colors BLUE='\033[0;36m' MAGENTA='\033[0;35m' +RED='\033[0;31m' NC='\033[0m' # No Color -# File and directory variables +# File and directory variables. Observation directory needs to be passed as first argument OBSERVATION_DIR=${1%/} SOLUTION_DIR=$OBSERVATION_DIR/ppk_solution RNX_DIR=$SOLUTION_DIR/rnx_files @@ -53,23 +54,47 @@ fi DATA_NAME="$(basename $OBSERVATION_DIR)" SOL_FILE_NAME=ppk_$DATA_NAME +# Check if solution was already computed for csv_file in $SOLUTION_DIR/*.csv; do if [[ -f "${csv_file}" ]]; then echo -e "${MAGENTA}\nFound csv file in solution directory. \nRecompute solution? (previous files will be lost) [Y/n] ${NC}" read user_input if [[ $user_input == "n" || $user_input == "N" ]]; then COMP_SOLUTION=false + else + # whipe directory + rm -r $SOLUTION_DIR + mkdir $SOLUTION_DIR + mkdir $RNX_DIR fi fi done +# Convert from SBP to RINEX if $COMP_SOLUTION; then + # If SBP base file found then this will be used instead of swisspos observation files + if [[ -f "$OBSERVATION_DIR/base_$DATA_NAME.sbp" ]]; then + LOOK_FOR_SWISSPOS_FILES=false + else + LOOK_FOR_SWISSPOS_FILES=true + fi + echo -e "${BLUE}\n=== Converting SBP Binaries to RINEX === \n ${NC}" - # Convert from SBP to RINEX - for binaries in "$OBSERVATION_DIR"/*.sbp; do + for binaries in "$OBSERVATION_DIR"/*$DATA_NAME.sbp; do sbp2rinex $binaries -d $RNX_DIR done + if $LOOK_FOR_SWISSPOS_FILES; then + if ls $OBSERVATION_DIR/*.20o &>/dev/null && ls $OBSERVATION_DIR/*.20n &>/dev/null; then + echo -e "${MAGENTA}\nFound SWISSPOS observation files. Using these to compute PPK Solution.\n ${NC}" + cp $OBSERVATION_DIR/*.20o $RNX_DIR/base_$DATA_NAME.obs + cp $OBSERVATION_DIR/*.20n $RNX_DIR/base_$DATA_NAME.nav + else + echo -e "${RED}\nCould not find any base observation files. Aborting.\n ${NC}" + exit 1 + fi + fi + echo -e "${BLUE}\n=== Creating PPK Solution === \n ${NC}" rnx2rtkp $RNX_DIR/$DATA_NAME.obs $RNX_DIR/$DATA_NAME.nav $RNX_DIR/base_$DATA_NAME.obs $RNX_DIR/base_$DATA_NAME.nav -k $OBSERVATION_DIR/*.conf -o $SOLUTION_DIR/$SOL_FILE_NAME.csv From 32ec9e0f2da9926fc403b1123067ba52145a04da Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Wed, 7 Jul 2021 16:16:38 +0200 Subject: [PATCH 16/17] Fix: Script could not find swisspos files eventhough they where in folder Now user has to specify if reference files should be loaded from an sbp binary or from swisspos files. --- utils/piksi_rtklib_postp/sbp2pos.sh | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/utils/piksi_rtklib_postp/sbp2pos.sh b/utils/piksi_rtklib_postp/sbp2pos.sh index 4486241c..481750b7 100755 --- a/utils/piksi_rtklib_postp/sbp2pos.sh +++ b/utils/piksi_rtklib_postp/sbp2pos.sh @@ -72,27 +72,24 @@ done # Convert from SBP to RINEX if $COMP_SOLUTION; then - # If SBP base file found then this will be used instead of swisspos observation files - if [[ -f "$OBSERVATION_DIR/base_$DATA_NAME.sbp" ]]; then - LOOK_FOR_SWISSPOS_FILES=false - else - LOOK_FOR_SWISSPOS_FILES=true - fi - echo -e "${BLUE}\n=== Converting SBP Binaries to RINEX === \n ${NC}" for binaries in "$OBSERVATION_DIR"/*$DATA_NAME.sbp; do sbp2rinex $binaries -d $RNX_DIR done + echo -e "${MAGENTA}\nWould you like to use SwissPos files as reference? (If not an SBP file prefixed with 'base_' needs to be in directory) [Y/n] ${NC}" + read user_input + if [[ $user_input == "Y" || $user_input == "y" ]]; then + LOOK_FOR_SWISSPOS_FILES=true + else + LOOK_FOR_SWISSPOS_FILES=false + fi + + # Copy SwissPos to correct directory if $LOOK_FOR_SWISSPOS_FILES; then - if ls $OBSERVATION_DIR/*.20o &>/dev/null && ls $OBSERVATION_DIR/*.20n &>/dev/null; then echo -e "${MAGENTA}\nFound SWISSPOS observation files. Using these to compute PPK Solution.\n ${NC}" - cp $OBSERVATION_DIR/*.20o $RNX_DIR/base_$DATA_NAME.obs - cp $OBSERVATION_DIR/*.20n $RNX_DIR/base_$DATA_NAME.nav - else - echo -e "${RED}\nCould not find any base observation files. Aborting.\n ${NC}" - exit 1 - fi + cp $OBSERVATION_DIR/*.2*o $RNX_DIR/base_$DATA_NAME.obs + cp $OBSERVATION_DIR/*.2*n $RNX_DIR/base_$DATA_NAME.nav fi echo -e "${BLUE}\n=== Creating PPK Solution === \n ${NC}" From c699034b0978845baac18e53dfa08599b7ae3670 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Wed, 7 Jul 2021 16:19:48 +0200 Subject: [PATCH 17/17] Added script that computes mean position of base station --- .../compute_base_mean_position.py | 46 +++++++++++++++++++ utils/piksi_rtklib_postp/sbp2pos.sh | 7 +++ 2 files changed, 53 insertions(+) create mode 100644 utils/piksi_rtklib_postp/compute_base_mean_position.py diff --git a/utils/piksi_rtklib_postp/compute_base_mean_position.py b/utils/piksi_rtklib_postp/compute_base_mean_position.py new file mode 100644 index 00000000..99d0e59e --- /dev/null +++ b/utils/piksi_rtklib_postp/compute_base_mean_position.py @@ -0,0 +1,46 @@ +#!/usr/bin/python +import sys +import csv + +import numpy as np + +if __name__ == "__main__": + file_dir = sys.argv[1] + lat = [] + lon = [] + height = [] + + with open(file_dir, "r") as f: + reader = csv.reader(f, delimiter=" ", skipinitialspace=True) + # Run through header + in_header = True + observation_time = None + while in_header: + header_line = next(reader) + # Store which time is being used + if len(header_line) > 1 and header_line[1] == "obs": + observation_time = header_line[6] + if header_line[0] != "%": + in_header = False + + for row in reader: + # Only append postion if in Fix mode! + if row[5] == "1": + lat.append(float(row[2])) + lon.append(float(row[3])) + height.append(float(row[4])) + + if len(lat) == 0: + print( + "Could not compute mean position. No solutions with fix mode found in file!" + ) + sys.exit(1) + + # Compute mean: + mean_lat = sum(lat) / len(lat) + mean_lon = sum(lon) / len(lon) + mean_height = sum(height) / len(height) + + print( + f"Average base position:\nLatitude:\t{mean_lat}\nLongitude:\t{mean_lon}\nHeight:\t\t{mean_height}" + ) diff --git a/utils/piksi_rtklib_postp/sbp2pos.sh b/utils/piksi_rtklib_postp/sbp2pos.sh index 481750b7..75672380 100755 --- a/utils/piksi_rtklib_postp/sbp2pos.sh +++ b/utils/piksi_rtklib_postp/sbp2pos.sh @@ -118,3 +118,10 @@ if [[ $user_input == "Y" || $user_input == "y" ]]; then ROSBAG=$OBSERVATION_DIR/$DATA_NAME.bag python pos2bag.py $ROSBAG $SOLUTION_DIR/$SOL_FILE_NAME.csv $SOLUTION_DIR fi + +echo -e "${MAGENTA}\nWould you like to compute the mean position of the base station? [Y/n] ${NC}" +read user_input +if [[ $user_input == "Y" || $user_input == "y" ]]; then + echo -e "${BLUE}\n=== Computing mean base position from values when in FIX mode === \n ${NC}" + python compute_base_mean_position.py $SOLUTION_DIR/$SOL_FILE_NAME.csv +fi \ No newline at end of file