38 return tf_data.
hasMember(
"child_frame_id") &&
40 tf_data[
"header"].
hasMember(
"frame_id") &&
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");
53 int main(
int argc,
char ** argv)
58 geometry_msgs::TransformStamped msg;
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]);
70 msg.header.frame_id = argv[8];
71 msg.child_frame_id = argv[9];
75 msg.transform.translation.x = atof(argv[1]);
76 msg.transform.translation.y = atof(argv[2]);
77 msg.transform.translation.z = atof(argv[3]);
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();
87 msg.header.frame_id = argv[7];
88 msg.child_frame_id = argv[8];
91 const std::string param_name = argv[1];
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"];
114 msg.header.frame_id = (std::string) tf_data[
"header"][
"frame_id"];
115 msg.child_frame_id = (std::string) tf_data[
"child_frame_id"];
119 printf(
"A command line utility for manually sending a transform.\n");
121 printf(
"Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
123 printf(
"Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \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");
133 if (msg.header.frame_id == msg.child_frame_id)
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());
141 ROS_INFO(
"Spinning until killed publishing %s to %s",
142 msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_FATAL_STREAM(args)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
ROSCPP_DECL bool has(const std::string &key)
#define ROS_INFO_STREAM(args)
bool hasMember(const std::string &name) const