Changes to work directly with files sbg_data.dat

README.md

@@ -1,3 +1,14 @@
+## usage
+
+To run the sbg_driver using a file as source instead of the device
+
+```
+roslaunch sbg_driver sbg_file.launch file_path:=path
+```
+
+
+# below, the original README
+
 # sbg_driver
 
 [![Build Status](https://build.ros.org/buildStatus/icon?job=Mdev__sbg_driver__ubuntu_bionic_amd64&build=lastBuild)](https://build.ros.org/job/Mdev__sbg_driver__ubuntu_bionic_amd64/lastBuild/)

config/sbg_device_uart_default.yaml

@@ -11,8 +11,13 @@ driver:
 # Configuration of the device with ROS.
 confWithRos: false
 
+# File configuration
+# can be configured by command line as defined in README
+#filesConf:
+# path: 'sbg_data.dat'
+
 # Uart configuration
-uartConf:
+uartConf_:
   # Port Name
   portName: "/dev/ttyUSB0"
 
@@ -207,6 +212,8 @@ odom:
   # 2 Measurement is always accepted
   rejectMode: 1
 
+  enable: true
+
 # ToDo: event & CAN configuration
 
 ############################### Output configuration ###############################
@@ -235,11 +242,11 @@ output:
   #
   # "ros" : ROS time (default)
   # "ins_unix" : INS absolute time referenced to UNIX epoch (00:00:00 UTC on 1 January 1970)
-  time_reference: "ros"
+  time_reference: "ins_unix" #"ros"
 
   # Ros standard output:
   # Note: If true publish ROS standard messages.
-  ros_standard: false
+  ros_standard: true
 
   # Frame convention:
   # Note: If true messages are expressed in the ENU convention.
@@ -256,25 +263,25 @@ output:
   # Status general, clock, com aiding, solution, heave
   log_status: 200
   # Includes IMU status, acc., gyro, temp delta speeds and delta angles values
-  log_imu_data: 8
+  log_imu_data: 3
   # Includes roll, pitch, yaw and their accuracies on each axis
   log_ekf_euler: 0
   # Includes the 4 quaternions values
   log_ekf_quat: 8
   # Position and velocities in NED coordinates with the accuracies on each axis
-  log_ekf_nav: 0
+  log_ekf_nav: 8
   # Heave, surge and sway and accelerations on each axis for up to 4 points
   log_ship_motion: 0
   # Provides UTC time reference
   log_utc_time: 200
   # Magnetic data with associated accelerometer on each axis
-  log_mag: 0
+  log_mag: 4
   # Magnetometer calibration data (raw buffer)
-  log_mag_calib: 0
+  log_mag_calib: 5
   # GPS velocities from primary or secondary GPS receiver
   log_gps1_vel: 0
   # GPS positions from primary or secondary GPS receiver
-  log_gps1_pos: 0
+  log_gps1_pos: 14
   # GPS true heading from dual antenna system
   log_gps1_hdt: 0
   # GPS 1 raw data for post processing.
@@ -287,6 +294,10 @@ output:
   log_event_c: 0
   log_event_d: 0
   # Air data
-  log_air_data: 0
+  log_air_data: 36
   # Short IMU data
   log_imu_short: 8
+
+
+odometry:
+  enable: true

config/sbg_device_udp_default.yaml

@@ -199,6 +199,8 @@ odom:
   # 2 Measurement is always accepted
   rejectMode: 1
 
+  enable: true
+
 # ToDo: event & CAN configuration
 
 ############################### Output configuration ###############################
@@ -227,11 +229,11 @@ output:
   #
   # "ros" : ROS time (default)
   # "ins_unix" : INS absolute time referenced to UNIX epoch (00:00:00 UTC on 1 January 1970)
-  time_reference: "ros"
+  time_reference: "ins_unix" #"ros"
 
   # Ros standard output:
   # Note: If true publish ROS standard messages.
-  ros_standard: false
+  ros_standard: true
 
   # Frame convention:
   # Note: If true messages are expressed in the ENU convention.
@@ -248,25 +250,25 @@ output:
   # Status general, clock, com aiding, solution, heave
   log_status: 200
   # Includes IMU status, acc., gyro, temp delta speeds and delta angles values
-  log_imu_data: 8
+  log_imu_data: 3
   # Includes roll, pitch, yaw and their accuracies on each axis
   log_ekf_euler: 0
   # Includes the 4 quaternions values
   log_ekf_quat: 8
   # Position and velocities in NED coordinates with the accuracies on each axis
-  log_ekf_nav: 0
+  log_ekf_nav: 8
   # Heave, surge and sway and accelerations on each axis for up to 4 points
   log_ship_motion: 0
   # Provides UTC time reference
   log_utc_time: 200
   # Magnetic data with associated accelerometer on each axis
-  log_mag: 0
+  log_mag: 4
   # Magnetometer calibration data (raw buffer)
-  log_mag_calib: 0
+  log_mag_calib: 5
   # GPS velocities from primary or secondary GPS receiver
   log_gps1_vel: 0
   # GPS positions from primary or secondary GPS receiver
-  log_gps1_pos: 0
+  log_gps1_pos: 14
   # GPS true heading from dual antenna system
   log_gps1_hdt: 0
   # GPS 1 raw data for post processing.
@@ -279,6 +281,10 @@ output:
   log_event_c: 0
   log_event_d: 0
   # Air data
-  log_air_data: 0
+  log_air_data: 36
   # Short IMU data
-  log_imu_short: 0
+  log_imu_short: 8
+
+
+odometry:
+  enable: true

include/sbg_driver/config_store.h

@@ -83,6 +83,9 @@ class ConfigStore
   uint32_t                    m_in_port_address_;
   bool                        m_upd_communication_;
 
+  std::string                 m_sbg_file_;
+  bool                        m_files_communication_;
+
   bool                        m_configure_through_ros_;
 
   SbgEComInitConditionConf    m_init_condition_conf_;
@@ -317,6 +320,20 @@ class ConfigStore
    */
   uint32_t getInputPortAddress(void) const;
 
+  /*!
+   * Check if the interface configuration is to read dump files.
+   *
+   * \return                      True if the interface is dump files, False otherwise.
+   */
+  bool isInterfaceFiles(void) const;
+
+  /*!
+   * Get the input files.
+   *
+   * \return                      Input files.
+   */
+  const std::string &getFile(void) const;
+
   /*!
    * Get the initial conditions configuration.
    *

launch/sbg_device.launch

@@ -1,5 +1,5 @@
 <launch>
   <node name="sbg_device" pkg="sbg_driver" type="sbg_device" output="screen">
-  	<rosparam command="load" file="$(find sbg_driver)/config/sbg_device_uart_default.yaml" />
+  	<rosparam command="load" file="$(find sbg_driver)/config/sbg_device_udp_default.yaml" />
   </node>
 </launch>

launch/sbg_file.launch

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="file_path"/>
+  <node name="sbg_device" pkg="sbg_driver" type="sbg_device" output="screen">
+  	<rosparam command="load" file="$(find sbg_driver)/config/sbg_device_uart_default.yaml" />
+    <param name="/filesConf/path" type="string" value="$(arg file_path)" />
+  </node>
+</launch>

src/config_store.cpp

@@ -59,6 +59,11 @@ void ConfigStore::loadCommunicationParameters(const ros::NodeHandle& ref_node_ha
     m_out_port_address_   = getParameter<uint32_t>(ref_node_handle, "ipConf/out_port", 0);
     m_in_port_address_    = getParameter<uint32_t>(ref_node_handle, "ipConf/in_port", 0);
   }
+  else if (ref_node_handle.hasParam("filesConf"))
+  {
+    m_files_communication_ = true;
+    m_sbg_file_ = ref_node_handle.param<std::string>("filesConf/path", std::string("*.*"));
+  }
   else
   {
     throw ros::Exception("SBG DRIVER - Invalid communication interface parameters.");
@@ -240,6 +245,16 @@ uint32_t ConfigStore::getInputPortAddress(void) const
   return m_in_port_address_;
 }
 
+bool ConfigStore::isInterfaceFiles(void) const
+{
+  return m_files_communication_;
+}
+
+const std::string &ConfigStore::getFile(void) const
+{
+  return m_sbg_file_;
+}
+
 const SbgEComInitConditionConf &ConfigStore::getInitialConditions(void) const
 {
   return m_init_condition_conf_;

src/sbg_device.cpp

@@ -115,6 +115,10 @@ void SbgDevice::onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const Sb
   // Publish the received SBG log.
   //
   m_message_publisher_.publish(msg_class, msg, ref_sbg_data);
+
+  // if Sbg driver is reading from file
+  if (_config_store_.isInterfaceFiles())
+    usleep(50);
 }
 
 void SbgDevice::loadParameters(void)
@@ -142,6 +146,10 @@ void SbgDevice::connect(void)
   {
     error_code = sbgInterfaceUdpCreate(&m_sbg_interface_, m_config_store_.getIpAddress(), m_config_store_.getInputPortAddress(), m_config_store_.getOutputPortAddress());
   }
+  else if (m_config_store_.isInterfaceFiles())
+  {
+    error_code = sbgInterfaceFileOpen(&m_sbg_interface_, m_config_store_.getFile().c_str());
+  }
   else
   {
     throw ros::Exception("Invalid interface type for the SBG device.");
@@ -159,7 +167,8 @@ void SbgDevice::connect(void)
     throw ros::Exception("SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code)));
   }
 
-  readDeviceInfo();
+  if (!m_config_store_.isInterfaceFiles())
+    readDeviceInfo();
 }
 
 void SbgDevice::readDeviceInfo(void)