32 #include <sensor_msgs/Imu.h>
33 #include "geometry_msgs/Vector3Stamped.h"
34 #include "geometry_msgs/QuaternionStamped.h"
36 #include "geometry_msgs/Pose.h"
37 #include <sensor_msgs/JointState.h>
38 #include <tf/transform_listener.h>
39 #include <nav_msgs/Odometry.h>
40 #include "geometry_msgs/Pose.h"
41 #include "geometry_msgs/Quaternion.h"
42 #include "tf/transform_datatypes.h"
44 #include <std_msgs/Float64.h>
45 #include <tf/transform_broadcaster.h>
69 tf::StampedTransform transform;
72 transform.stamp_=msg->header.stamp;
73 transform.frame_id_=
"base_footprint";
74 transform.child_frame_id_ =
"/imu_frame";
75 tf::Vector3 transl(0,0,0);
79 transform.setOrigin(transl);
81 tf::Quaternion odom_quat_ = tf::createQuaternionFromRPY(0,0,0);
82 transform.setRotation(odom_quat_);
89 tf::Quaternion orientation;
90 tf::quaternionMsgToTF(msg->orientation, orientation);
91 double roll,pitch,yaw;
92 tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
94 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
97 inercial_.orientation_covariance[0]= 0.000001;
98 inercial_.orientation_covariance[4]= 0.000001;
99 inercial_.orientation_covariance[8]= 0.000001;
100 inercial_.angular_velocity_covariance[0]= 0.000304617;
101 inercial_.angular_velocity_covariance[4]= 0.000304617;
102 inercial_.angular_velocity_covariance[8]= 0.000304617;
103 inercial_.linear_acceleration_covariance[0]= 0.0000004;
104 inercial_.linear_acceleration_covariance[4]= 0.0000004;
105 inercial_.linear_acceleration_covariance[8]= 0.0000004;
114 odometry_.header.stamp=msg->header.stamp;
116 odometry_.child_frame_id=
"base_footprint";
118 odometry_.pose.covariance[0]=msg->pose.covariance[0];
119 odometry_.pose.covariance[7]=msg->pose.covariance[7];
120 odometry_.pose.covariance[14]=99999999999;
121 odometry_.pose.covariance[21]=99999999999;
122 odometry_.pose.covariance[28]=99999999999;
123 odometry_.pose.covariance[35]=msg->pose.covariance[35];
141 int main(
int argc,
char** argv)
145 ros::init(argc, argv,
"imu_egomotion");
146 ROS_INFO(
"Starting message edition node");
149 tf::TransformBroadcaster odom_broadcaster;
152 odom_pub = n.advertise<nav_msgs::Odometry>(
"odom", 50);
153 imu_pub = n.advertise<sensor_msgs::Imu>(
"/imu_data", 50);
155 ros::Subscriber imu= n.subscribe(
"/atc/imu/data",1,
ImuCallback);
159 ros::Rate loop_rate(1000);
void ImuCallback(const sensor_msgs::Imu::ConstPtr &msg)
ImuCallback Function to add covariance to IMU data.
nav_msgs::Odometry odometry_
int main(int argc, char **argv)
tf::TransformBroadcaster * p_odom_broadcaster
sensor_msgs::Imu inercial_
void OdometryCallback(const nav_msgs::Odometry::ConstPtr &msg)