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/tf_transform_bounding_box.h"
00038 #include "jsk_pcl_ros/pcl_conversion_util.h"
00039
00040 namespace jsk_pcl_ros
00041 {
00042
00043 void TfTransformBoundingBox::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 if (!pnh_->getParam("target_frame_id", target_frame_id_)) {
00047 JSK_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_ = TfListenerSingleton::getInstance();
00054 pub_ = advertise<jsk_recognition_msgs::BoundingBox>(*pnh_, "output", 1);
00055 }
00056
00057 void TfTransformBoundingBox::subscribe()
00058 {
00059 if (use_latest_tf_) {
00060 sub_ = pnh_->subscribe("input", 1, &TfTransformBoundingBox::transform, this);
00061 }
00062 else {
00063 sub_filter_.subscribe(*pnh_, "input", 10);
00064 tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::BoundingBox>(
00065 sub_filter_,
00066 *tf_listener_,
00067 target_frame_id_,
00068 tf_queue_size_));
00069 tf_filter_->registerCallback(boost::bind(&TfTransformBoundingBox::transform, this, _1));
00070 }
00071 }
00072
00073 void TfTransformBoundingBox::unsubscribe()
00074 {
00075 if (use_latest_tf_) {
00076 sub_.shutdown();
00077 }
00078 else {
00079 sub_filter_.unsubscribe();
00080 }
00081 }
00082
00083 void TfTransformBoundingBox::transform(
00084 const jsk_recognition_msgs::BoundingBox::ConstPtr& msg)
00085 {
00086 vital_checker_->poke();
00087 try
00088 {
00089 jsk_recognition_msgs::BoundingBox transformed_box;
00090 transformed_box.header.stamp = msg->header.stamp;
00091 transformed_box.header.frame_id = target_frame_id_;
00092 transformed_box.dimensions = msg->dimensions;
00093 tf::StampedTransform tf_transform;
00094 if (use_latest_tf_) {
00095 tf_listener_->lookupTransform(target_frame_id_, msg->header.frame_id,
00096 ros::Time(0), tf_transform);
00097 }
00098 else {
00099 tf_listener_->lookupTransform(target_frame_id_, msg->header.frame_id,
00100 msg->header.stamp, tf_transform);
00101 }
00102 Eigen::Affine3f pose;
00103 tf::poseMsgToEigen(msg->pose, pose);
00104 Eigen::Affine3f transform;
00105 tf::transformTFToEigen(tf_transform, transform);
00106 Eigen::Affine3f new_pose = transform * pose;
00107 tf::poseEigenToMsg(new_pose, transformed_box.pose);
00108 pub_.publish(transformed_box);
00109 }
00110 catch (tf2::ConnectivityException &e)
00111 {
00112 JSK_NODELET_ERROR("Transform error: %s", e.what());
00113 }
00114 catch (tf2::InvalidArgumentException &e)
00115 {
00116 JSK_NODELET_ERROR("Transform error: %s", e.what());
00117 }
00118 catch (...)
00119 {
00120 NODELET_ERROR("Unknown transform error");
00121 }
00122 }
00123
00124 }
00125 #include <pluginlib/class_list_macros.h>
00126 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::TfTransformBoundingBox, nodelet::Nodelet);