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