Skip to content
Snippets Groups Projects
Commit 5b83924e authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Merge branch 'ned_to_emu' into 'master' 

Added the conversion from NED to ENU.

See merge request mobile_robotics/add_project/imu_msg_fix!1
parents 3c71cf94 794377e3
No related branches found
No related tags found
1 merge request!1Added the conversion from NED to ENU.
Hide whitespace changes
Inline Side-by-side
cfg/ImuMsgFix.cfg
+ 13
− 5
View file @ 5b83924e
......@@ -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
+ 0
− 5
View file @ 5b83924e
......@@ -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
+ 2
− 1
View file @ 5b83924e
......@@ -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>
src/imu_msg_fix_alg_node.cpp
+ 75
− 20
View file @ 5b83924e
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment