new imu_z_up parameter

include/eskf_odom_alg_node.h

@@ -72,6 +72,8 @@ private:
 	double t_ini_flow2d_; // Initial RANGE sensor time.
 	double t_ini_px4_;   // Initial PX4 sensor time.
 
+	bool imu_z_up_; //Indicates if IMU frame is upwards or downwards.
+
 	int seq_; // Odometry sequence counter.
 	std::string odom_frame_id_; // Odometry frame ID.
 	std::string robot_frame_id_; // Robot frame ID.

launch/params.yaml

@@ -11,6 +11,9 @@ dwb0_std: [0.0037, 0.0040, 0.0]
 dg0_std: [0.0, 0.0, 0.0]
 frame: global # Frame r.t. orientation derivation is computed (global or local)
 
+#IMU pose`
+imu_z_up: true
+
 #IMU STD values
 imu_a_std: [0.2,0.2,0.2]
 imu_w_std: [0.005,0.005,0.005]

src/eskf_odom_alg_node.cpp

@@ -95,6 +95,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
   imu_params.std.segment(3,3) = read_vec("imu_w_std", 3);
   imu_params.std.segment(6,3) = read_vec("imu_ab_std", 3);
   imu_params.std.segment(9,3) = read_vec("imu_wb_std", 3);
+  this->public_node_handle_.param<bool>("imu_z_up", this->imu_z_up_, true);
 
 	std::string imu_met;
 	this->public_node_handle_.param<std::string>("imu_method", imu_met, "continuous");
@@ -428,14 +429,26 @@ void EskfOdomAlgNode::imu_mutex_exit(void)
 
 void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const float& msg_time)
 {
-	// Get sensor values
-	this->imu_mutex_enter();
-	float ax = msg->linear_acceleration.x;
-	float ay = -msg->linear_acceleration.y; // axis change according to simulation
-	float az = -msg->linear_acceleration.z; // axis change according to simulation
-	float wx = msg->angular_velocity.x;
-	float wy = -msg->angular_velocity.y; // axis change according to simulation
-	float wz = -msg->angular_velocity.z; // axis change according to simulation
+  this->imu_mutex_enter();
+  float ax, ay, az, wx, wy, wz;
+  if (this->imu_z_up_)
+  {
+    ax = msg->linear_acceleration.x;
+    ay = msg->linear_acceleration.y; // axis change according to simulation
+    az = msg->linear_acceleration.z; // axis change according to simulation
+    wx = msg->angular_velocity.x;
+    wy = msg->angular_velocity.y; // axis change according to simulation
+    wz = msg->angular_velocity.z; // axis change according to simulation
+  }
+  else
+  {
+    ax = msg->linear_acceleration.x;
+    ay = -msg->linear_acceleration.y; // axis change according to simulation
+    az = -msg->linear_acceleration.z; // axis change according to simulation
+    wx = msg->angular_velocity.x;
+    wy = -msg->angular_velocity.y; // axis change according to simulation
+    wz = -msg->angular_velocity.z; // axis change according to simulation
+  }
 	this->imu_mutex_exit();
 
 	Eigen::Vector3f a;