Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <cstdio>
00031 #include <tf2/LinearMath/Quaternion.h>
00032 #include "tf2_ros/static_transform_broadcaster.h"
00033
00034
00035 bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data) {
00036
00037
00038 return tf_data.hasMember("child_frame_id") &&
00039 tf_data.hasMember("header") &&
00040 tf_data["header"].hasMember("frame_id") &&
00041 tf_data.hasMember("transform") &&
00042 tf_data["transform"].hasMember("translation") &&
00043 tf_data["transform"]["translation"].hasMember("x") &&
00044 tf_data["transform"]["translation"].hasMember("y") &&
00045 tf_data["transform"]["translation"].hasMember("z") &&
00046 tf_data["transform"].hasMember("rotation") &&
00047 tf_data["transform"]["rotation"].hasMember("x") &&
00048 tf_data["transform"]["rotation"].hasMember("y") &&
00049 tf_data["transform"]["rotation"].hasMember("z") &&
00050 tf_data["transform"]["rotation"].hasMember("w");
00051 };
00052
00053 int main(int argc, char ** argv)
00054 {
00055
00056 ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
00057 tf2_ros::StaticTransformBroadcaster broadcaster;
00058 geometry_msgs::TransformStamped msg;
00059
00060 if(argc == 10)
00061 {
00062 msg.transform.translation.x = atof(argv[1]);
00063 msg.transform.translation.y = atof(argv[2]);
00064 msg.transform.translation.z = atof(argv[3]);
00065 msg.transform.rotation.x = atof(argv[4]);
00066 msg.transform.rotation.y = atof(argv[5]);
00067 msg.transform.rotation.z = atof(argv[6]);
00068 msg.transform.rotation.w = atof(argv[7]);
00069 msg.header.stamp = ros::Time::now();
00070 msg.header.frame_id = argv[8];
00071 msg.child_frame_id = argv[9];
00072 }
00073 else if (argc == 9)
00074 {
00075 msg.transform.translation.x = atof(argv[1]);
00076 msg.transform.translation.y = atof(argv[2]);
00077 msg.transform.translation.z = atof(argv[3]);
00078
00079 tf2::Quaternion quat;
00080 quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4]));
00081 msg.transform.rotation.x = quat.x();
00082 msg.transform.rotation.y = quat.y();
00083 msg.transform.rotation.z = quat.z();
00084 msg.transform.rotation.w = quat.w();
00085
00086 msg.header.stamp = ros::Time::now();
00087 msg.header.frame_id = argv[7];
00088 msg.child_frame_id = argv[8];
00089 }
00090 else if (argc == 2) {
00091 const std::string param_name = argv[1];
00092 ROS_INFO_STREAM("Looking for TF in parameter: " << param_name);
00093 XmlRpc::XmlRpcValue tf_data;
00094
00095 if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) {
00096 ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name);
00097 return -1;
00098 }
00099
00100
00101 if (!validateXmlRpcTf(tf_data)) {
00102 ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data);
00103 return -1;
00104 }
00105
00106 msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"];
00107 msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"];
00108 msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"];
00109 msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"];
00110 msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"];
00111 msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"];
00112 msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"];
00113 msg.header.stamp = ros::Time::now();
00114 msg.header.frame_id = (std::string) tf_data["header"]["frame_id"];
00115 msg.child_frame_id = (std::string) tf_data["child_frame_id"];
00116 }
00117 else
00118 {
00119 printf("A command line utility for manually sending a transform.\n");
00120
00121 printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
00122 printf("OR \n");
00123 printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
00124 printf("OR \n");
00125 printf("Usage: static_transform_publisher /param_name \n");
00126 printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
00127 printf("of the child_frame_id. \n");
00128 ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
00129 return -1;
00130 }
00131
00132
00133 if (msg.header.frame_id == msg.child_frame_id)
00134 {
00135 ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work",
00136 msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
00137 return 1;
00138 }
00139
00140 broadcaster.sendTransform(msg);
00141 ROS_INFO("Spinning until killed publishing %s to %s",
00142 msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
00143 ros::spin();
00144 return 0;
00145 };