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 */


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52