Merge pull request #30 from Brazilian-Institute-of-Robotics/imu_samples

Add imu_samples port
Brazilian-Institute-of-Robotics · Nov 25, 2016 · d904876 · d904876
2 parents 589169b + 2fa53f5
commit d904876
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 2 deletions.
diff --git a/rock_gazebo.orogen b/rock_gazebo.orogen
@@ -114,5 +114,8 @@ task_context "ImuTask" do
 
     # Provides orientation samples reported by the IMU sensor.'
     output_port('orientation_samples', '/base/samples/RigidBodyState')
+
+    # IMU samples containing linear acceleration, orientation and angular velocity
+    output_port('imu_samples', '/base/samples/IMUSensors')
 end
 
diff --git a/tasks/ImuTask.cpp b/tasks/ImuTask.cpp
@@ -53,8 +53,11 @@ void ImuTask::updateHook()
     {
         imu_reading.sourceFrame = _imu_frame.value();
         imu_reading.targetFrame = _world_frame.value();
+        imu_reading.time = getCurrentTime();
         _orientation_samples.write(imu_reading);
         imu_reading.invalidate();
+        imu_samples.time = getCurrentTime();
+        _imu_samples.write(imu_samples);
     }
 }
 void ImuTask::errorHook()
@@ -90,16 +93,26 @@ void ImuTask::readInput( ConstIMUPtr & imuMsg)
         throw std::runtime_error("rock_gazebo::ImuTask requires has_orientation");
     if(!imuMsg->has_angular_velocity())
         throw std::runtime_error("rock_gazebo::ImuTask requires has_angular_velocity");
+    if(!imuMsg->has_linear_acceleration())
+        throw std::runtime_error("rock_gazebo::ImuTask requires has_linear_acceleration");
 
     const ::gazebo::msgs::Quaternion &quat = imuMsg->orientation();
     if(!quat.has_x() || !quat.has_y() || !quat.has_z() || !quat.has_w())
         throw std::runtime_error("rock_gazebo::ImuTask: invalid quaternion");
     imu_reading.orientation = base::Orientation(quat.w(),quat.x(),quat.y(),quat.z());
+    imu_samples.mag = base::Vector3d(imu_reading.getRoll(), imu_reading.getPitch(),
+            imu_reading.getYaw());
 
     const ::gazebo::msgs::Vector3d& avel =  imuMsg->angular_velocity();
     if(!avel.has_x() || !avel.has_y() || !avel.has_z())
-        throw std::runtime_error("rock_gazebo::ImuTask: invalid Vector3d");
-    imu_reading.angular_velocity = base::Vector3d(avel.x(),avel.y(),avel.z());
+        throw std::runtime_error("rock_gazebo::ImuTask: invalid Vector3d for angular velocity");
     imu_reading.time = getCurrentTime();
+    imu_reading.angular_velocity = base::Vector3d(avel.x(),avel.y(),avel.z());
+    imu_samples.gyro = imu_reading.angular_velocity;
+
+    const ::gazebo::msgs::Vector3d& linacc =  imuMsg->linear_acceleration();
+    if(!linacc.has_x() || !linacc.has_y() || !linacc.has_z())
+        throw std::runtime_error("rock_gazebo::ImuTask: invalid Vector3d for linear acceleration");
+    imu_samples.acc = base::Vector3d(linacc.x(), linacc.y(), linacc.z());
 }
 
diff --git a/tasks/ImuTask.hpp b/tasks/ImuTask.hpp
@@ -6,6 +6,7 @@
 #include <gazebo/physics/physics.hh>
 #include <gazebo/transport/transport.hh>
 #include <gazebo/msgs/imu.pb.h>
+#include <base/samples/IMUSensors.hpp>
 
 #include "rock_gazebo/ImuTaskBase.hpp"
 
@@ -117,6 +118,7 @@ namespace rock_gazebo{
         gazebo::transport::NodePtr node;
         gazebo::transport::SubscriberPtr imu_subscriber;
         base::samples::RigidBodyState imu_reading;
+        base::samples::IMUSensors imu_samples;
     };
 }