new subscribers for raw_data

CMakeLists.txt

@@ -16,12 +16,10 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   dynamic_reconfigure
   wolf_ros_node
+  novatel_oem7_msgs
 )
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-# find_package(Ceres REQUIRED)
-# find_package(Eigen3 REQUIRED)
 find_package(wolf REQUIRED)
 find_package(wolfgnss REQUIRED)
 
@@ -141,7 +139,9 @@ add_library(subscriber_${PROJECT_NAME}
   src/subscriber_gnss.cpp
   src/subscriber_gnss_fix.cpp
   src/subscriber_gnss_tdcp.cpp
-  src/subscriber_gnss_ublox.cpp)
+  src/subscriber_gnss_receiver.cpp
+  src/subscriber_gnss_ublox.cpp
+  src/subscriber_gnss_novatel.cpp)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -163,6 +163,8 @@ add_library(subscriber_${PROJECT_NAME}
 ## same as for the library above
 #add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg)
 #add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
+add_dependencies(subscriber_${PROJECT_NAME} iri_gnss_msgs_generate_messages_cpp)
+add_dependencies(subscriber_${PROJECT_NAME} novatel_oem7_msgs_generate_messages_cpp)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(subscriber_${PROJECT_NAME}

include/subscriber_gnss_novatel.h

+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/yaml/parser_yaml.h>
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include <core/utils/params_server.h>
+#include <gnss/capture/capture_gnss.h>
+#include <gnss/sensor/sensor_gnss.h>
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include "novatel_oem7_msgs/Oem7RawMsg.h"
+
+/**************************
+ *    WOLF-ROS includes   *
+ **************************/
+#include "subscriber_gnss_receiver.h"
+
+/**************************
+ *     Other includes     *
+ **************************/
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/receivers/novatel_raw.h"
+
+using namespace wolf;
+
+class SubscriberGnssNovatel : public SubscriberGnssReceiver
+{
+    public:
+        // Constructor
+        SubscriberGnssNovatel(const SensorBasePtr& sensor_ptr);
+
+        virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic);
+
+        void callback(const novatel_oem7_msgs::Oem7RawMsg& msg);
+
+        static std::shared_ptr<Subscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr);
+
+    protected:
+
+        GnssUtils::ReceiverRawAbstractPtr receiver_;
+};
+WOLF_REGISTER_SUBSCRIBER(SubscriberGnssNovatel)

include/subscriber_gnss_receiver.h

+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/yaml/parser_yaml.h>
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include <core/utils/params_server.h>
+#include <gnss/capture/capture_gnss.h>
+#include <gnss/sensor/sensor_gnss.h>
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+
+/**************************
+ *    WOLF-ROS includes   *
+ **************************/
+#include "subscriber.h"
+
+/**************************
+ *     Other includes     *
+ **************************/
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/receiver_raw_base.h"
+
+using namespace wolf;
+
+class SubscriberGnssReceiver : public Subscriber
+{
+    public:
+        // Constructor
+        SubscriberGnssReceiver(const SensorBasePtr& sensor_ptr, GnssUtils::ReceiverRawAbstractPtr _receiver);
+
+    protected:
+
+        void createCaptureAndProcess();
+
+        GnssUtils::ReceiverRawAbstractPtr receiver_;
+};

include/subscriber_gnss_ublox.h

@@ -17,18 +17,18 @@
 /**************************
  *    WOLF-ROS includes   *
  **************************/
-#include "subscriber.h"
+#include "subscriber_gnss_receiver.h"
 
 /**************************
  *     Other includes     *
  **************************/
 #include "gnss_utils/navigation.h"
 #include "gnss_utils/observations.h"
-#include "gnss_utils/ublox_raw.h"
+#include "gnss_utils/receivers/ublox_raw.h"
 
 using namespace wolf;
 
-class SubscriberGnssUblox : public Subscriber
+class SubscriberGnssUblox : public SubscriberGnssReceiver
 {
     public:
         // Constructor
@@ -40,8 +40,5 @@ class SubscriberGnssUblox : public Subscriber
 
         static std::shared_ptr<Subscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr);
 
-    protected:
-
-        GnssUtils::UBloxRaw ublox_raw_;
 };
 WOLF_REGISTER_SUBSCRIBER(SubscriberGnssUblox)

src/subscriber_gnss_novatel.cpp

