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