static_transform_broadcaster_program.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>
33 
34 
36  // Validate a TF stored in XML RPC format: ensures the appropriate fields
37  // exist. Note this does not check data types.
38  return tf_data.hasMember("child_frame_id") &&
39  tf_data.hasMember("header") &&
40  tf_data["header"].hasMember("frame_id") &&
41  tf_data.hasMember("transform") &&
42  tf_data["transform"].hasMember("translation") &&
43  tf_data["transform"]["translation"].hasMember("x") &&
44  tf_data["transform"]["translation"].hasMember("y") &&
45  tf_data["transform"]["translation"].hasMember("z") &&
46  tf_data["transform"].hasMember("rotation") &&
47  tf_data["transform"]["rotation"].hasMember("x") &&
48  tf_data["transform"]["rotation"].hasMember("y") &&
49  tf_data["transform"]["rotation"].hasMember("z") &&
50  tf_data["transform"]["rotation"].hasMember("w");
51 };
52 
53 int main(int argc, char ** argv)
54 {
55  //Initialize ROS
56  ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
58  geometry_msgs::TransformStamped msg;
59 
60  if(argc == 10)
61  {
62  msg.transform.translation.x = atof(argv[1]);
63  msg.transform.translation.y = atof(argv[2]);
64  msg.transform.translation.z = atof(argv[3]);
65  msg.transform.rotation.x = atof(argv[4]);
66  msg.transform.rotation.y = atof(argv[5]);
67  msg.transform.rotation.z = atof(argv[6]);
68  msg.transform.rotation.w = atof(argv[7]);
69  msg.header.stamp = ros::Time::now();
70  msg.header.frame_id = argv[8];
71  msg.child_frame_id = argv[9];
72  }
73  else if (argc == 9)
74  {
75  msg.transform.translation.x = atof(argv[1]);
76  msg.transform.translation.y = atof(argv[2]);
77  msg.transform.translation.z = atof(argv[3]);
78 
79  tf2::Quaternion quat;
80  quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4]));
81  msg.transform.rotation.x = quat.x();
82  msg.transform.rotation.y = quat.y();
83  msg.transform.rotation.z = quat.z();
84  msg.transform.rotation.w = quat.w();
85 
86  msg.header.stamp = ros::Time::now();
87  msg.header.frame_id = argv[7];
88  msg.child_frame_id = argv[8];
89  }
90  else if (argc == 2) {
91  const std::string param_name = argv[1];
92  ROS_INFO_STREAM("Looking for TF in parameter: " << param_name);
93  XmlRpc::XmlRpcValue tf_data;
94 
95  if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) {
96  ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name);
97  return -1;
98  }
99 
100  // Check that all required members are present & of the right type.
101  if (!validateXmlRpcTf(tf_data)) {
102  ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data);
103  return -1;
104  }
105 
106  msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"];
107  msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"];
108  msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"];
109  msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"];
110  msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"];
111  msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"];
112  msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"];
113  msg.header.stamp = ros::Time::now();
114  msg.header.frame_id = (std::string) tf_data["header"]["frame_id"];
115  msg.child_frame_id = (std::string) tf_data["child_frame_id"];
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 qx qy qz qw frame_id child_frame_id \n");
122  printf("OR \n");
123  printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
124  printf("OR \n");
125  printf("Usage: static_transform_publisher /param_name \n");
126  printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
127  printf("of the child_frame_id. \n");
128  ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
129  return -1;
130  }
131 
132  // Checks: frames should not be the same.
133  if (msg.header.frame_id == msg.child_frame_id)
134  {
135  ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work",
136  msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
137  return 1;
138  }
139 
140  broadcaster.sendTransform(msg);
141  ROS_INFO("Spinning until killed publishing %s to %s",
142  msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
143  ros::spin();
144  return 0;
145 };
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
#define ROS_FATAL(...)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_FATAL_STREAM(args)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define ROS_INFO(...)
ROSCPP_DECL bool has(const std::string &key)
bool hasMember(const std::string &name) const
#define ROS_INFO_STREAM(args)
bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data)
static Time now()
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.
void sendTransform(const geometry_msgs::TransformStamped &transform)
Send a TransformStamped message The stamped data structure includes frame_id, and time...
#define ROS_ERROR(...)


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 10 2019 12:25:55