artoolkitplus_node.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2013 by Markus Bader                                    *
00003  *   markus.bader@tuwien.ac.at                                             *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 #include <tuw_artoolkitplus/artoolkitplus.h>
00022 #include <tuw_artoolkitplus/artoolkitplus_defaults.h>
00023 #include <tuw_artoolkitplus/TransformArrayStamped.h>
00024 
00025 int main(int argc, char **argv) {
00026     ros::init(argc, argv, "arMarker");
00027     ros::NodeHandle n;
00028     ARToolKitPlusNode ar(n);
00029     ros::spin();
00030     return 0;
00031 }
00032 
00033 int ARToolKitPlusNode::matrix2Tf(const ARFloat M[3][4], tf::Transform &transform) {
00034     tf::Matrix3x3 R(M[0][0], M[0][1], M[0][2], M[1][0], M[1][1], M[1][2], M[2][0], M[2][1], M[2][2]);
00035     tf::Vector3 T(M[0][3], M[1][3], M[2][3]);
00036     //transform = tf::Transform(R, T); // this causes a TF to MSG: Quaternion Not Properly Normalized message
00037     tf::Quaternion quat;
00038     R.getRotation(quat);
00039     transform = tf::Transform(quat, T);
00040 }
00041 
00042 
00043 void ARToolKitPlusNode::publishTf() {
00044     for(std::list<tf::StampedTransform>::iterator it =  markerTransforms_.begin(); it != markerTransforms_.end(); it++) {
00045         transformBroadcaster_.sendTransform(*it);
00046     }
00047 }
00048 
00049 
00050 void ARToolKitPlusNode::publishPerceptions (const std_msgs::Header &header) {
00051     if(pub_perceptions_.getNumSubscribers() < 1) return;
00052     tuw_artoolkitplus::TransformArrayStamped msg;
00053     if(markerTransforms_.size() > 0) {
00054         msg.header = header;
00055         msg.child_frame_id.resize(markerTransforms_.size());
00056         msg.transform.resize(markerTransforms_.size());
00057         std::list<tf::StampedTransform>::iterator it =  markerTransforms_.begin();
00058         for(unsigned int i; i < markerTransforms_.size(); it++, i++) {
00059             geometry_msgs::Vector3 &desT = msg.transform[i].translation;
00060             geometry_msgs::Quaternion &desQ = msg.transform[i].rotation;
00061             tf::Vector3 &srcT = it->getOrigin();
00062             tf::Quaternion srcQ = it->getRotation();
00063             desT.x = srcT.x(), desT.y = srcT.y(), desT.z = srcT.z();
00064             desQ.x = srcQ.x(), desQ.y = srcQ.y(), desQ.z = srcQ.z(), desQ.w = srcQ.w();
00065             msg.child_frame_id[i] = it->child_frame_id_;
00066         }
00067         pub_perceptions_.publish(msg);
00068     }
00069 }
00070 


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun May 29 2016 02:50:12