SimulatorViewerApp.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2013-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarXSimulation::application::SimulatorViewer
19  * @author Nikolaus Vahrenkamp ( vahrenkamp at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "SimulatorViewerApp.h"
29 
30 //SoQT
31 #include <Inventor/Qt/SoQt.h>
32 
33 using namespace armarx;
34 
35 // Redraw with 30Hz
36 #define MAIN_LOOP_SLEEP_MS 33
37 
38 // print warnings if render time is low (ms)
39 #define MAX_RENDER_TIME_WARNING_MS 33
40 
42  : Application(),
43  exitApplication(false)
44 {
45 }
46 
48 {
49  ARMARX_VERBOSE_S << "destructor";
50  //SoQt::done();
51  qApplication->quit();
52 
53 }
54 
57  Ice::PropertiesPtr properties)
58 {
59  // init SoDB and Qt, SoQt::init also creates an application object
60  VirtualRobot::init("SimulatorViewerApp");
61 
62  mutex.reset(new CoinViewer::RecursiveMutex());
63 
64 
65  // create the debugdrawer / entityDrawer component
66  if (getProperty<bool>("UseDebugDrawer").getValue())
67  {
68  std::stringstream ddName;
69  ddName << getName() << "_EntityDrawer";
70  debugDrawer = Component::create<memoryx::EntityDrawerComponent>(properties, ddName.str());
71  debugDrawer->setMutex(mutex);
72  registry->addObject(debugDrawer);
73  }
74 
75  std::stringstream svName;
76  svName << getName() << "_PhysicsWorldVisualization";
77  simVisu = Component::create<ArmarXPhysicsWorldVisualization>(properties, svName.str());
78  //simVisu.reset(new ArmarXPhysicsWorldVisualization(getArmarXManager()));
79  simVisu->setMutex(mutex);
80  registry->addObject(simVisu);
81 
82 
83  // create top-level widget
84  std::stringstream swName;
85  swName << getName() << "_SimulationWindow";
86  mainWindow = Component::create<ArmarXSimulatorWindow>(properties, swName.str());
87  mainWindow->setMutex(mutex);
88  registry->addObject(mainWindow);
89 
90  //mainWindow.reset(new ArmarXSimulatorWindow(registry, simVisu, debugDrawer));
91 
92 
93 }
94 
95 
96 int SimulatorViewerApp::exec(const ArmarXManagerPtr& armarXManager)
97 {
98 
99  mainWindow->initialize(simVisu, debugDrawer);
100  mainWindow->setCamPoseFromConfig();
101  qApplication = QApplication::instance();
102  qApplication->connect(mainWindow.get(), SIGNAL(closeRequest()), this, SLOT(closeRequest_sent()));
103 
104  //int timeoutMs = 30000;
105  //if (!world->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectInitialized, timeoutMs))
106  // ARMARX_WARNING_S << "ManagedIceObject " << world->getName() << " was not initialized after "<< timeoutMs/1000 << " seconds" << std::endl;
107 
108  std::cout << "Started ArmarXSimulatorViewerApp" << std::endl;
109 
110  IceUtil::Time lastWarnTimeRender = IceUtil::Time::now();
111  /*
112  ARMARX_INFO_S << "Waiting for ManagedIceObject startup: " << physicsWorld->getName() << armarx::flush;
113  int timeoutMs = 30000;
114  ARMARX_INFO_S << "Waiting for ManagedIceObject startup: " << physicsWorld->getName() << ":ok" << armarx::flush;
115  */
116  // initialize visualization
117 
118  mainWindow->show();
119 
120 
121 #if 0
122  memoryx::EntityDrawerInterfacePrx prxDD = armarXManager->getIceManager()->getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
123  memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx = armarXManager->getIceManager()->getProxy<memoryx::PriorKnowledgeInterfacePrx>("PriorKnowledge");
124 
125  std::string objClassName = "vitaliscereal";
126  memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
127  memoryx::EntityBasePtr classesEntity = classesSegmentPrx->getEntityByName(objClassName);
128  memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classesEntity);
129 
130 
131  Eigen::Matrix4f p;
132  p.setIdentity();
133  p(2, 3) = 1000.0f;
134  PosePtr gp(new Pose(p));
135  prxDD->setObjectVisu("debug", "vitalis", objectClass, gp);
136 
137  int color = 0;
138 #endif
139  armarx::CycleUtil cycle(IceUtil::Time::secondsDouble(1.0 / getProperty<float>("UpdateRate").getValue()));
140  // loop until ice quit or window closed
141  while (!armarXManager->isShutdown() && !exitApplication)
142  {
143  try
144  {
145 
146 #if 0
147  color = (color + 1) % 1000;
148  DrawColor col;
149  col.r = 1.0f;
150  col.g = 0.0f;
151  col.b = 0.0f;
152  col.a = float(color) / 1000.0f;
153 
154  prxDD->updateObjectColor("debug", "vitalis", col);
155 #endif
156  /*if (prxDD)
157  {
158  //ARMARX_INFO_S << "TESTING DEBUG DRAWER";
159  // just a test
160  Eigen::Matrix4f p;
161  p.setIdentity();
162  p(2,3) = 1000.0f;
163  PosePtr gp(new Pose(p));
164  prxDD->setPoseDebugLayerVisu("testPose",gp);
165 
166  armarx::Vector3Ptr p1(new armarx::Vector3());
167  p1->x = 0;
168  p1->y = 0;
169  p1->z = 0;
170  armarx::Vector3Ptr p2(new armarx::Vector3());
171  p2->x = 1000.0f;
172  p2->y = 1000.0f;
173  p2->z = 1000.0f;
174 
175  armarx::DrawColor c;
176  c.r = 1.0f;
177  c.g = 0;
178  c.b = 0;
179  c.a = 1;
180  prxDD->setLineDebugLayerVisu("test",p1,p2,10.0f,c);
181  }*/
182 
183  // PAINT WINDOW
184  IceUtil::Time startTime = IceUtil::Time::now();
185  {
186  {
187  auto startTime1 = IceUtil::Time::now();
188  auto l = mainWindow->getScopedLock();
189  IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
190  if (duration1.toMilliSecondsDouble() > 5)
191  {
192  ARMARX_VERBOSE << deactivateSpam(10) << " mainWindow->getScopedLock() took long:" << duration1.toMilliSecondsDouble();
193  }
194  simVisu->synchronizeVisualizationData();
195  }
196 
197  // ensure engine is locked while we draw the content (there might be requests from ice to update the models)
198  auto startTime1 = IceUtil::Time::now();
199  // allow max 20ms for event processing
200  qApplication->processEvents(QEventLoop::AllEvents);
201  IceUtil::Time duration1 = IceUtil::Time::now() - startTime1;
202  if (duration1.toMilliSecondsDouble() > 20)
203  {
204  ARMARX_VERBOSE << deactivateSpam(10) << " qApplication->processEvents took long:" << duration1.toMilliSecondsDouble();
205  }
206  }
207  IceUtil::Time duration = IceUtil::Time::now() - startTime;
208  mainWindow->setDrawTimeMeasured((float)duration.toMilliSecondsDouble());
209  ARMARX_VERBOSE << deactivateSpam(5) << "*** END paint, drawTimeMS:" << duration.toMilliSecondsDouble();
210 
211  if (duration.toMilliSecondsDouble() > MAX_RENDER_TIME_WARNING_MS)
212  {
213  IceUtil::Time lastWarnDuration = IceUtil::Time::now() - lastWarnTimeRender;
214 
215  if (lastWarnDuration.toSecondsDouble() > 4.0)
216  {
217  lastWarnTimeRender = IceUtil::Time::now();
218  ARMARX_DEBUG << "*** Rendering slow !! Render time in ms:" << duration.toMilliSecondsDouble();
219  }
220  }
221 
222  cycle.waitForCycleDuration();
223  }
224  catch (...)
225  {
227  }
228  }
229 
230  ARMARX_IMPORTANT_S << "Leaving main loop";
231 
232  //interruptCallback(0);// signal to the application it should terminate the ice connection
233  armarXManager->shutdown();
234 
235  armarXManager->waitForShutdown();
236 
237  mainWindow = NULL;
238  debugDrawer = NULL;
239  simVisu = NULL;//.reset();
240  qApplication->quit();
241 
242  return 0;
243 }
244 
246 {
247  ARMARX_IMPORTANT_S << "Interrupting ArmarXSimulatorViewerApp";
248  exitApplication = true;
249 }
250 
251 void SimulatorViewerApp::closeRequest_sent()
252 {
253  ARMARX_INFO_S << "Stopping ArmarXSimulatorViewerApp";
254  exitApplication = true;
255 }
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
ARMARX_IMPORTANT_S
#define ARMARX_IMPORTANT_S
Definition: Logging.h:203
armarx::SimulatorViewerApp::exitApplication
bool exitApplication
Definition: SimulatorViewerApp.h:116
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::SimulatorViewerApp::mutex
CoinViewer::RecursiveMutexPtr mutex
Definition: SimulatorViewerApp.h:114
armarx::SimulatorViewerApp::simVisu
ArmarXPhysicsWorldVisualizationPtr simVisu
Definition: SimulatorViewerApp.h:111
Pose.h
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
armarx::SimulatorViewerApp::qApplication
QCoreApplication * qApplication
Definition: SimulatorViewerApp.h:107
armarx::SimulatorViewerApp::interruptCallback
void interruptCallback(int signal) override
interruptCallback Recieve interrupt callbacks to gracefully shut down simulator
Definition: SimulatorViewerApp.cpp:245
armarx::SimulatorViewerApp::exec
int exec(const ArmarXManagerPtr &armarXManager) override
Runs the Qt Event Loop.
Definition: SimulatorViewerApp.cpp:96
armarx::SimulatorViewerApp::debugDrawer
memoryx::EntityDrawerComponentPtr debugDrawer
Definition: SimulatorViewerApp.h:109
IceInternal::Handle< ::Ice::Properties >
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::SimulatorViewerApp::setup
void setup(const ManagedIceObjectRegistryInterfacePtr &registry, Ice::PropertiesPtr properties) override
Definition: SimulatorViewerApp.cpp:55
armarx::SimulatorViewerApp::~SimulatorViewerApp
~SimulatorViewerApp() override
Definition: SimulatorViewerApp.cpp:47
armarx::SimulatorViewerApp::mainWindow
ArmarXSimulatorWindowPtr mainWindow
Definition: SimulatorViewerApp.h:108
ArmarXObjectScheduler.h
armarx::CycleUtil::waitForCycleDuration
IceUtil::Time waitForCycleDuration()
This function will wait (virtual or system time) until the cycle time is reached.
Definition: CycleUtil.cpp:53
armarx::Application
Baseclass for all ArmarX applications.
Definition: Application.h:193
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::Application::getName
std::string getName() const
Retrieve name of the application.
Definition: Application.cpp:614
CycleUtil.h
SimulatorViewerApp.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
float
#define float
Definition: 16_Level.h:22
IceUtil::Handle< ManagedIceObjectRegistryInterface >
armarx::CoinViewer::RecursiveMutex
std::recursive_mutex RecursiveMutex
Definition: CoinViewer.h:51
MAX_RENDER_TIME_WARNING_MS
#define MAX_RENDER_TIME_WARNING_MS
Definition: SimulatorViewerApp.cpp:39
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:200
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:141
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::SimulatorViewerApp::SimulatorViewerApp
SimulatorViewerApp()
Constructor.
Definition: SimulatorViewerApp.cpp:41