apply_to_imu.cpp
Go to the documentation of this file.
00001 
00036 #include <ros/ros.h>
00037 #include <tf/transform_listener.h>
00038 
00039 #include "sensor_msgs/Imu.h"
00040 #include "std_msgs/Float32.h"
00041 #include "geometry_msgs/Vector3Stamped.h"
00042 #include "geometry_msgs/QuaternionStamped.h"
00043 
00044 #include <boost/bind.hpp>
00045 
00046 
00047 class DeclinationTransform : public tf::Transform {
00048 public:
00049   void set_declination(double decl) {
00050     tf::Quaternion quat;
00051     quat.setEuler(decl, 0.0, 0.0);
00052     setRotation(quat); 
00053   }
00054 
00055   void msg(const std_msgs::Float32ConstPtr declination_msg)
00056   {
00057     set_declination(declination_msg->data);
00058   }
00059 };
00060 
00061 
00062 static void imu_to_frame(const tf::TransformListener& tf_listener,
00063                          std::string& tf_link,
00064                          const sensor_msgs::Imu& imu_in, 
00065                          sensor_msgs::Imu& imu_out)
00066 {
00067   tf::Stamped<tf::Quaternion> orient_in, orient_out;
00068   tf::quaternionMsgToTF(imu_in.orientation, orient_in);
00069   orient_in.frame_id_ = imu_in.header.frame_id;
00070   tf_listener.transformQuaternion(tf_link, orient_in, orient_out);
00071   tf::quaternionTFToMsg(orient_out, imu_out.orientation);
00072 
00073   tf::Stamped<tf::Vector3> vel_in, vel_out;
00074   tf::vector3MsgToTF(imu_in.angular_velocity, vel_in);
00075   vel_in.frame_id_ = imu_in.header.frame_id;
00076   tf_listener.transformVector(tf_link, vel_in, vel_out);
00077   tf::vector3TFToMsg(vel_out, imu_out.angular_velocity);
00078   
00079   tf::Stamped<tf::Vector3> accel_in, accel_out;
00080   tf::vector3MsgToTF(imu_in.linear_acceleration, accel_in);
00081   accel_in.frame_id_ = imu_in.header.frame_id;
00082   tf_listener.transformVector(tf_link, accel_in, accel_out);
00083   tf::vector3TFToMsg(accel_out, imu_out.linear_acceleration);
00084 
00085   imu_out.header.stamp = imu_in.header.stamp;
00086   imu_out.header.frame_id = tf_link;
00087 }
00088 
00089 
00090 static void handle_imu(const sensor_msgs::ImuConstPtr& imu_ptr,
00091                        const ros::Publisher& pub_imu,
00092                        const tf::TransformListener& tf_listener,
00093                        const DeclinationTransform& transform,
00094                        std::string tf_link)
00095 {
00096   // Must move imu into base_link frame before applying declination.
00097   sensor_msgs::Imu imu;
00098   imu_to_frame(tf_listener, tf_link, *imu_ptr, imu);
00099 
00100   // Rotate orientation by declination amount.
00101   tf::Quaternion orient;
00102   tf::quaternionMsgToTF(imu.orientation, orient); 
00103   orient = transform * orient;
00104   tf::quaternionTFToMsg(orient, imu.orientation);
00105 
00106   pub_imu.publish(imu);
00107 }
00108 
00109 
00110 int main(int argc, char **argv)
00111 {
00112   ros::init(argc, argv, "apply_declination_to_imu");
00113 
00114   ros::NodeHandle np("~");
00115   std::string tf_link;
00116   np.param<std::string>("tf_link", tf_link, "base_link");
00117 
00118   double declination_rads;
00119   np.param<double>("default", declination_rads, 0.0);
00120 
00121   DeclinationTransform declination_transform;
00122   declination_transform.set_declination(declination_rads);
00123 
00124   ros::NodeHandle n;
00125   tf::TransformListener tf_listener(n);
00126   ros::Subscriber sub = n.subscribe("declination", 5, 
00127       &DeclinationTransform::msg, &declination_transform);
00128   ros::Publisher pub_imu = n.advertise<sensor_msgs::Imu>("data_decl", 5);
00129   ros::Subscriber sub_enu = n.subscribe<sensor_msgs::Imu>("data", 5,
00130       boost::bind(handle_imu, _1, boost::ref(pub_imu), boost::ref(tf_listener), 
00131                   boost::ref(declination_transform), tf_link));
00132 
00133   ros::spin();
00134   return 0;
00135 }
00136 


declination
Author(s):
autogenerated on Fri Aug 28 2015 10:27:05