Added the conversion from NED to ENU.

cfg/ImuMsgFix.cfg

@@ -37,12 +37,20 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
+enum_imu_reference = gen.enum([ 
+gen.const("NED", int_t, 0, "North East Down"),
+gen.const("ENU", int_t, 1, "East North Up"),
+], "Possible reference systems for the IMU.")
+
+
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
-gen.add("frame_id",  str_t,  0,                               "frame_id name to override msg with",  "")
-gen.add("fix_cov",   bool_t,  0,                              "enable covariance fix",  False)
-gen.add("cov_orientation", double_t,  0,   "orientation covariance value to be set",  0.01, 0.0, 100.0)
-gen.add("cov_vel",         double_t,  0,   "velocity covariance value to be set",     0.000001, 0.0, 100.0)
-gen.add("cov_acc",         double_t,  0,   "acceleration covariance value to be set", 0.000001, 0.0, 100.0)
+gen.add("fix_frame_id",            bool_t,    0,                               "enable frame_id fix",  False)
+gen.add("frame_id",                str_t,     0,                               "frame_id name to override msg with",  "")
+gen.add("fix_cov",                 bool_t,    0,                               "enable covariance fix",  False)
+gen.add("cov_orientation",         double_t,  0,                               "orientation covariance value to be set",  0.01, 0.0, 100.0)
+gen.add("cov_vel",                 double_t,  0,                               "velocity covariance value to be set",     0.000001, 0.0, 100.0)
+gen.add("cov_acc",                 double_t,  0,                               "acceleration covariance value to be set", 0.000001, 0.0, 100.0)
+gen.add("imu_reference",           int_t,     0,                               "IMU reference system",           0,       0,    1, edit_method=enum_imu_reference)
 
 exit(gen.generate(PACKAGE, "ImuMsgFixAlgorithm", "ImuMsgFix"))

include/imu_msg_fix_alg_node.h

@@ -44,13 +44,9 @@ class ImuMsgFixAlgNode : public algorithm_base::IriBaseAlgorithm<ImuMsgFixAlgori
 {
   private:
     // [publisher attributes]
-    ros::Publisher pose_publisher_;
-    geometry_msgs::PoseStamped pose_PoseStamped_msg_;
-
     ros::Publisher imu_out_publisher_;
     sensor_msgs::Imu imu_out_Imu_msg_;
 
-
     // [subscriber attributes]
     ros::Subscriber imu_in_subscriber_;
     void imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg);
@@ -58,7 +54,6 @@ class ImuMsgFixAlgNode : public algorithm_base::IriBaseAlgorithm<ImuMsgFixAlgori
     void imu_in_mutex_enter(void);
     void imu_in_mutex_exit(void);
 
-
     // [service attributes]
 
     // [client attributes]

launch/node.launch

@@ -6,5 +6,6 @@
   <node pkg="imu_msg_fix" name="imu_msg_fix" type="imu_msg_fix" output="screen">
     <remap from="~imu_in" to="$(arg imu_in)"/>
     <remap from="~imu_out" to="$(arg imu_out)"/>
+    
   </node>
-</launch>
+</launch>
\ No newline at end of file

src/imu_msg_fix_alg_node.cpp

@@ -7,13 +7,11 @@ ImuMsgFixAlgNode::ImuMsgFixAlgNode(void) :
   //this->loop_rate_ = 2;//in [Hz]
 
   // [init publishers]
-  this->pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("pose", 1);
   this->imu_out_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("imu_out", 1);
   
   // [init subscribers]
   this->imu_in_subscriber_ = this->public_node_handle_.subscribe("imu_in", 1, &ImuMsgFixAlgNode::imu_in_callback, this);
   pthread_mutex_init(&this->imu_in_mutex_,NULL);
-
   
   // [init services]
   
@@ -61,37 +59,94 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
   //this->imu_in_mutex_enter();
-  this->imu_out_Imu_msg_ = *msg;
   std::string frame;
-  if(this->config_.frame_id.size()!=0)
-  {
+  if(this->config_.fix_frame_id)
     frame=this->config_.frame_id;
-  }
   else
   {
-    frame = this->imu_out_Imu_msg_.header.frame_id;
-    
-    //if(!strncmp(frame, "/", strlen("/")))
+    frame = msg->header.frame_id;
     if(frame[0]=='/')
       frame.erase (0,1);
   }
   
