$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 <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 // Return the rotation in Euler angles 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 // this->Normalize(); 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 // Roll 00069 vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz); 00070 00071 // Pitch 00072 vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y)); 00073 00074 // Yaw 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 // cout << "Odom:: (" << ground_truth.pos.position.x << "), (" << ground_truth.pos.orientation.x << ")" << std::endl; 00103 }; 00104 00105 void groundTruthMsgReceived() 00106 { 00107 // cout << "Odom:: (" << ground_truth.pos.position.x << "), (" << ground_truth.pos.orientation.x << ")" << std::endl; 00108 }; 00109 }; 00110 00111 int main( int argc, char** argv ) 00112 { 00113 00114 /*********** Initialize ROS ****************/ 00115 ros::init(argc,argv); 00116 ros::Node *node = new ros::Node("test_run_base_controller"); 00117 00118 00119 // receive messages from 2dnav stack 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 /*********** Start moving the robot ************/ 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 // ang_rates = GetAsEuler(tb.ground_truth.vel.ang_vel); 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 // cout << delta_time.toSec() << " " << run_time << endl; 00179 node->publish("cmd_vel",cmd); 00180 sleep_time.sleep(); 00181 } 00182 00183 00184 }