Fawkes API  Fawkes Development Version
qa_modules.cpp
1 
2 /***************************************************************************
3  * qa_modules.cpp - QA for OpenRAVE Environment class
4  *
5  * Created: Thu Sep 16 14:50:34 2010
6  * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 // Do not include in api reference
25 ///@cond QA
26 
27 #include <openrave/openrave.h>
28 
29 #include <plugins/openrave/environment.h>
30 #include <plugins/openrave/robot.h>
31 #include <logging/console.h>
32 #include <cstdio>
33 #include <iostream>
34 #include <sstream>
35 #include <vector>
36 #include <list>
37 
38 using namespace fawkes;
39 using namespace std;
40 
41 void
42 printVector(vector<float> &v)
43 {
44  stringstream s;
45  //printf("## size:%u \n", v.size());
46  for(unsigned int i=0; i<v.size(); i++)
47  {
48  s << "(" << i << ")" << v[i] << " ";
49  //printf("## %u:)%f \n", i, v[i]);
50  }
51  printf("%s \n", s.str().c_str());
52 }
53 
54 int
55 main(int argc, char **argv)
56 {
57  printf("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
58 
59  string robotFile = SRCDIR"/../manipulators/katana.robot.xml";
60 
61  ConsoleLogger* cl = new ConsoleLogger();
62 
63  OpenRaveRobotPtr robot( new OpenRaveRobot(cl) );
65 
66  vector<OpenRAVE::RobotBasePtr> robots;
67  list<OpenRAVE::ModuleBasePtr> modules;
68 
69 
70 
71  env->create();
72 
73  env->get_env_ptr()->GetModules(modules);
74  env->get_env_ptr()->GetRobots(robots);
75  cl->log_debug("qa_modules", "Environment created");
76  cl->log_debug("qa_modules", "#modules:%u #robots:%u", modules.size(), robots.size());
77 
78 
79 
80 
81  try {
82  robot->load(robotFile, env);
83  } catch (Exception &e) {
84  cl->log_error("qa_modules", "error:%s", e.what());
85  return 0;
86  }
87 
88  env->get_env_ptr()->GetModules(modules);
89  env->get_env_ptr()->GetRobots(robots);
90  cl->log_debug("qa_modules", "Robot loaded");
91  cl->log_debug("qa_modules", "#modules:%u #robots:%u", modules.size(), robots.size());
92 
93 
94 
95  env->add_robot(robot);
96  robot->set_ready();
97 
98  env->get_env_ptr()->GetModules(modules);
99  env->get_env_ptr()->GetRobots(robots);
100  cl->log_debug("qa_modules", "Robot initialized");
101  cl->log_debug("qa_modules", "#modules:%u #robots:%u", modules.size(), robots.size());
102 
103 
104 
105  robot = NULL;
106 
107  env->get_env_ptr()->GetModules(modules);
108  env->get_env_ptr()->GetRobots(robots);
109  cl->log_debug("qa_modules", "Robot Destroyed");
110  cl->log_debug("qa_modules", "#modules:%u #robots:%u", modules.size(), robots.size());
111 
112 
113 
114 
115  env->destroy();
116 
117  return 0;
118 }
119 
120 
121 /// @endcond
Interface for logging to stderr.
Definition: console.h:36
Fawkes library namespace.
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Definition: console.cpp:137
STL namespace.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: console.cpp:167
OpenRAVE Robot class.
Definition: robot.h:39
Base class for exceptions in Fawkes.
Definition: exception.h:36
OpenRaveEnvironment class.
Definition: environment.h:45
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49