-  if(this->config_.fix_cov)
+  if(this->config_.imu_reference==1)
   {
-    this->imu_out_Imu_msg_.orientation_covariance[0]=this->config_.cov_orientation;
-    this->imu_out_Imu_msg_.orientation_covariance[4]=this->config_.cov_orientation;
-    this->imu_out_Imu_msg_.orientation_covariance[8]=this->config_.cov_orientation;
+    if(this->config_.fix_cov)
+    {
+      this->imu_out_Imu_msg_.orientation_covariance[0]=this->config_.cov_orientation;
+      this->imu_out_Imu_msg_.orientation_covariance[4]=this->config_.cov_orientation;
+      this->imu_out_Imu_msg_.orientation_covariance[8]=this->config_.cov_orientation;
     
-    this->imu_out_Imu_msg_.angular_velocity_covariance[0]=this->config_.cov_vel;
-    this->imu_out_Imu_msg_.angular_velocity_covariance[4]=this->config_.cov_vel;
-    this->imu_out_Imu_msg_.angular_velocity_covariance[8]=this->config_.cov_vel;
+      this->imu_out_Imu_msg_.angular_velocity_covariance[0]=this->config_.cov_vel;
+      this->imu_out_Imu_msg_.angular_velocity_covariance[4]=this->config_.cov_vel;
+      this->imu_out_Imu_msg_.angular_velocity_covariance[8]=this->config_.cov_vel;
     
-    this->imu_out_Imu_msg_.linear_acceleration_covariance[0]=this->config_.cov_acc;
-    this->imu_out_Imu_msg_.linear_acceleration_covariance[4]=this->config_.cov_acc;
-    this->imu_out_Imu_msg_.linear_acceleration_covariance[8]=this->config_.cov_acc;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[0]=this->config_.cov_acc;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[4]=this->config_.cov_acc;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[8]=this->config_.cov_acc;
+    }
+    else 
+    {
+      this->imu_out_Imu_msg_.orientation_covariance[0]=msg->orientation_covariance[4];
+      this->imu_out_Imu_msg_.orientation_covariance[1]=msg->orientation_covariance[3];
+      this->imu_out_Imu_msg_.orientation_covariance[4]=msg->orientation_covariance[0];
+      this->imu_out_Imu_msg_.orientation_covariance[3]=msg->orientation_covariance[1];
+      this->imu_out_Imu_msg_.orientation_covariance[8]=msg->orientation_covariance[8];
+    
+      this->imu_out_Imu_msg_.angular_velocity_covariance[0]=msg->angular_velocity_covariance[4];
+      this->imu_out_Imu_msg_.angular_velocity_covariance[1]=msg->angular_velocity_covariance[3];
+      this->imu_out_Imu_msg_.angular_velocity_covariance[4]=msg->angular_velocity_covariance[0];
+      this->imu_out_Imu_msg_.angular_velocity_covariance[3]=msg->angular_velocity_covariance[1];
+      this->imu_out_Imu_msg_.angular_velocity_covariance[8]=msg->angular_velocity_covariance[8];
+    
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[0]=msg->linear_acceleration_covariance[4];
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[1]=msg->linear_acceleration_covariance[3];
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[4]=msg->linear_acceleration_covariance[0];
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[3]=msg->linear_acceleration_covariance[1];
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[8]=msg->linear_acceleration_covariance[8];
+    }
+    this->imu_out_Imu_msg_.orientation.x=msg->orientation.y;
+    this->imu_out_Imu_msg_.orientation.y=msg->orientation.x;
+    this->imu_out_Imu_msg_.orientation.z=-msg->orientation.z;
+    this->imu_out_Imu_msg_.orientation.w=-msg->orientation.w;
+
+    this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
+    this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
+    this->imu_out_Imu_msg_.angular_velocity.z=-msg->angular_velocity.z;
+
+    this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.y;
+    this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.x;
+    this->imu_out_Imu_msg_.linear_acceleration.z=-msg->linear_acceleration.z;
   }
-  
+  else
+  {
+    if(this->config_.fix_cov)
+    {
+      this->imu_out_Imu_msg_.orientation_covariance[0]=this->config_.cov_orientation;
+      this->imu_out_Imu_msg_.orientation_covariance[4]=this->config_.cov_orientation;
+      this->imu_out_Imu_msg_.orientation_covariance[8]=this->config_.cov_orientation;
+    
+      this->imu_out_Imu_msg_.angular_velocity_covariance[0]=this->config_.cov_vel;
+      this->imu_out_Imu_msg_.angular_velocity_covariance[4]=this->config_.cov_vel;
+      this->imu_out_Imu_msg_.angular_velocity_covariance[8]=this->config_.cov_vel;
+    
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[0]=this->config_.cov_acc;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[4]=this->config_.cov_acc;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance[8]=this->config_.cov_acc;
+    }
+    else 
+    {
+      this->imu_out_Imu_msg_.orientation_covariance=msg->orientation_covariance;
+      this->imu_out_Imu_msg_.angular_velocity_covariance=msg->angular_velocity_covariance;
+      this->imu_out_Imu_msg_.linear_acceleration_covariance=msg->linear_acceleration_covariance;
+    }
+    this->imu_out_Imu_msg_.orientation=msg->orientation;
+    this->imu_out_Imu_msg_.angular_velocity=msg->angular_velocity;
+    this->imu_out_Imu_msg_.linear_acceleration=msg->linear_acceleration;
+  }
+
   this->imu_out_Imu_msg_.header.frame_id = frame;
+  this->imu_out_Imu_msg_.header.stamp = msg->header.stamp;
   this->imu_out_publisher_.publish(this->imu_out_Imu_msg_);
   
 //   this->pose_PoseStamped_msg_.header = msg->header;