+#include "../include/subscriber_gnss_novatel.h"
+
+using namespace wolf;
+
+// Constructor
+SubscriberGnssNovatel::SubscriberGnssNovatel(const SensorBasePtr& sensor_ptr) :
+        SubscriberGnssReceiver(sensor_ptr, std::make_shared<GnssUtils::NovatelRaw>())
+{
+}
+
+void SubscriberGnssNovatel::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
+{
+    sub_     = nh.subscribe(topic, 100, &SubscriberGnssNovatel::callback, this);
+}
+
+void SubscriberGnssNovatel::callback(const novatel_oem7_msgs::Oem7RawMsg& msg)
+{
+    GnssUtils::RawDataType res = receiver_->addDataStream(msg.message_data);
+
+    //std::cout << "res = " << (int) res << std::endl;
+
+    // only create capture if observation is received
+    if (res == GnssUtils::OBS)
+    {
+        //ROS_INFO("Observation received!");
+        createCaptureAndProcess();
+    }
+}
+
+std::shared_ptr<Subscriber> SubscriberGnssNovatel::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)
+{
+    return std::make_shared<SubscriberGnssNovatel>(_sensor_ptr);
+}

src/subscriber_gnss_receiver.cpp

+#include "../include/subscriber_gnss_receiver.h"
+
+using namespace wolf;
+
+// Constructor
+SubscriberGnssReceiver::SubscriberGnssReceiver(const SensorBasePtr& sensor_ptr, GnssUtils::ReceiverRawAbstractPtr _receiver) :
+        Subscriber(sensor_ptr),
+        receiver_(_receiver)
+{
+}
+
+void SubscriberGnssReceiver::createCaptureAndProcess()
+{
+    auto snapshot_ptr = std::make_shared<GnssUtils::Snapshot>(std::make_shared<GnssUtils::Observations>(receiver_->getObservations()),
+                                                              std::make_shared<GnssUtils::Navigation>(receiver_->getNavigation()));
+    //GnssUtils::ObservationsPtr obs_ptr = std::make_shared<GnssUtils::Observations>(receiver_->getObservations());
+    //ROS_INFO("Observations copied!");
+    //GnssUtils::NavigationPtr   nav_ptr = std::make_shared<GnssUtils::Navigation>(receiver_->getNavigation());
+    //ROS_INFO("Navigation copied!");
+
+    ros::Time now = ros::Time::now();
+
+    auto cap_gnss_ptr = std::make_shared<CaptureGnss>(TimeStamp(now.sec, now.nsec),
+                                                      sensor_ptr_,
+                                                      snapshot_ptr);
+
+    //ROS_INFO("Capture GNSS created!");
+    cap_gnss_ptr->process();
+    //ROS_INFO("Capture GNSS processed!");
+}

src/subscriber_gnss_ublox.cpp

@@ -4,7 +4,7 @@ using namespace wolf;
 
 // Constructor
 SubscriberGnssUblox::SubscriberGnssUblox(const SensorBasePtr& sensor_ptr) :
-        Subscriber(sensor_ptr)
+        SubscriberGnssReceiver(sensor_ptr, std::make_shared<GnssUtils::UBloxRaw>())
 {
 }
 
@@ -17,7 +17,7 @@ void SubscriberGnssUblox::initSubscriber(ros::NodeHandle& nh, const std::string&
 void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
 
 {
-    GnssUtils::RawDataType res = ublox_raw_.addDataStream(msg.data);
+    GnssUtils::RawDataType res = receiver_->addDataStream(msg.data);
 
     //std::cout << "res = " << (int) res << std::endl;
 
@@ -25,22 +25,7 @@ void SubscriberGnssUblox::callback(const std_msgs::UInt8MultiArray& msg)
     if (res == GnssUtils::OBS)
     {
         //ROS_INFO("Observation received!");
-        auto snapshot_ptr = std::make_shared<GnssUtils::Snapshot>(std::make_shared<GnssUtils::Observations>(ublox_raw_.getObservations()),
-                                                                  std::make_shared<GnssUtils::Navigation>(ublox_raw_.getNavigation()));
-        //GnssUtils::ObservationsPtr obs_ptr = std::make_shared<GnssUtils::Observations>(ublox_raw_.getObservations());
-        //ROS_INFO("Observations copied!");
-        //GnssUtils::NavigationPtr   nav_ptr = std::make_shared<GnssUtils::Navigation>(ublox_raw_.getNavigation());
-        //ROS_INFO("Navigation copied!");
-
-        ros::Time now = ros::Time::now();
-
-        auto cap_gnss_ptr = std::make_shared<CaptureGnss>(TimeStamp(now.sec, now.nsec),
-                                                          sensor_ptr_,
-                                                          snapshot_ptr);
-
-        //ROS_INFO("Capture GNSS created!");
-        cap_gnss_ptr->process();
-        //ROS_INFO("Capture GNSS processed!");
+        createCaptureAndProcess();
     }
 }
 std::shared_ptr<Subscriber> SubscriberGnssUblox::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)