diff --git a/cfg/ImuMsgFix.cfg b/cfg/ImuMsgFix.cfg
index cc677fcb7aa41bcd6d66c1be9c59a31a1580817d..4fb2a3cc2a26baad87446b38861888d052e73310 100755
--- a/cfg/ImuMsgFix.cfg
+++ b/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"))
diff --git a/include/imu_msg_fix_alg_node.h b/include/imu_msg_fix_alg_node.h
index 01a0688780b94afa6a8796295362a9052c6e3247..9a3b16b9cc37104cc685b4a76acfb2189821993d 100755
--- a/include/imu_msg_fix_alg_node.h
+++ b/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]
diff --git a/launch/node.launch b/launch/node.launch
index 5e05de8d8263592fbb4c7ef54c2b60f28c227ad7..2095b069b6d6864f38a907d22f68fa1e1cea0c20 100755
--- a/launch/node.launch
+++ b/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>
\ No newline at end of file
+</launch>
diff --git a/src/imu_msg_fix_alg_node.cpp b/src/imu_msg_fix_alg_node.cpp
index 957fea1a940f4f28933ceeb0614aaf5cd1f1740e..de0786c7a79d14dd5ed8dceaa299f5c4fd077685 100755
--- a/src/imu_msg_fix_alg_node.cpp
+++ b/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;