static_transform_publisher.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 
30 #include <cstdio>
32 
34 {
35 public:
37  //constructor
38  TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& child_frame_id)
39  {
41  q.setRPY(roll, pitch,yaw);
42  transform_ = tf::StampedTransform(tf::Transform(q, tf::Vector3(x,y,z)), time, frame_id, child_frame_id );
43  };
44  TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string& frame_id, const std::string& child_frame_id) :
45  transform_(tf::Transform(tf::Quaternion(qx,qy,qz,qw), tf::Vector3(x,y,z)), time, frame_id, child_frame_id){};
46  //Clean up ros connections
48 
49  //A pointer to the rosTFServer class
51 
52 
53 
54  // A function to call to send data periodically
55  void send (ros::Time time) {
56  transform_.stamp_ = time;
57  broadcaster.sendTransform(transform_);
58  };
59 
60 private:
62 
63 };
64 
65 int main(int argc, char ** argv)
66 {
67  //Initialize ROS
68  ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
69 
70  if(argc == 11)
71  {
72  ros::Duration sleeper(atof(argv[10])/1000.0);
73 
74  if (strcmp(argv[8], argv[9]) == 0)
75  ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]);
76 
77  TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
78  atof(argv[4]), atof(argv[5]), atof(argv[6]), atof(argv[7]),
79  ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
80  argv[8], argv[9]);
81 
82 
83 
84  while(tf_sender.node_.ok())
85  {
86  tf_sender.send(ros::Time::now() + sleeper);
87  ROS_DEBUG("Sending transform from %s with parent %s\n", argv[8], argv[9]);
88  sleeper.sleep();
89  }
90 
91  return 0;
92  }
93  else if (argc == 10)
94  {
95  ros::Duration sleeper(atof(argv[9])/1000.0);
96 
97  if (strcmp(argv[7], argv[8]) == 0)
98  ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[7], argv[8]);
99 
100  TransformSender tf_sender(atof(argv[1]), atof(argv[2]), atof(argv[3]),
101  atof(argv[4]), atof(argv[5]), atof(argv[6]),
102  ros::Time() + sleeper, //Future dating to allow slower sending w/o timeout
103  argv[7], argv[8]);
104 
105 
106 
107  while(tf_sender.node_.ok())
108  {
109  tf_sender.send(ros::Time::now() + sleeper);
110  ROS_DEBUG("Sending transform from %s with parent %s\n", argv[7], argv[8]);
111  sleeper.sleep();
112  }
113 
114  return 0;
115 
116  }
117  else
118  {
119  printf("A command line utility for manually sending a transform.\n");
120  printf("It will periodicaly republish the given transform. \n");
121  printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds) \n");
122  printf("OR \n");
123  printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds) \n");
124  printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
125  printf("of the child_frame_id. \n");
126  ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
127  return -1;
128  }
129 
130 
131 };
132 
#define ROS_FATAL(...)
void send(ros::Time time)
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Definition: exceptions.h:38
ros::Time stamp_
The timestamp associated with this transform.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
TransformSender(double x, double y, double z, double qx, double qy, double qz, double qw, ros::Time time, const std::string &frame_id, const std::string &child_frame_id)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:94
tf::StampedTransform transform_
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
Definition: Vector3.h:293
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
void sendTransform(const StampedTransform &transform)
Send a StampedTransform The stamped data structure includes frame_id, and time, and parent_id already...
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string &frame_id, const std::string &child_frame_id)
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
int main(int argc, char **argv)
static Time now()
bool ok() const
tf::TransformBroadcaster broadcaster
#define ROS_ERROR(...)
This class provides an easy way to publish coordinate frame transform information. It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the necessary data needed for each message.
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38
#define ROS_DEBUG(...)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Feb 23 2018 06:13:06