testBroadcaster.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
31 #include "ros/ros.h"
32 
34 {
35 public:
36  //constructor
38  //Clean up ros connections
40 
41  //A pointer to the rosTFServer class
43 
44 
45  // A function to call to send data periodically
46  void test () {
47  broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "frame2", "frame1"));
48 
49  if (count > 9000)
50  {
51  count = 0;
52  std::cerr<<"Counter 0 rolledover at 9000"<< std::endl;
53  }
54  else
55  count ++;
56  //std::cerr<<count<<std::endl;
57  }
58 
59  // A function to call to send data periodically
60  void test_vector () {
61  std::vector<tf::StampedTransform> vec;
62  vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe2", "vframe1"));
63  vec.push_back(tf::StampedTransform(tf::Transform(tf::createIdentityQuaternion(), tf::Vector3(1,2,3)), ros::Time().fromSec(1), "vframe1", "vframe0"));
65 
66  if (count1 > 9000)
67  {
68  count1 = 0;
69  std::cerr<<"Counter 1 rolledover at 9000"<< std::endl;
70  }
71  else
72  count1 ++;
73  //std::cerr<<count1<<std::endl;
74  }
75 private:
76  int count;
77  int count1;
78 
79 };
80 
81 int main(int argc, char ** argv)
82 {
83  //Initialize ROS
84  ros::init(argc, argv, "testBroadcaster", ros::init_options::AnonymousName);
85 
86  //Construct/initialize the server
87  testBroadcaster myTestBroadcaster;
88 
89  ros::NodeHandle node;
90  while(ros::ok())//Check if a Ctrl-C or other shutdown command has been recieved
91  {
92  //Send some data
93  myTestBroadcaster.test();
94  myTestBroadcaster.test_vector();
95  usleep(1000);
96  }
97 
98  return 0;
99 }
100 
ros::init_options::AnonymousName
AnonymousName
testBroadcaster::test_vector
void test_vector()
Definition: testBroadcaster.cpp:60
transform_broadcaster.h
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
testBroadcaster
Definition: testBroadcaster.cpp:33
testBroadcaster::~testBroadcaster
~testBroadcaster()
Definition: testBroadcaster.cpp:39
main
int main(int argc, char **argv)
Definition: testBroadcaster.cpp:81
tf::StampedTransform
The Stamped Transform datatype used by tf.
Definition: transform_datatypes.h:81
ros::ok
ROSCPP_DECL bool ok()
tf::TransformBroadcaster
This class provides an easy way to publish coordinate frame transform information....
Definition: transform_broadcaster.h:50
tf::Transform
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
testBroadcaster::count
int count
Definition: testBroadcaster.cpp:76
testBroadcaster::test
void test()
Definition: testBroadcaster.cpp:46
testBroadcaster::broadcaster
tf::TransformBroadcaster broadcaster
Definition: testBroadcaster.cpp:42
ros::Time
tf::TransformBroadcaster::sendTransform
void sendTransform(const StampedTransform &transform)
Send a StampedTransform The stamped data structure includes frame_id, and time, and parent_id already...
Definition: transform_broadcaster.cpp:54
tf::createIdentityQuaternion
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
Definition: transform_datatypes.h:193
testBroadcaster::testBroadcaster
testBroadcaster()
Definition: testBroadcaster.cpp:37
ros::NodeHandle
testBroadcaster::count1
int count1
Definition: testBroadcaster.cpp:77


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07