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 <math.h>
00031 #include <cstdio>
00032 #include "tf2_ros/static_transform_broadcaster.h"
00033
00034 int main(int argc, char ** argv)
00035 {
00036
00037 ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
00038 tf2_ros::StaticTransformBroadcaster broadcaaster;
00039
00040 if(argc == 10)
00041 {
00042
00043 if (strcmp(argv[8], argv[9]) == 0)
00044 {
00045 ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]);
00046 return 1;
00047 }
00048
00049 geometry_msgs::TransformStamped msg;
00050 msg.transform.translation.x = atof(argv[1]);
00051 msg.transform.translation.y = atof(argv[2]);
00052 msg.transform.translation.z = atof(argv[3]);
00053 msg.transform.rotation.x = atof(argv[4]);
00054 msg.transform.rotation.y = atof(argv[5]);
00055 msg.transform.rotation.z = atof(argv[6]);
00056 msg.transform.rotation.w = atof(argv[7]);
00057 msg.header.stamp = ros::Time::now();
00058 msg.header.frame_id = argv[8];
00059 msg.child_frame_id = argv[9];
00060
00061
00062
00063 broadcaaster.sendTransform(msg);
00064 ROS_INFO("Spinning until killed publishing %s to %s", msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
00065 ros::spin();
00066
00067 return 0;
00068 }
00069 else if (argc == 9)
00070 {
00071 if (strcmp(argv[7], argv[8]) == 0)
00072 {
00073 ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work", argv[8], argv[9]);
00074 return 1;
00075 }
00076
00077 geometry_msgs::TransformStamped msg;
00078 msg.transform.translation.x = atof(argv[1]);
00079 msg.transform.translation.y = atof(argv[2]);
00080 msg.transform.translation.z = atof(argv[3]);
00081
00082 double halfYaw = atof(argv[4]) * 0.5;
00083 double halfPitch = atof(argv[5]) * 0.5;
00084 double halfRoll = atof(argv[6]) * 0.5;
00085 double cosYaw = cos(halfYaw);
00086 double sinYaw = sin(halfYaw);
00087 double cosPitch = cos(halfPitch);
00088 double sinPitch = sin(halfPitch);
00089 double cosRoll = cos(halfRoll);
00090 double sinRoll = sin(halfRoll);
00091
00092 msg.transform.rotation.x = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
00093 msg.transform.rotation.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
00094 msg.transform.rotation.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
00095 msg.transform.rotation.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
00096
00097
00098 broadcaaster.sendTransform(msg);
00099 ROS_INFO("Spinning until killed publishing %s to %s", msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
00100 ros::spin();
00101 return 0;
00102 }
00103 else
00104 {
00105 printf("A command line utility for manually sending a transform.\n");
00106
00107 printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
00108 printf("OR \n");
00109 printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
00110 printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
00111 printf("of the child_frame_id. \n");
00112 ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
00113 return -1;
00114 }
00115
00116
00117 };
00118