imu_transformer_nodelet.cpp
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 //#include "tf2_sensor_msgs/tf2_sensor_msgs.h"
00006 //Remove this header when https://github.com/ros/geometry_experimental/pull/78 is released
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     // TF Message filter doesn't support ShapeShifter, which we use for Mag messages. Uncomment when IMU Rep goes into
00030     // effect and we don't need to support two types of Mag message.
00031 //      mag_filter_.reset(new MagFilter(mag_sub_, *tf2_, target_frame_, 10, nh_in_));
00032 //      mag_filter_->registerCallback(boost::bind(&ImuTransformerNodelet::magCallback, this, _1));
00033 //      mag_filter_->registerFailureCallback(boost::bind(&ImuTransformerNodelet::failureCb, this, _2));
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   // Need to support two types of magnemoter message for now, replace with MagMsg subscriber when IMU REP goes into
00057   // effect
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)


imu_transformer
Author(s): Paul Bovbel
autogenerated on Sat Feb 2 2019 03:39:37