Feature #44 - odometry output

CMakeLists.txt

@@ -8,6 +8,9 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
   geometry_msgs
   message_generation
+  gps_common
+  tf2_ros
+  tf2_msgs
 )
 
 ################################################
@@ -147,4 +150,4 @@ install(DIRECTORY config/
 
 #############
 ## Testing ##
-#############
+#############

config/example/ellipse_E_default.yaml

@@ -15,14 +15,14 @@ driver:
   frequency: 400
 
 # Configuration of the device with ROS.
-confWithRos: false
+confWithRos: true
 
 uartConf:
   # Port Name
   portName: "/dev/ttyUSB0"
 
   # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
-  baudRate: 115200
+  baudRate: 921600
 
   # Port Id
   # 0 PORT_A: Main communication interface. Full duplex.
@@ -246,7 +246,7 @@ output:
 
   # 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.
@@ -267,7 +267,7 @@ output:
   # Includes roll, pitch, yaw and their accuracies on each axis
   log_ekf_euler: 8
   # Includes the 4 quaternions values
-  log_ekf_quat: 0
+  log_ekf_quat: 8
   # Position and velocities in NED coordinates with the accuracies on each axis
   log_ekf_nav: 8
   # Heave, surge and sway and accelerations on each axis for up to 4 points

config/example/ellipse_N_default.yaml

@@ -14,6 +14,16 @@ driver:
   #       it can lead to miss data frame and less accurate time stamping.
   frequency: 400
 
+odometry:
+    # Enable ROS odometry messages.
+    enable: true
+    # Publish odometry transforms.
+    publishTf: true
+    # Odometry frame IDs.
+    odomFrameId: "odom"
+    baseFrameId: "base_link"
+    initFrameId: "map"
+
 # Configuration of the device with ROS.
 confWithRos: false
 
@@ -22,7 +32,7 @@ uartConf:
   portName: "/dev/ttyUSB0"
 
   # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
-  baudRate: 115200
+  baudRate: 921600
 
   # Port Id
   # 0 PORT_A: Main communication interface. Full duplex.
@@ -246,7 +256,7 @@ output:
 
   # 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.
@@ -265,11 +275,11 @@ output:
   # Includes IMU status, acc., gyro, temp delta speeds and delta angles values
   log_imu_data: 8
   # Includes roll, pitch, yaw and their accuracies on each axis
-  log_ekf_euler: 0
+  log_ekf_euler: 8
   # Includes the 4 quaternions values
-  log_ekf_quat: 0
+  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

include/sbg_driver/config_store.h

@@ -115,6 +115,12 @@ class ConfigStore
   std::string                 m_frame_id_;
   bool						  m_use_enu_;
 
+  bool						  m_odom_enable_;
+  bool						  m_odom_publish_tf_;
+  std::string                 m_odom_frame_id_;
+  std::string                 m_odom_base_frame_id_;
+  std::string                 m_odom_init_frame_id_;
+
   //---------------------------------------------------------------------//
   //- Private  methods                                                  -//
   //---------------------------------------------------------------------//
@@ -152,6 +158,13 @@ class ConfigStore
    */
   void loadDriverParameters(const ros::NodeHandle& ref_node_handle);
 
+  /*!
+   * Load odometry parameters.
+   *
+   * \param[in] ref_node_handle   ROS nodeHandle.
+   */
+  void loadOdomParameters(const ros::NodeHandle& ref_node_handle);
+
   /*!
    * Load interface communication parameters.
    *
@@ -445,14 +458,50 @@ class ConfigStore
    */
    bool getUseEnu(void) const;
 
+  /*!
+   * Get odom enable.
+   *
+   * \return					 True if the odometry is enabled.
+   */
+   bool getOdomEnable(void) const;
+
+  /*!
+   * Get odom publish_tf.
+   *
+   * \return					 If true publish odometry transforms.
+   */
+   bool getOdomPublishTf(void) const;
+
+  /*!
+   * Get the odometry frame ID.
+   *
+   * \return                      Odometry frame ID.
+   */
+  const std::string &getOdomFrameId(void) const;
+
+
+  /*!
+   * Get the odometry base frame ID.
+   *
+   * \return                      Odometry base frame ID.
+   */
+  const std::string &getOdomBaseFrameId(void) const;
+
+
+  /*!
+   * Get the odometry init frame ID.
+   *
+   * \return                      Odometry init frame ID.
+   */
+  const std::string &getOdomInitFrameId(void) const;
+
   /*!
    * Get the time reference.
    *
    * \return                      Time reference.
    */
   TimeReference getTimeReference(void) const;
 
-
   //---------------------------------------------------------------------//
   //- Operations                                                        -//
   //---------------------------------------------------------------------//

include/sbg_driver/message_publisher.h

@@ -80,6 +80,7 @@ class MessagePublisher
   ros::Publisher          m_velocity_pub_;
   ros::Publisher          m_utc_reference_pub_;
   ros::Publisher          m_nav_sat_fix_pub_;
+  ros::Publisher          m_odometry_pub_;
 
   MessageWrapper          m_message_wrapper_;
   uint32_t                m_max_messages_;
@@ -111,8 +112,9 @@ class MessagePublisher
    * Define standard ROS publishers.
    *
    * \param[in] ref_ros_node_handle     Ros NodeHandle to advertise the publisher.
+   * \param[in] odom_enable             If true, enable odometry messages.
    */
-  void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle);
+  void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle, bool odom_enable);
 
   /*!
    * Publish a received SBG IMU log.
@@ -131,6 +133,11 @@ class MessagePublisher
    */
   void processRosImuMessage(void);
 
+  /*!
+   * Process a ROS odometry standard message.
+   */
+  void processRosOdoMessage(void);
+
   /*!
    * Publish a received SBG Magnetic log.
    *