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 };
ros::init_options::AnonymousName
AnonymousName
tf2_ros::StaticTransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
Send a TransformStamped message The stamped data structure includes frame_id, and time,...
Definition: static_transform_broadcaster.h:57
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros::param::get
ROSCPP_DECL bool get(const std::string &key, bool &b)
main
int main(int argc, char **argv)
Definition: static_transform_broadcaster_program.cpp:53
ROS_FATAL_STREAM
#define ROS_FATAL_STREAM(args)
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
validateXmlRpcTf
bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data)
Definition: static_transform_broadcaster_program.cpp:35
static_transform_broadcaster.h
tf2_ros::StaticTransformBroadcaster
This class provides an easy way to publish coordinate frame transform information....
Definition: static_transform_broadcaster.h:50
Quaternion.h
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
ROS_FATAL
#define ROS_FATAL(...)
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
ROS_ERROR
#define ROS_ERROR(...)
tf2::Quaternion
ros::spin
ROSCPP_DECL void spin()
ros::param::has
ROSCPP_DECL bool has(const std::string &key)
ROS_INFO
#define ROS_INFO(...)
XmlRpc::XmlRpcValue
ros::Time::now
static Time now()


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Wed May 12 2021 15:52:49