Go to the documentation of this file.00001 #include "imu_transformer/imu_transformer_nodelet.h"
00002 #include "pluginlib/class_list_macros.h"
00003 #include "geometry_msgs/Vector3Stamped.h"
00004
00005
00006
00007 #include "imu_transformer/tf2_sensor_msgs.h"
00008
00009 namespace imu_transformer
00010 {
00011
00012 void ImuTransformerNodelet::onInit(){
00013
00014 nh_in_ = ros::NodeHandle(getNodeHandle(), "imu_in");
00015 nh_out_ = ros::NodeHandle(getNodeHandle(), "imu_out");
00016 private_nh_ = getPrivateNodeHandle();
00017
00018 private_nh_.param<std::string>("target_frame", target_frame_, "base_link");
00019
00020 tf2_.reset(new tf2_ros::Buffer());
00021 tf2_listener_.reset(new tf2_ros::TransformListener(*tf2_));
00022
00023 imu_sub_.subscribe(nh_in_, "data", 10);
00024 imu_filter_.reset(new ImuFilter(imu_sub_, *tf2_, target_frame_, 10, nh_in_));
00025 imu_filter_->registerCallback(boost::bind(&ImuTransformerNodelet::imuCallback, this, _1));
00026 imu_filter_->registerFailureCallback(boost::bind(&ImuTransformerNodelet::failureCb, this, _2));
00027
00028 mag_sub_.subscribe(nh_in_, "mag", 10);
00029
00030
00031
00032
00033
00034 mag_sub_.registerCallback(boost::bind(&ImuTransformerNodelet::magCallback, this, _1));
00035 }
00036
00037 void ImuTransformerNodelet::imuCallback(const ImuMsg::ConstPtr &imu_in)
00038 {
00039 if(imu_pub_.getTopic().empty()){
00040 imu_pub_ = nh_out_.advertise<ImuMsg>("data", 10);
00041 }
00042
00043 try
00044 {
00045 ImuMsg imu_out;
00046 tf2_->transform(*imu_in, imu_out, target_frame_);
00047 imu_pub_.publish(imu_out);
00048 }
00049 catch (tf2::TransformException ex)
00050 {
00051 NODELET_ERROR_STREAM_THROTTLE(1.0, "IMU Transform failure: " << ex.what());
00052 return;
00053 }
00054 }
00055
00056
00057
00058 void ImuTransformerNodelet::magCallback(const topic_tools::ShapeShifter::ConstPtr &msg)
00059 {
00060
00061 std::string error;
00062 try
00063 {
00064 MagMsg::ConstPtr mag_in = msg->instantiate<MagMsg>();
00065
00066 if(tf2_->canTransform(target_frame_, mag_in->header.frame_id, mag_in->header.stamp, &error)){
00067
00068 if(mag_pub_.getTopic().empty()){
00069 mag_pub_ = nh_out_.advertise<MagMsg>("mag", 10);
00070 }
00071
00072 MagMsg out;
00073 tf2_->transform(*mag_in, out, target_frame_);
00074 mag_pub_.publish(out);
00075 }else{
00076 NODELET_WARN_STREAM_THROTTLE(1.0, error);
00077 }
00078 return;
00079 }
00080 catch (topic_tools::ShapeShifterException &e) {
00081 NODELET_DEBUG_STREAM(e.what());
00082 }
00083
00084 try
00085 {
00086 geometry_msgs::Vector3Stamped::ConstPtr mag_in = msg->instantiate<geometry_msgs::Vector3Stamped>();
00087
00088 if(tf2_->canTransform(target_frame_, mag_in->header.frame_id, mag_in->header.stamp, &error)){
00089
00090 if(mag_pub_.getTopic().empty()){
00091 mag_pub_ = nh_out_.advertise<geometry_msgs::Vector3Stamped>("mag", 10);
00092 }
00093
00094 MagMsg mag_temp_in, mag_temp_out;
00095 geometry_msgs::Vector3Stamped mag_out;
00096
00097 mag_temp_in.header = mag_in->header;
00098 mag_temp_in.magnetic_field = mag_in->vector;
00099 tf2_->transform(mag_temp_in, mag_temp_out, target_frame_);
00100 mag_out.header = mag_temp_out.header;
00101 mag_out.vector = mag_temp_out.magnetic_field;
00102
00103 mag_pub_.publish(mag_out);
00104 }else{
00105 NODELET_WARN_STREAM_THROTTLE(1.0, error);
00106 }
00107 return;
00108 }
00109 catch (topic_tools::ShapeShifterException &e) {
00110 NODELET_DEBUG_STREAM(e.what());
00111 }
00112
00113 NODELET_ERROR_STREAM_THROTTLE(1.0, "imu_transformer only accepts sensor_msgs::MagneticField and "
00114 "geometry_msgs::Vector3Stamped message types on the imu_in/mag_in topic, received " << msg->getDataType());
00115
00116 }
00117
00118 void ImuTransformerNodelet::failureCb(tf2_ros::filter_failure_reasons::FilterFailureReason reason)
00119 {
00120 NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform incoming IMU data to " << target_frame_ << " " <<
00121 reason);
00122 }
00123
00124 }
00125
00126 PLUGINLIB_EXPORT_CLASS(imu_transformer::ImuTransformerNodelet, nodelet::Nodelet)