artoolkitplus_node.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2013 by Markus Bader *
3  * markus.bader@tuwien.ac.at *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
23 #include <tuw_artoolkitplus/TransformArrayStamped.h>
24 
25 int main(int argc, char **argv) {
26  ros::init(argc, argv, "arMarker");
28  ARToolKitPlusNode ar(n);
29  ros::spin();
30  return 0;
31 }
32 
33 void ARToolKitPlusNode::matrix2Tf(const ARFloat M[3][4], tf::Transform &transform) {
34  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]);
35  tf::Vector3 T(M[0][3], M[1][3], M[2][3]);
36  //transform = tf::Transform(R, T); // this causes a TF to MSG: Quaternion Not Properly Normalized message
37  tf::Quaternion quat;
38  R.getRotation(quat);
39  transform = tf::Transform(quat, T);
40 }
41 
42 
44  for(std::list<tf::StampedTransform>::iterator it = markerTransforms_.begin(); it != markerTransforms_.end(); it++) {
46  }
47 }
48 
49 void ARToolKitPlusNode::publishMarkers(const std_msgs::Header &header) {
50  marker_msgs::MarkerDetection msg;
51 
52  msg.header = header;
53  msg.distance_min = 0; //TODO
54  msg.distance_max = 8; //TODO
55  msg.distance_max_id = 5; //TODO
56  msg.view_direction.x = 0; //TODO
57  msg.view_direction.y = 0; //TODO
58  msg.view_direction.z = 0; //TODO
59  msg.view_direction.w = 1; //TODO
60  msg.fov_horizontal = 6; //TODO
61  msg.fov_vertical = 0; //TODO
62 
63  msg.markers = marker_msgs::MarkerDetection::_markers_type();
64 
65  assert (markerTransforms_.size() == markerTransformsID_.size());
66  std::list<tf::StampedTransform>::iterator it = markerTransforms_.begin();
67  for(size_t i = 0; i < markerTransforms_.size(); it++, i++) {
68  tf::StampedTransform stf = *it;
69  marker_msgs::Marker marker;
70 
71  assert (markerTransformsID_[i] >= 0);
72  marker.ids.resize(1);
73  marker.ids_confidence.resize(1);
74  marker.ids[0] = markerTransformsID_[i];
75  marker.ids_confidence[0] = 1;
76  tf::poseTFToMsg(stf, marker.pose);
77 
78  msg.markers.push_back(marker);
79  }
80 
81  pub_markers_.publish(msg);
82 }
83 
84 void ARToolKitPlusNode::publishPerceptions (const std_msgs::Header &header) {
85  if(pub_perceptions_.getNumSubscribers() < 1) return;
86  tuw_artoolkitplus::TransformArrayStamped msg;
87  if(markerTransforms_.size() > 0) {
88  msg.header = header;
89  msg.child_frame_id.resize(markerTransforms_.size());
90  msg.transform.resize(markerTransforms_.size());
91  std::list<tf::StampedTransform>::iterator it = markerTransforms_.begin();
92  for(unsigned int i; i < markerTransforms_.size(); it++, i++) {
93  geometry_msgs::Vector3 &desT = msg.transform[i].translation;
94  geometry_msgs::Quaternion &desQ = msg.transform[i].rotation;
95  tf::Vector3 &srcT = it->getOrigin();
96  tf::Quaternion srcQ = it->getRotation();
97  desT.x = srcT.x(), desT.y = srcT.y(), desT.z = srcT.z();
98  desQ.x = srcQ.x(), desQ.y = srcQ.y(), desQ.z = srcQ.z(), desQ.w = srcQ.w();
99  msg.child_frame_id[i] = it->child_frame_id_;
100  }
102  }
103 }
104 
105 void ARToolKitPlusNode::callbackParameters ( tuw_artoolkitplus::ARParamConfig &config, uint32_t level ) {
106  ((tuw_artoolkitplus::ARParamConfig&) param_) = config;
107 }
108 
void publish(const boost::shared_ptr< M > &message) const
void publishMarkers(const std_msgs::Header &header)
std::list< tf::StampedTransform > markerTransforms_
Definition: artoolkitplus.h:99
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void callbackParameters(tuw_artoolkitplus::ARParamConfig &config, uint32_t level)
ros::Publisher pub_perceptions_
void getRotation(Quaternion &q) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
int main(int argc, char **argv)
std::vector< int > markerTransformsID_
TFSIMD_FORCE_INLINE const tfScalar & y() const
void sendTransform(const StampedTransform &transform)
ROSCPP_DECL void spin()
ros::Publisher pub_markers_
tf::TransformBroadcaster transformBroadcaster_
Definition: artoolkitplus.h:95
void matrix2Tf(const ARFloat M[3][4], tf::Transform &transform)
uint32_t getNumSubscribers() const
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
float ARFloat
Definition: config.h:60
void publishPerceptions(const std_msgs::Header &header)


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun Sep 4 2016 03:24:33