static_variance_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_variance_publisher", ros::init_options::AnonymousName);
00010 
00011     std::cout << "USAGE: static_variance_publisher frame_id var_x var_y var_z var_rot_z var_rot_y var_rot_x period(milliseconds)" << std::endl;
00012 
00013     if (argc < 2)
00014         exit(0);
00015 
00016     ros::NodeHandle node;
00017 
00018     //ros::AsyncSpinner spinner(1);
00019     //spinner.start();
00020 
00021     UncertainTransformBroadcaster utfb;
00022 
00023     StampedCovariance stc;
00024     stc.resize(6,6);
00025     stc.setZero();
00026 
00027     //set values
00028     for (int i = 0; i < std::min(argc - 2, 6); ++i)
00029     {
00030         stc(i,i) = atof(argv[i+2]);
00031     }
00032 
00033     stc.frame_id_ = argv[1];
00034 
00035     double period = 100;
00036 
00037     if (argc>8)
00038         period = atof(argv[8]);
00039 
00040     ros::Rate rate(1000 / period);
00041 
00042     std::cout << "frame_id: " << stc.frame_id_ << std::endl;
00043 
00044     std::cout << "Cov: " << std::endl << stc << std::endl;
00045 
00046     std::cout << "Period: " << period << std::endl;
00047 
00048     ros::Time start_time = ros::Time::now();
00049 
00050     while (node.ok())
00051     {
00052         stc.stamp_ = ros::Time::now();
00053 
00054         utfb.sendCovariance(stc);
00055 
00056         rate.sleep();
00057     }
00058     return 0;
00059 };
00060 
 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