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_array.h"
00038 #include "jsk_recognition_utils/pcl_conversion_util.h"
00039
00040 namespace jsk_pcl_ros_utils
00041 {
00042
00043 void TfTransformBoundingBoxArray::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::BoundingBoxArray>(*pnh_, "output", 1);
00055
00056 onInitPostProcess();
00057 }
00058
00059 void TfTransformBoundingBoxArray::subscribe()
00060 {
00061 if (use_latest_tf_) {
00062 sub_ = pnh_->subscribe("input", 1, &TfTransformBoundingBoxArray::transform, this);
00063 }
00064 else {
00065 sub_filter_.subscribe(*pnh_, "input", 10);
00066 tf_filter_.reset(new tf::MessageFilter<jsk_recognition_msgs::BoundingBoxArray>(
00067 sub_filter_,
00068 *tf_listener_,
00069 target_frame_id_,
00070 tf_queue_size_));
00071 tf_filter_->registerCallback(boost::bind(&TfTransformBoundingBoxArray::transform, this, _1));
00072 }
00073 }
00074
00075 void TfTransformBoundingBoxArray::unsubscribe()
00076 {
00077 if (use_latest_tf_) {
00078 sub_.shutdown();
00079 }
00080 else {
00081 sub_filter_.unsubscribe();
00082 }
00083 }
00084
00085 void TfTransformBoundingBoxArray::transform(
00086 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg)
00087 {
00088 vital_checker_->poke();
00089 try
00090 {
00091 jsk_recognition_msgs::BoundingBoxArray transformed_box;
00092 transformed_box.header.stamp = msg->header.stamp;
00093 transformed_box.header.frame_id = target_frame_id_;
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
00104 Eigen::Affine3f transform;
00105 tf::transformTFToEigen(tf_transform, transform);
00106 for (size_t i = 0; i < msg->boxes.size(); i++) {
00107 jsk_recognition_msgs::BoundingBox box = jsk_recognition_msgs::BoundingBox(msg->boxes[i]);
00108 Eigen::Affine3f pose;
00109 tf::poseMsgToEigen(box.pose, pose);
00110 Eigen::Affine3f new_pose = transform * pose;
00111 tf::poseEigenToMsg(new_pose, box.pose);
00112 box.header.frame_id = target_frame_id_;
00113 transformed_box.boxes.push_back(box);
00114 }
00115 pub_.publish(transformed_box);
00116 }
00117 catch (tf2::ConnectivityException &e)
00118 {
00119 NODELET_ERROR("Transform error: %s", e.what());
00120 }
00121 catch (tf2::InvalidArgumentException &e)
00122 {
00123 NODELET_ERROR("Transform error: %s", e.what());
00124 }
00125 catch (...)
00126 {
00127 NODELET_ERROR("Unknown transform error");
00128 }
00129 }
00130
00131 }
00132 #include <pluginlib/class_list_macros.h>
00133 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::TfTransformBoundingBoxArray, nodelet::Nodelet);