diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h
index 2fb830f86989a9644ec016de5f267287a9d0ce9c..c7b8cfda5f7fd329954fcf5fceb73d232aa05bf5 100644
--- a/include/subscriber_imu.h
+++ b/include/subscriber_imu.h
@@ -25,13 +25,11 @@
/**************************
* WOLF includes *
**************************/
-#include <core/yaml/parser_yaml.h>
-#include <core/common/wolf.h>
-#include <core/problem/problem.h>
-#include <core/utils/params_server.h>
-#include <imu/capture/capture_imu.h>
-#include <imu/sensor/sensor_imu.h>
-#include <core/processor/processor_motion.h>
+#include "core/yaml/parser_yaml.h"
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/processor/processor_motion.h"
+#include "core/utils/params_server.h"
/**************************
* ROS includes *
@@ -59,8 +57,8 @@ class SubscriberImu : public Subscriber
protected:
bool in_degrees_;
std::string cov_source_;
- bool publish_bias_;
- ros::Publisher pub_bias_accel_, pub_bias_gyro_, pub_imu_corrected_, pub_imu_remapped_, pub_pose_from_origin_;
+ bool non_orthogonal_gravity_;
+ ros::Publisher pub_bias_accel_, pub_bias_gyro_, pub_imu_corrected_, pub_imu_remapped_, pub_pose_from_origin_, pub_slope_;
int imu_x_axis_, imu_y_axis_, imu_z_axis_;
bool imu_x_neg_, imu_y_neg_, imu_z_neg_;
ProcessorMotionPtr processor_motion_ptr_;
@@ -75,10 +73,6 @@ class SubscriberImu : public Subscriber
virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
void callback(const sensor_msgs::Imu::ConstPtr& msg);
-
- protected:
-
- void publishBias() const;
};
WOLF_REGISTER_SUBSCRIBER(SubscriberImu)
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 2de7c427a31ed1788e2d3427f4c8beaab3f21c51..a3992452a51aef6924c1d2a4af4541eb42894819 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -20,7 +20,11 @@
//
//--------LICENSE_END--------
#include "../include/subscriber_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/sensor/sensor_imu2d.h"
#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "core/math/SE3.h"
#include "core/math/SE2.h"
@@ -31,52 +35,31 @@ using namespace wolf;
SubscriberImu::SubscriberImu(const std::string& _unique_name,
const ParamsServer& _server,
const SensorBasePtr _sensor_ptr) :
- Subscriber(_unique_name, _server, _sensor_ptr),
- publish_bias_(false)
+ Subscriber(_unique_name, _server, _sensor_ptr)
{
assert((_sensor_ptr->getProblem()->getDim() == 3 or _sensor_ptr->getProblem()->getDim() == 2) && "the problem's dimmension should be 2 or 3");
assert(_sensor_ptr->getProblem()->getDim() != 3 or std::dynamic_pointer_cast<SensorImu>(_sensor_ptr) != nullptr);
- //TO BE UNCOMENTED assert(_sensor_ptr->getProblem()->getDim() != 2 or std::dynamic_pointer_cast<SensorImu2d>(_sensor_ptr) != nullptr);
+ assert(_sensor_ptr->getProblem()->getDim() != 2 or std::dynamic_pointer_cast<SensorImu2d>(_sensor_ptr) != nullptr);
+
+ non_orthogonal_gravity_ = std::dynamic_pointer_cast<SensorImu2d>(sensor_ptr_) != nullptr and
+ not std::static_pointer_cast<SensorImu2d>(sensor_ptr_)->isGravityOrthogonal();
cov_source_ = _server.getParam<std::string>(prefix_ + "/cov_source");
assert(cov_source_ == "msg" or cov_source_=="sensor");
- try
- {
- in_degrees_ = _server.getParam<bool>(prefix_ + "/in_degrees");
- }
- catch(...)
- {
- WOLF_INFO("SubscriberImu: parameter 'in_degrees' not set, Using default: false (gyro in rad/s)");
- in_degrees_ = false;
- }
-
+ in_degrees_ = getParamWithDefault<bool>(_server, prefix_ + "/in_degrees", false);
+
// AXES MAPPING
- try
- {
- int imu_x_axis = _server.getParam<int>(prefix_ + "/imu_x_axis");
- int imu_y_axis = _server.getParam<int>(prefix_ + "/imu_y_axis");
- int imu_z_axis = _server.getParam<int>(prefix_ + "/imu_z_axis");
- imu_x_axis_ = abs(imu_x_axis)-1;
- imu_y_axis_ = abs(imu_y_axis)-1;
- imu_z_axis_ = abs(imu_z_axis)-1;
- imu_x_neg_ = imu_x_axis < 0;
- imu_y_neg_ = imu_y_axis < 0;
- imu_z_neg_ = imu_z_axis < 0;
- //WOLF_INFO("SubscriberImu: parameters: imu_x_axis = ", imu_x_axis, " imu_y_axis = ", imu_y_axis, " imu_z_axis = ", imu_z_axis);
- //WOLF_INFO("SubscriberImu: parameters: imu_x_axis_ = ", imu_x_axis_, " imu_y_axis_ = ", imu_y_axis_, " imu_z_axis_ = ", imu_z_axis_);
- //WOLF_INFO("SubscriberImu: parameters: imu_x_neg_ = ", imu_x_neg_, " imu_y_neg_ = ", imu_y_neg_, " imu_z_neg_ = ", imu_z_neg_);
- }
- catch(...)
- {
- WOLF_INFO("SubscriberImu: parameters 'imu_x_axis'm, 'imu_y_axis' and 'imu_z_axis' not set. Using default values (1,2 and 3)");
- imu_x_axis_ = 0;
- imu_y_axis_ = 1;
- imu_z_axis_ = 2;
- imu_x_neg_ = false;
- imu_y_neg_ = false;
- imu_z_neg_ = false;
- }
+ int imu_x_axis = getParamWithDefault<int>(_server, prefix_ + "/imu_x_axis", 1);
+ int imu_y_axis = getParamWithDefault<int>(_server, prefix_ + "/imu_y_axis", 2);
+ int imu_z_axis = getParamWithDefault<int>(_server, prefix_ + "/imu_z_axis", 3);
+ imu_x_axis_ = abs(imu_x_axis)-1;
+ imu_y_axis_ = abs(imu_y_axis)-1;
+ imu_z_axis_ = abs(imu_z_axis)-1;
+ imu_x_neg_ = imu_x_axis < 0;
+ imu_y_neg_ = imu_y_axis < 0;
+ imu_z_neg_ = imu_z_axis < 0;
+
if (imu_x_axis_ < 0 or imu_x_axis_ > 2 or
imu_y_axis_ < 0 or imu_y_axis_ > 2 or
imu_z_axis_ < 0 or imu_z_axis_ > 2 or
@@ -93,11 +76,12 @@ void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic)
{
sub_ = nh.subscribe(topic, 100, &SubscriberImu::callback, this);
- pub_bias_accel_ = nh.advertise<geometry_msgs::Vector3> (topic + "_bias_accel", 1);
- pub_bias_gyro_ = nh.advertise<geometry_msgs::Vector3> (topic + "_bias_gyro", 1);
- pub_imu_corrected_ = nh.advertise<sensor_msgs::Imu> (topic + "_corrected", 1);
- pub_imu_remapped_ = nh.advertise<sensor_msgs::Imu> (topic + "_remapped", 1);
- pub_pose_from_origin_ = nh.advertise<geometry_msgs::PoseStamped> (topic + "_pose_from_origin", 1);
+ pub_bias_accel_ = nh.advertise<geometry_msgs::Vector3> (topic + "_bias_accel", 1);
+ pub_bias_gyro_ = nh.advertise<geometry_msgs::Vector3> (topic + "_bias_gyro", 1);
+ pub_imu_corrected_ = nh.advertise<sensor_msgs::Imu> (topic + "_corrected", 1);
+ pub_imu_remapped_ = nh.advertise<sensor_msgs::Imu> (topic + "_remapped", 1);
+ pub_pose_from_origin_ = nh.advertise<geometry_msgs::PoseStamped> (topic + "_pose_from_origin", 1);
+ pub_slope_ = nh.advertise<geometry_msgs::Vector3Stamped> (topic + "_slope_and_orientation", 1);
}
void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
@@ -327,4 +311,34 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
pub_pose_from_origin_.publish(pose_msg);
}
+ // Publish gravity projection onto sloped 2D plane
+ if (pub_slope_.getNumSubscribers() != 0)
+ {
+ Eigen::Vector2d g_local_xy;
+
+ if (non_orthogonal_gravity_)
+ g_local_xy = std::static_pointer_cast<SensorImu2d>(sensor_ptr_)->getStateVector("G");
+
+ else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
+ {
+ auto q_first = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
+ g_local_xy = (q_first.conjugate() * gravity()).head<2>();
+ }
+ else
+ return;
+
+ // Fill msg
+ geometry_msgs::Vector3Stamped g_msg;
+ g_msg.header.stamp = msg->header.stamp;
+ g_msg.header.frame_id = "/map";
+ // slope
+ g_msg.vector.x = asin(g_local_xy.norm() / gravity().norm());
+ // orientation
+ g_msg.vector.y = atan2(g_local_xy(1),g_local_xy(0));
+ // empty
+ g_msg.vector.z = 0;
+
+ // publish
+ pub_slope_.publish(g_msg);
+ }
}