Fawkes API  Fawkes Development Version
qa_modules.cpp
00001 
00002 /***************************************************************************
00003  *  qa_modules.cpp - QA for OpenRAVE Environment class
00004  *
00005  *  Created: Thu Sep 16 14:50:34 2010
00006  *  Copyright  2010  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 // Do not include in api reference
00025 ///@cond QA
00026 
00027 #include <openrave/openrave.h>
00028 
00029 #include <plugins/openrave/environment.h>
00030 #include <plugins/openrave/robot.h>
00031 #include <logging/console.h>
00032 #include <cstdio>
00033 #include <iostream>
00034 #include <sstream>
00035 #include <vector>
00036 #include <list>
00037 
00038 using namespace fawkes;
00039 using namespace std;
00040 
00041 void
00042 printVector(vector<float> &v)
00043 {
00044   stringstream s;
00045   //printf("## size:%u \n", v.size());
00046   for(unsigned int i=0; i<v.size(); i++)
00047   {
00048     s << "(" << i << ")" << v[i] << "    ";
00049     //printf("## %u:)%f \n", i, v[i]);
00050   }
00051   printf("%s \n", s.str().c_str());
00052 }
00053 
00054 int
00055 main(int argc, char **argv)
00056 {
00057   printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
00058 
00059   string robotFile = SRCDIR"/../manipulators/katana.robot.xml";
00060 
00061   ConsoleLogger* cl = new ConsoleLogger();
00062 
00063   OpenRaveRobot* robot = new OpenRaveRobot(cl);
00064   OpenRaveEnvironment* env = new OpenRaveEnvironment(cl);
00065 
00066   vector<OpenRAVE::RobotBasePtr> robots;
00067   list<OpenRAVE::ModuleBasePtr> modules;
00068 
00069 
00070 
00071   env->create();
00072 
00073   env->get_env_ptr()->GetModules(modules);
00074   env->get_env_ptr()->GetRobots(robots);
00075   cl->log_debug("qa_modules", "Environment created");
00076   cl->log_debug("qa_modules", "#modules:%u  #robots:%u", modules.size(), robots.size());
00077 
00078 
00079 
00080 
00081   try {
00082     robot->load(robotFile, env);
00083   } catch (Exception &e) {
00084     cl->log_error("qa_modules", "error:%s", e.what());
00085     return 0;
00086   }
00087 
00088   env->get_env_ptr()->GetModules(modules);
00089   env->get_env_ptr()->GetRobots(robots);
00090   cl->log_debug("qa_modules", "Robot loaded");
00091   cl->log_debug("qa_modules", "#modules:%u  #robots:%u", modules.size(), robots.size());
00092 
00093 
00094 
00095   env->add_robot(robot);
00096   robot->set_ready();
00097 
00098   env->get_env_ptr()->GetModules(modules);
00099   env->get_env_ptr()->GetRobots(robots);
00100   cl->log_debug("qa_modules", "Robot initialized");
00101   cl->log_debug("qa_modules", "#modules:%u  #robots:%u", modules.size(), robots.size());
00102 
00103 
00104 
00105   delete robot;
00106 
00107   env->get_env_ptr()->GetModules(modules);
00108   env->get_env_ptr()->GetRobots(robots);
00109   cl->log_debug("qa_modules", "Robot Destroyed");
00110   cl->log_debug("qa_modules", "#modules:%u  #robots:%u", modules.size(), robots.size());
00111 
00112 
00113 
00114 
00115   env->destroy();
00116 
00117   return 0;
00118 }
00119 
00120 
00121 /// @endcond