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
00031
00032
00033
00034
00035 #include <pr2_mechanism_controllers/Pose3D.h>
00036 #include <ros/node.h>
00037 #include <geometry_msgs/Twist.h>
00038 #include <nav_msgs/Odometry.h>
00039 #include <geometry_msgs/Quaternion.h>
00040 #include <iostream>
00041 #include <fstream>
00042
00043 static int done = 0;
00044
00045 void finalize(int donecare)
00046 {
00047 done = 1;
00048 }
00049
00050
00052
00053 libTF::Vector GetAsEuler(geometry_msgs::Quaternion quat)
00054 {
00055 libTF::Vector vec;
00056
00057 double squ;
00058 double sqx;
00059 double sqy;
00060 double sqz;
00061
00062
00063
00064 squ = quat.w * quat.w;
00065 sqx = quat.x * quat.x;
00066 sqy = quat.y * quat.y;
00067 sqz = quat.z * quat.z;
00068
00069
00070 vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz);
00071
00072
00073 vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y));
00074
00075
00076 vec.z = atan2(2 * (quat.x*quat.y + quat.w*quat.z), squ + sqx - sqy - sqz);
00077
00078 return vec;
00079 }
00080
00081
00082 class test_run_base
00083 {
00084 public:
00085
00086 test_run_base(){};
00087
00088 ~test_run_base() {}
00089
00090 nav_msgs::Odometry odom;
00091
00092 void odomMsgReceived()
00093 {
00094 };
00095
00096 };
00097
00098 int main( int argc, char** argv )
00099 {
00100
00101
00102 ros::init(argc,argv);
00103 ros::Node *node = new ros::Node("test_run_base_controller");
00104
00105 test_run_base tb;
00106
00107 node->subscribe("odom",tb.odom,&test_run_base::odomMsgReceived,&tb,10);
00108
00109 signal(SIGINT, finalize);
00110 signal(SIGQUIT, finalize);
00111 signal(SIGTERM, finalize);
00112
00113
00114
00115 geometry_msgs::Twist cmd;
00116 cmd.linear.x = 0;
00117 cmd.linear.y = 0;
00118 cmd.linear.z = 0;
00119 cmd.angular.x = 0;
00120 cmd.angular.y = 0;
00121 cmd.angular.z = 0;
00122
00123 double run_time = 0;
00124 bool run_time_set = false;
00125 int file_num = 0;
00126
00127 if(argc >= 2)
00128 cmd.linear.x = atof(argv[1]);
00129
00130 if(argc >= 3)
00131 cmd.linear.y = atof(argv[2]);
00132
00133 if(argc >= 4)
00134 cmd.angular.z = atof(argv[3]);
00135
00136 if(argc >=5)
00137 {
00138 run_time = atof(argv[4]);
00139 run_time_set = true;
00140 }
00141
00142 if(argc ==6)
00143 {
00144 file_num = atoi(argv[5]);
00145 }
00146
00147 node->advertise<geometry_msgs::Twist>("cmd_vel",10);
00148 sleep(1);
00149
00150 libTF::Vector ang_rates;
00151 ros::Time start_time = ros::Time::now();
00152 ros::Duration sleep_time = ros::Duration().fromSec(0.01);
00153
00154 std::ofstream odom_log_file;
00155 odom_log_file.open("odom_log.txt");
00156
00157
00158 while(!done)
00159 {
00160 ros::Duration delta_time = ros::Time::now() - start_time;
00161
00162 if(run_time_set && delta_time.toSec() > run_time)
00163 break;
00164
00165 odom_log_file << tb.odom.pose.pose.position.x << " " << tb.odom.pose.pose.position.y << " " << tf::getYaw(tb.odom.pose.pose.orientation) << " " << tb.odom.twist.twist.linear.x << " " << tb.odom.twist.twist.linear.y << " " << tb.odom.twist.twist.angulat.z << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
00166
00167 cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.twist.twist.linear.x << endl << " y:" << tb.odom.twist.twist.linear.y << endl << " omega:" << tb.odom.twist.twist.angular.z << std::endl;
00168 node->publish("cmd_vel",cmd);
00169 sleep_time.sleep();
00170 }
00171
00172 node->unsubscribe("odom");
00173 node->unadvertise("cmd_vel");
00174
00175 odom_log_file.close();
00176
00177
00178 }