test_base_controller.cpp
Go to the documentation of this file.00001 00002 #include <pr2_mechanism_controllers/base_controller.h> 00003 #include <hardware_interface/hardware_interface.h> 00004 #include <pr2_mechanism_model/robot.h> 00005 #include <urdf/parser.h> 00006 #include <ros/node.h> 00007 00008 00009 int main( int argc, char** argv ) 00010 { 00011 if(argc != 3) 00012 { 00013 printf("Usage: %s <robot_xml> <controller_xml>\n",argv[0]); 00014 exit(-1); 00015 } 00016 printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]); 00017 00018 00019 /*********** Create the robot model ****************/ 00020 pr2_mechanism::Robot *robot_model = new pr2_mechanism::Robot; 00021 controller::BaseControllerNode bc; 00022 HardwareInterface hw(0); 00023 robot_model->hw_ = &hw; 00024 00025 00026 /*********** Initialize ROS ****************/ 00027 ros::init(argc,argv); 00028 00029 /*********** Load the robot model and state file ************/ 00030 char *xml_robot_file = argv[1]; 00031 TiXmlDocument xml(xml_robot_file); // Load robot description 00032 xml.LoadFile(); 00033 TiXmlElement *root = xml.FirstChildElement("robot"); 00034 urdf::normalizeXml(root); 00035 robot_model->initXml(root); 00036 pr2_mechanism_model::RobotState *robot_state = new pr2_mechanism_model::RobotState(robot_model, &hw); 00037 00038 00039 /*********** Load the controller file ************/ 00040 #error Broken because the base controller does not have initXml anymore 00041 char *xml_control_file = argv[2]; 00042 TiXmlDocument xml_control(xml_control_file); // Load robot description 00043 xml_control.LoadFile(); 00044 TiXmlElement *root_control = xml_control.FirstChildElement("controllers"); 00045 TiXmlElement *root_controller = root_control->FirstChildElement("controller"); 00046 //bc.initXml(robot_state,root_controller); 00047 00048 00049 /************ Testing the odometry calculations themselves ******************/ 00050 /* NEWMAT::Matrix A(16,3); 00051 00052 A.Row(1) << 10 << 8.95 << 0.05; 00053 A.Row(2) << 0 << -2 << 0; 00054 A.Row(3) << 1 << -0.1 << 0.01; 00055 A.Row(4) << 2 << 1.1 << 0; 00056 00057 A.Row(5) << 3 << 2 << -0.05; 00058 A.Row(6) << 4 << 3 << 0.01; 00059 A.Row(7) << 5 << 4.1 << 0.05; 00060 A.Row(8) << -1 << -2 << 0.025; 00061 00062 A.Row(9) << 6.15 << 5.05 << 0.01; 00063 A.Row(10) << 6.985 << 6.02 << 0.01; 00064 A.Row(11) << 8.01 << 8.05 << -0.05; 00065 A.Row(12) << 9.03 << 8.1 << -0.01; 00066 00067 A.Row(13) << -8.03 << -9.1 << 0.01; 00068 A.Row(14) << -10.03 << -13.1 << 0.05; 00069 A.Row(15) << -15.03 << -16.1 << -0.015; 00070 A.Row(16) << -16.03 << -17.1 << -0.01; 00071 00072 NEWMAT::Matrix B(16,1); 00073 B << 1.1 << 1 << 1.1 << 1.15 << 0.95 << 0.99 << 0.98 << 0.95 << 1.05 << 1.1 << 1.05 << 1 << 1.13 << 0.995 << 1.035 << 1.08; 00074 NEWMAT::Matrix xfit(3,1); 00075 00076 xfit = bc.c_->iterativeLeastSquares(A,B,"Gaussian",10); 00077 cout << "done" << xfit << endl; 00078 */ 00079 00080 delete robot_model; 00081 delete robot_state; 00082 } 00083 00084 00085 00086 00087 00088 00089 /*class BaseControllerTest : public testing::Test { 00090 protected: 00091 virtual void SetUp() { 00092 } 00093 00094 // virtual void TearDown() {} 00095 00096 Queue<int> q0_; 00097 Queue<int> q1_; 00098 Queue<int> q2_; 00099 }; 00100 00101 00102 TEST (BaseControllerTests, constructionDestruction) 00103 { 00104 controller::BaseController *bc = new controller::BaseController(); 00105 delete bc; 00106 } 00107 00108 TEST (BaseControllerTests, loadXML) 00109 { 00110 pr2_mechanism::Robot *robot = new pr2_mechanism::Robot("r2d2"); 00111 controller::BaseController bc; 00112 00113 const string xml_controller_file = "controller_base.xml"; 00114 const string xml_robot_file = "pr2_base.xml" 00115 00116 TiXmlDocument xml(xml_robot_file); // Load robot description 00117 xml.LoadFile(); 00118 TiXmlElement *root = xml.FirstChildElement("robot"); 00119 robot->initXml(root); 00120 } 00121 00122 int main(int argc, char **argv) 00123 { 00124 testing::InitGoogleTest(&argc, argv); 00125 return RUN_ALL_TESTS(); 00126 } 00127 */