New functions added

CMakeLists.txt

@@ -1,5 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(imu_3dm_gx4)
+
+find_package(Boost REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   diagnostic_updater
   message_generation
@@ -11,20 +13,55 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(DIRECTORY msg)
 generate_messages(DEPENDENCIES geometry_msgs)
 
-catkin_package()
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}_ros
+  DEPENDS boost)
 
-# include boost
-find_package(Boost REQUIRED)
-link_directories(${Boost_LIBRARY_DIR})
-include_directories(${Boost_INCLUDE_DIR})
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIR})
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+  ${Boost_LIBRARY_DIR})
 
-add_definitions("-std=c++0x -Wall -Werror")
 
-add_executable(${PROJECT_NAME} src/imu_3dm_gx4.cpp src/imu.cpp)
+#add_definitions("-std=c++0x -Wall -Werror")
+add_definitions("-std=c++0x -Wall -Wno-unused-variable -Wno-uninitialized")
+
+add_library(${PROJECT_NAME}
+  src/imu.cpp
+)
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
 
+add_library(${PROJECT_NAME}_ros
+  src/imu_ros_base.cpp
+)
+target_link_libraries(${PROJECT_NAME}_ros
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_cont_node
+  src/imu_3dm_gx4_cont.cpp
+)
+target_link_libraries(${PROJECT_NAME}_cont_node
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_executable(${PROJECT_NAME}_single_node
+  src/imu_3dm_gx4_single.cpp
+)
+target_link_libraries(${PROJECT_NAME}_single_node
+  ${PROJECT_NAME}
+  ${PROJECT_NAME}_ros
+  ${catkin_LIBRARIES}
+)
+
 add_dependencies(${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}

src/imu.hpp → include/imu_3dm_gx4/imu.hpp

@@ -377,6 +377,32 @@ class Imu {
    */
   void enableFilterStream(bool enabled);
 
+  /**
+   * @brief enableIMUStream Poll IMU data once
+   * @param enabled If true, an ack package will be returned from the device
+   * before the actual data package. The default is set to false in order
+   * not to jam the bandwidth.
+   */
+  void pollIMUData(bool enable_ack = false);
+
+  /**
+   * @brief enableIMUStream Poll filter data once
+   * @param enabled If true, an ack package will be returned from the device
+   * before the actual data package. The default is set to false in order
+   * not to jam the bandwidth.
+   */
+  void pollFilterData(bool enable_ack = false);
+
+  /**
+   * @brief saveIMUFormat Save the IMU data format
+   */
+  void saveIMUFormat();
+
+  /**
+   * @brief saveFilterFormat Save the filter data format
+   */
+  void saveFilterFormat();
+
   /**
    * @brief Set the IMU data callback.
    * @note The IMU data callback is called every time new IMU data is read.
@@ -398,7 +424,7 @@ class Imu {
   int pollInput(unsigned int to);
 
   std::size_t handleByte(const uint8_t& byte, bool& found);
-  
+
   int handleRead(size_t);
 
   void processPacket();

include/imu_3dm_gx4/imu_ros_base.hpp

@@ -0,0 +1,169 @@
+/*
+ * imu_ros.hpp
+ *
+ *  Copyright (c) 2014 Kumar Robotics. All rights reserved.
+ *
+ *  This file is part of galt.
+ *
+ *  Created on: 08/04/2014
+ *		  Author: Ke Sun
+ */
+
+
+#ifndef IMU_ROS_H
+#define IMU_ROS_H
+
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <diagnostic_updater/publisher.h>
+
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/MagneticField.h>
+#include <sensor_msgs/FluidPressure.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/QuaternionStamped.h>
+
+#include <imu_3dm_gx4/FilterOutput.h>
+#include <imu_3dm_gx4/imu.hpp>
+
+namespace imu_3dm_gx4 {
+
+  /**
+   * @brief ImuRosBase The class is a ros wrapper for the Imu class
+   * @author Ke Sun
+   */
+  class ImuRosBase {
+  public:
+    /**
+     * @brief Constructor of the class
+     */
+    ImuRosBase(const ros::NodeHandle& nh, const std::string& device, const bool verbose = false);
+    /**
+     * @brief Destructor of the class
+     * @note Disconnect the device automatically
+     */
+    ~ImuRosBase() {
+      disconnect();
+    }
+    /**
+     * @brief initialize Initialize IMU according to the settings
+     */
+    bool initialize();
+    /**
+     * @brief enableIMUStream Enable or disable IMU stream
+     * @param enabled If ture, the continuous stream is enabled
+     */
+    void enableIMUStream(bool enabled){
+      imu.enableIMUStream(enabled);
+      return;
+    }
+    /**
+     * @brief enableFilterStream Enable or disable filter stream
+     * @param enabled If ture, the continuous stream is enabled
+     */
+    void enableFilterStream(bool enabled){
+      if (enableFilter)
+        imu.enableFilterStream(enabled);
+      else
+        imu.enableFilterStream(false);
+
+      return;
+    }
+    /**
+     * @brief requestIMUOnce Request IMU data for a single time
+     * @param enable_ack If ture, an ack package will come before the data
+     */
+    void requestIMUOnce(bool enable_ack = false){
+      imu.pollIMUData(enable_ack);
+      return;
+    }
+    /**
+     * @brief requestFilterOnce Request filter data for a single time
+     * @param enable_ack If ture, an ack package will come before the data
+     */
+    void requestFilterOnce(bool enable_ack = false){
+      if (enableFilter)
+        imu.pollFilterData(enable_ack);
+      return;
+    }
+    /**
+     * @brief idle Set the device to idle mode
+     * @need_reply If ture, a ack package is expected
+     */
+    void idle(bool need_reply = true) {
+      imu.idle(need_reply);
+    }
+    /**
+     * @brief resume Resume data streaming
+     */
+    void resume() {
+      imu.resume();
+    }
+    /**
+     * @brief disconnect Disconnect the device
+     */
+    void disconnect() {
+      imu.disconnect();
+    }
+    /**
+     * @brief runOnce Wait for reading the data
+     */
+    void runOnce() {
+      imu.runOnce();
+      updater->update();
+    }
+
+    /**
+     * @Note Do not use these functions outside the class.
+     * They are defined in the public section due to the callback
+     * issue in the original imu_3dm_gx4_25 driver
+     * TODO: consider seal these functions
+     */
+    void publishIMUData(const Imu::IMUData& data);
+    void publishFilterData(const Imu::FilterData& data);
+
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> configTopicDiagnostic(
+        const std::string& name, double * target);
+    void updateDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper& stat,
+        imu_3dm_gx4::Imu* imu);
+
+  private:
+    // Settings
+    int baudrate;
+    int requestedImuRate;
+    int requestedFilterRate;
+    bool enableFilter;
+    bool enableMagUpdate;
+    bool enableAccelUpdate;
+    std::string frameId;
+
+    ros::NodeHandle nh;
+    Imu imu;
+
+    // Publishers
+    ros::Publisher pubIMU;
+    ros::Publisher pubMag;
+    ros::Publisher pubPressure;
+    ros::Publisher pubFilter;
+
+    Imu::Info info;
+    Imu::DiagnosticFields fields;
+
+    // diagnostic_updater resources
+    std::shared_ptr<diagnostic_updater::Updater> updater;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> imuDiag;
+    std::shared_ptr<diagnostic_updater::TopicDiagnostic> filterDiag;
+
+    // Disable copy constructor and assign operator
+    ImuRosBase(const ImuRosBase&);
+    ImuRosBase& operator=(const ImuRosBase&);
+
+    bool loadParameters();
+    void createPublishers();
+  };
+
+}
+
+#endif
+

launch/imu.launch

@@ -26,7 +26,7 @@
     <arg name="enable_accel_update" default="false"/>
     <arg name="enable_mag_update" default="false"/>
 
-    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4" output="$(arg output)">
+    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4_cont_node" output="$(arg output)">
         <param name="device" type="string" value="$(arg device)" />
         <param name="verbose" type="bool" value="$(arg verbose)"/>
         <param name="baudrate" type="int" value="$(arg baudrate)" />

launch/imu_single.launch

@@ -0,0 +1,40 @@
+<launch>
+    <!-- Node Settings -->
+    <arg name="output" default="screen"/>
+    <arg name="imu" default="imu"/>
+
+    <!-- IMU Settings -->
+    <arg name="device" default="/dev/ttyACM0" />
+
+    <!-- Verbose logging -->
+    <arg name="verbose" default="false"/>
+
+    <!-- Frame ID for messages -->
+    <arg name="frame_id" default="$(arg imu)"/>
+
+    <!-- Baudrate of serial comms (see manual for allowed values) -->
+    <arg name="baudrate" default="115200"/>
+
+    <!-- Data rate in Hz -->
+    <arg name="imu_rate" default="100"/>
+    <arg name="filter_rate" default="100"/>
+
+    <!-- Enable/Disable the filter -->
+    <arg name="enable_filter" default="true"/>
+
+    <!-- Enable/Disable filter updates -->
+    <arg name="enable_accel_update" default="true"/>
+    <arg name="enable_mag_update" default="true"/>
+
+    <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4_single_node" output="$(arg output)">
+        <param name="device" type="string" value="$(arg device)" />
+        <param name="verbose" type="bool" value="$(arg verbose)"/>
+        <param name="baudrate" type="int" value="$(arg baudrate)" />
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="imu_rate" type="int" value="$(arg imu_rate)" />
+        <param name="filter_rate" type="int" value="$(arg filter_rate)"/>
+        <param name="enable_filter" type="bool" value="$(arg enable_filter)"/>
+        <param name="enable_accel_update" type="bool" value="$(arg enable_accel_update)"/>
+        <param name="enable_mag_update" type="bool" value="$(arg enable_mag_update)"/>
+    </node>
+</launch>