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 
31 #include <Eigen/Geometry>
32 
33 #include <boost/program_options.hpp>
34 namespace po=boost::program_options;
35 
36 static void usage (const char* prog_name, const po::options_description &opts, bool desc=false) {
37  if (desc) {
38  std::cout << "A command line utility for manually defining a (static) transform" << std::endl;
39  std::cout << "from parent_frame_id to child_frame_id." << std::endl;
40  }
41  std::cout << std::endl;
42  std::cout << "Usage: static_transform_publisher [options] x y z <rotation> parent_frame_id child_frame_id" << std::endl;
43  std::cout << opts << std::endl;
44 }
45 
46 static void parse_arguments(int argc, char **argv,
47  geometry_msgs::TransformStamped &msg) {
48  std::string mode;
49  po::options_description options_description("allowed options");
50  options_description.add_options()
51  ("help,h", "show this help message")
52  ("mode,m", po::value<std::string>(&mode))
53  ;
54 
55  po::variables_map variables_map;
56  std::vector<std::string> args;
57  std::vector<std::string>::const_iterator arg;
58  try {
59  po::parsed_options parsed =
60  po::command_line_parser(argc, argv)
61  .options(options_description)
62  .allow_unregistered()
63  .run();
64 
65  po::store(parsed, variables_map);
66  po::notify(variables_map);
67  args = po::collect_unrecognized(parsed.options, po::include_positional);
68  arg = args.begin();
69 
70  if (variables_map.count("help")) {
71  usage(argv[0], options_description, true);
72  exit (EXIT_SUCCESS);
73  }
74  const size_t numArgs = 3 + 2;
75 
76  bool bQuatMode = (mode == "wxyz" || mode == "xyzw");
77  if (mode == "")
78  {
79  if (args.size() == numArgs+4) {
80  bQuatMode = true; // 4 rotational args trigger quaternion mode too
81  mode = "xyzw";
82  } else if (args.size() == numArgs+3) {
83  mode = "zyx";
84  } else {
85  throw po::error("invalid number of positional arguments");
86  }
87  }
88 
89  // consume position arguments
90  msg.transform.translation.x = boost::lexical_cast<double>(*arg); ++arg;
91  msg.transform.translation.y = boost::lexical_cast<double>(*arg); ++arg;
92  msg.transform.translation.z = boost::lexical_cast<double>(*arg); ++arg;
93 
94  // consume orientation arguments
95  Eigen::Quaterniond q;
96  if (bQuatMode) { // parse Quaternion
97  if (args.size() != numArgs+4)
98  throw po::error("quaternion mode requires " +
99  boost::lexical_cast<std::string>(numArgs+4) +
100  " positional arguments");
101 
102  const std::string eigen_order("xyzw");
103  double data[4];
104  for (size_t i=0; i<4; ++i) {
105  size_t idx = eigen_order.find(mode[i]);
106  data[idx] = boost::lexical_cast<double>(*arg); ++arg;
107  }
108  q = Eigen::Quaterniond(data);
109 
110  } else { // parse Euler angles
111  if (args.size() != numArgs+3)
112  throw po::error("Euler angles require " +
113  boost::lexical_cast<std::string>(numArgs+3) +
114  " positional arguments");
115  if (mode.size() != 3)
116  throw po::error("mode specification for Euler angles requires a string from 3 chars (xyz)");
117 
118  const std::string axes_order("xyz");
119  size_t axes_idxs[3];
120  double angles[3];
121 
122  for (size_t i=0; i<3; ++i) {
123  size_t idx = axes_order.find(mode[i]);
124  if (idx == std::string::npos)
125  throw po::error("invalid axis specification for Euler angles: " +
126  boost::lexical_cast<std::string>(mode[i]));
127  axes_idxs[i] = idx;
128  angles[i] = boost::lexical_cast<double>(*arg); ++arg;
129  }
130  q = Eigen::AngleAxisd(angles[0], Eigen::Vector3d::Unit(axes_idxs[0])) *
131  Eigen::AngleAxisd(angles[1], Eigen::Vector3d::Unit(axes_idxs[1])) *
132  Eigen::AngleAxisd(angles[2], Eigen::Vector3d::Unit(axes_idxs[2]));
133  }
134  // assign quaternion
135  q.normalize();
136  msg.transform.rotation.x = q.x();
137  msg.transform.rotation.y = q.y();
138  msg.transform.rotation.z = q.z();
139  msg.transform.rotation.w = q.w();
140 
141  // consume link arguments
142  msg.header.frame_id = *arg++;
143  msg.child_frame_id = *arg++;
144  } catch (const po::error &e) {
145  ROS_FATAL_STREAM(e.what());
146  usage(argv[0], options_description);
147  exit (EXIT_FAILURE);
148  } catch (const boost::bad_lexical_cast &e) {
149  ROS_FATAL_STREAM("failed to parse numerical value: " << *arg);
150  usage(argv[0], options_description);
151  exit (EXIT_FAILURE);
152  }
153 }
154 
155 int main(int argc, char ** argv)
156 {
157  // Initialize ROS
158  ros::init(argc, argv, "static_transform_publisher", ros::init_options::AnonymousName);
159 
160  geometry_msgs::TransformStamped msg;
161  parse_arguments(argc, argv, msg);
162 
163  if (msg.header.frame_id.empty() || msg.child_frame_id.empty())
164  {
165  ROS_FATAL("target or source frame is empty");
166  exit(1);
167  }
168  if (msg.header.frame_id == msg.child_frame_id)
169  {
170  ROS_FATAL("target and source frame are the same (%s, %s) this cannot work",
171  msg.child_frame_id.c_str(), msg.header.frame_id.c_str());
172  exit(1);
173  }
174 
176  broadcaster.sendTransform(msg);
177 
178  ROS_INFO("Spinning until killed, publishing %s to %s",
179  msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
180  ros::spin();
181 
182  return 0;
183 }
#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)
#define ROS_INFO(...)
static void usage(const char *prog_name, const po::options_description &opts, bool desc=false)
void sendTransform(const geometry_msgs::TransformStamped &transform)
static void parse_arguments(int argc, char **argv, geometry_msgs::TransformStamped &msg)


agni_tf_tools
Author(s): Robert Haschke
autogenerated on Fri Jun 7 2019 22:04:59