Merge pull request #1 from ayrton04/indigo-devel

Making orientation, angular rate, and accelerations completely consistent with REP-103

src/imu_node.cc

@@ -442,23 +442,23 @@ class ImuNode
 
     imu.receiveAccelAngrateOrientation(&time, accel, angrate, orientation);
     data.linear_acceleration.x = accel[0];
-    data.linear_acceleration.y = accel[1];
-    data.linear_acceleration.z = accel[2];
+    data.linear_acceleration.y = -accel[1];
+    data.linear_acceleration.z = -accel[2];
 
     data.angular_velocity.x = angrate[0];
-    data.angular_velocity.y = angrate[1];
-    data.angular_velocity.z = angrate[2];
+    data.angular_velocity.y = -angrate[1];
+    data.angular_velocity.z = -angrate[2];
 
-    tf::Quaternion quat;
-    (tf::Matrix3x3(-1,0,0,
-		 0,1,0,
-		 0,0,-1)*
-    tf::Matrix3x3(orientation[0], orientation[3], orientation[6],
-		 orientation[1], orientation[4], orientation[7],
-		 orientation[2], orientation[5], orientation[8])).getRotation(quat);
+    tf::Matrix3x3 mat(orientation[0], orientation[3], orientation[6],
+		      orientation[1], orientation[4], orientation[7],
+		      orientation[2], orientation[5], orientation[8]);
 
+    double r, p, y;
+    tf::Quaternion quat;
+    mat.getRPY(r, p, y);
+    quat.setRPY(r, -p, -y);
     tf::quaternionTFToMsg(quat, data.orientation);
-      
+
     data.header.stamp = ros::Time::now().fromNSec(time);
   }