objectdetection_transform_echo.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <message_filters/subscriber.h>
00037 #include <message_filters/synchronizer.h>
00038 #include <message_filters/sync_policies/approximate_time.h>
00039 #include <posedetection_msgs/ObjectDetection.h>
00040 #include <ros/ros.h>
00041 #include <tf/transform_listener.h>
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <tf2/exceptions.h>
00044 
00045 tf::TransformListener* g_listener_;
00046 
00047 void getTransformation(const geometry_msgs::Pose& pose1,
00048                        const geometry_msgs::Pose& pose2,
00049                        Eigen::Vector3d& pos,
00050                        Eigen::Quaterniond& rot)
00051 {
00052   Eigen::Affine3d affine1, affine2;
00053   tf::poseMsgToEigen(pose1, affine1);
00054   tf::poseMsgToEigen(pose2, affine2);
00055     
00056   // compute transformation between two transform...
00057   Eigen::Affine3d transform = affine1 * affine2.inverse();
00058   pos = Eigen::Vector3d(transform.translation());
00059   rot = Eigen::Quaterniond(transform.rotation());
00060 }
00061 
00062 
00063 void callback(const posedetection_msgs::ObjectDetection::ConstPtr& detection1,
00064               const posedetection_msgs::ObjectDetection::ConstPtr& detection2)
00065 {
00066   if (detection1->objects.size() > 0 && detection2->objects.size() > 0) {
00067     geometry_msgs::Pose pose1 = detection1->objects[0].pose;
00068     geometry_msgs::Pose pose2 = detection2->objects[0].pose;
00069     Eigen::Vector3d pos;
00070     Eigen::Quaterniond rot;
00071     getTransformation(pose1, pose2, pos, rot);
00072     ROS_INFO("without tf resolving::");
00073     ROS_INFO("  pos: [%f %f %f]", pos[0], pos[1], pos[2]);
00074     ROS_INFO("  rot: [%f %f %f %f]", rot.x(), rot.y(), rot.z(), rot.w());
00075     if (detection1->header.frame_id != detection2->header.frame_id) {
00076       geometry_msgs::PoseStamped pose2_stamped, pose2_stamped_transformed;
00077       pose2_stamped.header = detection2->header;
00078       pose2_stamped.pose = pose2;
00079       try {
00080         g_listener_->transformPose(detection1->header.frame_id,
00081                                    pose2_stamped,
00082                                    pose2_stamped_transformed);
00083         geometry_msgs::Pose pose2_transformed = pose2_stamped_transformed.pose;
00084         Eigen::Vector3d pos_transformed;
00085         Eigen::Quaterniond rot_transformed;
00086         getTransformation(pose1, pose2_transformed, pos_transformed, rot_transformed);
00087         ROS_INFO("with tf resolving::");
00088         ROS_INFO("  pos: [%f %f %f]", pos_transformed[0], pos_transformed[1], pos_transformed[2]);
00089         ROS_INFO("  rot: [%f %f %f %f]", rot_transformed.x(), rot_transformed.y(), rot_transformed.z(), rot_transformed.w());
00090       }
00091       catch(tf2::ConnectivityException &e) {
00092         ROS_ERROR("failed to resolve tf: %s", e.what());
00093       }
00094       catch(tf2::InvalidArgumentException &e) {
00095         ROS_ERROR("failed to resolve tf: %s", e.what());
00096       }
00097       catch(tf2::TransformException &e) {
00098         ROS_ERROR("failed to resolve tf: %s", e.what());
00099       }
00100     }
00101     
00102   }
00103 }
00104 
00105 
00106 int main(int argc, char** argv)
00107 {
00108   ros::init(argc, argv, "objectdetection_transform_echo");
00109   g_listener_ = new tf::TransformListener();
00110   typedef message_filters::sync_policies::ApproximateTime<
00111     posedetection_msgs::ObjectDetection, posedetection_msgs::ObjectDetection>
00112     SyncPolicy;
00113   
00114   ros::NodeHandle nh;
00115   message_filters::Subscriber<posedetection_msgs::ObjectDetection>
00116     detection1_sub(nh, "detection1", 1);
00117   message_filters::Subscriber<posedetection_msgs::ObjectDetection>
00118     detection2_sub(nh, "detection2", 1);
00119   
00120   message_filters::Synchronizer<SyncPolicy> sync(
00121     SyncPolicy(1000),
00122     detection1_sub, detection2_sub);
00123   sync.registerCallback(boost::bind(&callback, _1, _2));
00124   ros::spin();
00125   return 0;
00126 }


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Wed Sep 16 2015 04:37:41