apply_to_imu.cpp
Go to the documentation of this file.
1 
36 #include <ros/ros.h>
37 #include <tf/transform_listener.h>
38 
39 #include "sensor_msgs/Imu.h"
40 #include "std_msgs/Float32.h"
41 #include "geometry_msgs/Vector3Stamped.h"
42 #include "geometry_msgs/QuaternionStamped.h"
43 
44 #include <boost/bind.hpp>
45 
46 
48 public:
49  void set_declination(double decl) {
50  tf::Quaternion quat;
51  quat.setEuler(decl, 0.0, 0.0);
52  setRotation(quat);
53  }
54 
55  void msg(const std_msgs::Float32ConstPtr declination_msg)
56  {
57  set_declination(declination_msg->data);
58  }
59 };
60 
61 
62 static void imu_to_frame(const tf::TransformListener& tf_listener,
63  std::string& tf_link,
64  const sensor_msgs::Imu& imu_in,
65  sensor_msgs::Imu& imu_out)
66 {
67  tf::Stamped<tf::Quaternion> orient_in, orient_out;
68  tf::quaternionMsgToTF(imu_in.orientation, orient_in);
69  orient_in.frame_id_ = imu_in.header.frame_id;
70  tf_listener.transformQuaternion(tf_link, orient_in, orient_out);
71  tf::quaternionTFToMsg(orient_out, imu_out.orientation);
72 
73  tf::Stamped<tf::Vector3> vel_in, vel_out;
74  tf::vector3MsgToTF(imu_in.angular_velocity, vel_in);
75  vel_in.frame_id_ = imu_in.header.frame_id;
76  tf_listener.transformVector(tf_link, vel_in, vel_out);
77  tf::vector3TFToMsg(vel_out, imu_out.angular_velocity);
78 
79  tf::Stamped<tf::Vector3> accel_in, accel_out;
80  tf::vector3MsgToTF(imu_in.linear_acceleration, accel_in);
81  accel_in.frame_id_ = imu_in.header.frame_id;
82  tf_listener.transformVector(tf_link, accel_in, accel_out);
83  tf::vector3TFToMsg(accel_out, imu_out.linear_acceleration);
84 
85  imu_out.header.stamp = imu_in.header.stamp;
86  imu_out.header.frame_id = tf_link;
87 }
88 
89 
90 static void handle_imu(const sensor_msgs::ImuConstPtr& imu_ptr,
91  const ros::Publisher& pub_imu,
92  const tf::TransformListener& tf_listener,
93  const DeclinationTransform& transform,
94  std::string tf_link)
95 {
96  // Must move imu into base_link frame before applying declination.
97  sensor_msgs::Imu imu;
98  imu_to_frame(tf_listener, tf_link, *imu_ptr, imu);
99 
100  // Rotate orientation by declination amount.
101  tf::Quaternion orient;
102  tf::quaternionMsgToTF(imu.orientation, orient);
103  orient = transform * orient;
104  tf::quaternionTFToMsg(orient, imu.orientation);
105 
106  pub_imu.publish(imu);
107 }
108 
109 
110 int main(int argc, char **argv)
111 {
112  ros::init(argc, argv, "apply_declination_to_imu");
113 
114  ros::NodeHandle np("~");
115  std::string tf_link;
116  np.param<std::string>("tf_link", tf_link, "base_link");
117 
118  double declination_rads;
119  np.param<double>("default", declination_rads, 0.0);
120 
121  DeclinationTransform declination_transform;
122  declination_transform.set_declination(declination_rads);
123 
124  ros::NodeHandle n;
125  tf::TransformListener tf_listener(n);
126  ros::Subscriber sub = n.subscribe("declination", 5,
127  &DeclinationTransform::msg, &declination_transform);
128  ros::Publisher pub_imu = n.advertise<sensor_msgs::Imu>("data_decl", 5);
129  ros::Subscriber sub_enu = n.subscribe<sensor_msgs::Imu>("data", 5,
130  boost::bind(handle_imu, _1, boost::ref(pub_imu), boost::ref(tf_listener),
131  boost::ref(declination_transform), tf_link));
132 
133  ros::spin();
134  return 0;
135 }
136 
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
static void handle_imu(const sensor_msgs::ImuConstPtr &imu_ptr, const ros::Publisher &pub_imu, const tf::TransformListener &tf_listener, const DeclinationTransform &transform, std::string tf_link)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void vector3TFToMsg(const Vector3 &bt_v, geometry_msgs::Vector3 &msg_v)
void msg(const std_msgs::Float32ConstPtr declination_msg)
void set_declination(double decl)
ROSCPP_DECL void spin(Spinner &spinner)
void transformQuaternion(const std::string &target_frame, const geometry_msgs::QuaternionStamped &stamped_in, geometry_msgs::QuaternionStamped &stamped_out) const
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
std::string frame_id_
int main(int argc, char **argv)
static void imu_to_frame(const tf::TransformListener &tf_listener, std::string &tf_link, const sensor_msgs::Imu &imu_in, sensor_msgs::Imu &imu_out)
void transformVector(const std::string &target_frame, const geometry_msgs::Vector3Stamped &stamped_in, geometry_msgs::Vector3Stamped &stamped_out) const


declination
Author(s):
autogenerated on Sat May 23 2020 03:41:45