static_covariance_publisher.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <uncertain_tf/UncertainTransformBroadcaster.h>
00003 
00004 using namespace uncertain_tf;
00005 using namespace std;
00006 
00007 int main(int argc, char** argv)
00008 {
00009     ros::init(argc, argv, "static_covariance_publisher", ros::init_options::AnonymousName);
00010 
00011     std::cout << "USAGE: static_covariance_publisher frame_id var_x  cov_xy cov_xz cov_xZ cov_xY cov_xX" << std::endl
00012               << "                                            cov_yx var_y  cov_yz cov_yZ cov_yY cov_yX" << std::endl
00013               << "                                            cov_zx cov_zy var_z  cov_zZ cov_zY cov_zX" << std::endl
00014               << "                                            cov_Zx cov_Zy cov_Zz var_Z  cov_ZY cov_ZX" << std::endl
00015               << "                                            cov_Yx cov_Yy cov_Yz cov_YZ var_Y  cov_YX" << std::endl
00016               << "                                            cov_Xx cov_Xy cov_Xz cov_XZ cov_XY var_X   period(milliseconds)" << std::endl
00017               << " (36 var/cov values in total + period)" << std::endl;
00018 
00019     if (argc < 6*6+1)
00020         exit(0);
00021 
00022     ros::NodeHandle node;
00023 
00024     //ros::AsyncSpinner spinner(1);
00025     //spinner.start();
00026 
00027     UncertainTransformBroadcaster utfb;
00028 
00029     StampedCovariance stc;
00030     stc.resize(6,6);
00031     stc.setZero();
00032 
00033     //set values
00034     for (int i = 0; i < 6; ++i)
00035         for (int j = 0; j < 6; ++j)
00036         {
00037             stc(i,j) = atof(argv[2+i+j * 6]);
00038         }
00039 
00040     stc.frame_id_ = argv[1];
00041 
00042     double period = 100;
00043 
00044     if (argc>6*6+2)
00045         period = atof(argv[6*6+2]);
00046 
00047     ros::Rate rate(1000 / period);
00048 
00049     std::cout << "frame_id: " << stc.frame_id_ << std::endl;
00050 
00051     std::cout << "Cov: " << std::endl << stc << std::endl;
00052 
00053     std::cout << "Period: " << period << std::endl;
00054 
00055     ros::Time start_time = ros::Time::now();
00056 
00057     while (node.ok())
00058     {
00059         stc.stamp_ = ros::Time::now();
00060 
00061         utfb.sendCovariance(stc);
00062 
00063         rate.sleep();
00064     }
00065     return 0;
00066 };
00067 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


uncertain_tf
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 17:49:36