Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036
00037 #include "jsk_pcl_ros_utils/tf_transform_bounding_box.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039
00040 namespace jsk_pcl_ros_utils
00041 {
00042
00043 void TfTransformBoundingBox::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 if (!pnh_->getParam("target_frame_id", target_frame_id_)) {
00047 ROS_FATAL("~target_frame_id is not specified");
00048 return;
00049 }
00050
00051 pnh_->param("use_latest_tf", use_latest_tf_, false);
00052 pnh_->param("tf_queue_size", tf_queue_size_, 10);
00053 tf_listener_ = jsk_recognition_utils::TfListenerSingleton::getInstance();
00054 pub_ = advertise<jsk_recognition_msgs::BoundingBox>(*pnh_, "output", 1);
00055 onInitPostProcess();
00056 }
00057
00058 void TfTransformBoundingBox::subscribe()
00059 {
00060 if (use_latest_tf_) {
00061 sub_ = pnh_->subscribe("input", 1, &TfTransformBoundingBox::transform, this);
00062 }
00063 else {
00064 sub_filter_.subscribe(*pnh_, "input", 10);
00065 tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::BoundingBox>(
00066 sub_filter_,
00067 *tf_listener_,
00068 target_frame_id_,
00069 tf_queue_size_));
00070 tf_filter_->registerCallback(boost::bind(&TfTransformBoundingBox::transform, this, _1));
00071 }
00072 }
00073
00074 void TfTransformBoundingBox::unsubscribe()
00075 {
00076 if (use_latest_tf_) {
00077 sub_.shutdown();
00078 }
00079 else {
00080 sub_filter_.unsubscribe();
00081 }
00082 }
00083
00084 void TfTransformBoundingBox::transform(
00085 const jsk_recognition_msgs::BoundingBox::ConstPtr& msg)
00086 {
00087 vital_checker_->poke();
00088 try
00089 {
00090 jsk_recognition_msgs::BoundingBox transformed_box;
00091 transformed_box.header.stamp = msg->header.stamp;
00092 transformed_box.header.frame_id = target_frame_id_;
00093 transformed_box.dimensions = msg->dimensions;
00094 tf::StampedTransform tf_transform;
00095 if (use_latest_tf_) {
00096 tf_listener_->lookupTransform(target_frame_id_, msg->header.frame_id,
00097 ros::Time(0), tf_transform);
00098 }
00099 else {
00100 tf_listener_->lookupTransform(target_frame_id_, msg->header.frame_id,
00101 msg->header.stamp, tf_transform);
00102 }
00103 Eigen::Affine3f pose;
00104 tf::poseMsgToEigen(msg->pose, pose);
00105 Eigen::Affine3f transform;
00106 tf::transformTFToEigen(tf_transform, transform);
00107 Eigen::Affine3f new_pose = transform * pose;
00108 tf::poseEigenToMsg(new_pose, transformed_box.pose);
00109 pub_.publish(transformed_box);
00110 }
00111 catch (tf2::ConnectivityException &e)
00112 {
00113 NODELET_ERROR("Transform error: %s", e.what());
00114 }
00115 catch (tf2::InvalidArgumentException &e)
00116 {
00117 NODELET_ERROR("Transform error: %s", e.what());
00118 }
00119 catch (...)
00120 {
00121 NODELET_ERROR("Unknown transform error");
00122 }
00123 }
00124
00125 }
00126 #include <pluginlib/class_list_macros.h>
00127 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::TfTransformBoundingBox, nodelet::Nodelet);