$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 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 // Return the rotation in Euler angles 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 // this->Normalize(); 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 // Roll 00070 vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz); 00071 00072 // Pitch 00073 vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y)); 00074 00075 // Yaw 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 /*********** Initialize ROS ****************/ 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 /*********** Start moving the robot ************/ 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 }