restructure code to accomodate ascii case

Author: andre-nguyen

src/imu_vn_100.cpp

@@ -40,15 +40,6 @@ void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3,
 void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat,
                                 const vn::math::vec4f& vn_vec4);
 
-/**
- * @brief FillImuMessage
- * @param imu_msg
- * @param p
- * @param binary_output
- */
-void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p,
-                    bool binary_output);
-
 /**
  * @brief asciiOrBinaryAsyncMessageReceived
  * @param userData
@@ -112,7 +103,6 @@ ImuVn100::ImuVn100(const ros::NodeHandle& pnh)
       baudrate_(921600),
       frame_id_(std::string("imu")) {
   Initialize();
-  // TODO pointer hack
 }
 
 ImuVn100::~ImuVn100() { Disconnect(); }
@@ -145,6 +135,12 @@ void ImuVn100::LoadParameters() {
   pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000);
 
   pnh_.param("binary_output", binary_output_, true);
+
+  if (!binary_output_ && (enable_pres_ | enable_temp_)) {
+    ROS_ERROR("VN: Ascii mode cannot support mag, pressure and temp.");
+    enable_pres_ = enable_temp_ = false;
+  }
+
   int vn_serial_output_tmp = 4;  // init on invalid number
   pnh_.param("vn_serial_output", vn_serial_output_tmp, 1);
   switch (vn_serial_output_tmp) {
@@ -277,7 +273,7 @@ void ImuVn100::Stream(bool async) {
       imu_.writeBinaryOutput1(bor, true);
     } else {
       // Set the ASCII output data type and data rate
-      imu_.writeAsyncDataOutputType(VNIMU, true);
+      imu_.writeAsyncDataOutputType(VNQMR, true);
     }
 
     // add a callback function for new data event
@@ -310,13 +306,41 @@ void ImuVn100::PublishData(vn::protocol::uart::Packet& p) {
   imu_msg.header.stamp = ros::Time::now();
   imu_msg.header.frame_id = frame_id_;
 
-  FillImuMessage(imu_msg, p, binary_output_);
+  vn::math::vec4f quaternion;
+  vn::math::vec3f linear_accel;
+  vn::math::vec3f angular_rate;
+  vn::math::vec3f magnetometer;
+  if (binary_output_) {
+    // Note: With this library, we are responsible for extracting the data
+    // in the appropriate order! Need to refer to manual and to how we
+    // configured the common output group
+    quaternion = p.extractVec4f();  // COMMONGROUP_QUATERNION
+    // NOTE: The IMU angular velocity and linear acceleration outputs are
+    // swapped. And also why are they different?
+    linear_accel = p.extractVec3f();  // COMMONGROUP_IMU
+    angular_rate = p.extractVec3f();  // COMMONGROUP_IMU
+
+  } else {
+    // ascii format
+    if (p.type() != vn::protocol::uart::Packet::TYPE_ASCII) return;
+
+    if (p.determineAsciiAsyncType() != vn::protocol::uart::VNQMR) return;
+
+    // TODO check if angular rate and linear acceleration are also swapped
+    // in ascii mode
+    p.parseVNQMR(&quaternion, &magnetometer, &linear_accel, &angular_rate);
+  }
+
+  RosQuaternionFromVnVector4(imu_msg.orientation, quaternion);
+  RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate);
+  RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel);
+
   pd_imu_.Publish(imu_msg);
 
   if (enable_mag_) {
     sensor_msgs::MagneticField mag_msg;
     mag_msg.header = imu_msg.header;
-    vn::math::vec3f magnetometer = p.extractVec3f();  // COMMONGROUP_MAGPRES
+    magnetometer = p.extractVec3f();  // COMMONGROUP_MAGPRES
     RosVector3FromVnVector3(mag_msg.magnetic_field, magnetometer);
     pd_mag_.Publish(mag_msg);
   }
@@ -358,26 +382,6 @@ void RosQuaternionFromVnVector4(geometry_msgs::Quaternion& ros_quat,
   ros_quat.w = vn_vec4[3];
 }
 
-void FillImuMessage(sensor_msgs::Imu& imu_msg, vn::protocol::uart::Packet& p,
-                    bool binary_output) {
-  if (binary_output) {
-    // Note: With this library, we are responsible for extracting the data
-    // in the appropriate order! Need to refer to manual and to how we
-    // configured the common output group
-    vn::math::vec4f quaternion = p.extractVec4f();  // COMMONGROUP_QUATERNION
-    // NOTE: The IMU angular velocity and linear acceleration outputs are
-    // swapped. And also why are they different?
-    vn::math::vec3f linear_accel = p.extractVec3f();  // COMMONGROUP_IMU
-    vn::math::vec3f angular_rate = p.extractVec3f();  // COMMONGROUP_IMU
-
-    RosQuaternionFromVnVector4(imu_msg.orientation, quaternion);
-    RosVector3FromVnVector3(imu_msg.angular_velocity, angular_rate);
-    RosVector3FromVnVector3(imu_msg.linear_acceleration, linear_accel);
-  } else {
-    // TODO
-  }
-}
-
 void asciiOrBinaryAsyncMessageReceived(void* userData,
                                        vn::protocol::uart::Packet& p,
                                        size_t index) {