void onItemAdded(Item* item)
{
MessageView* mv = MessageView::instance();
if(BodyItem* bodyItem = dynamic_cast<BodyItem*>(item)){
Body* body = bodyItem->body();
for(size_t i=0; i < body->numDevices(); ++i){
Device* device = body->device(i);
if(!camera){
camera = dynamic_pointer_cast<Camera>(device);
if(camera){
mv->putln(format("RTCVisionSensorSamplePlugin: Detected Camera \"%1%\" of %2%.")
% camera->name() % body->name());
visionSensorSampleRTC->setCameraLocalT(camera->T_local());
}
}
if(!rangeSensor){
rangeSensor = dynamic_pointer_cast<RangeSensor>(device);
if(rangeSensor){
mv->putln(format("RTCVisionSensorSamplePlugin: Detected RangeSensor \"%1%\" of %2%.")
% rangeSensor->name() % body->name());
visionSensorSampleRTC->setRangeSensorLocalT(rangeSensor->T_local());
}
}
}
}else if(PointSetItem* pointSetItem = dynamic_cast<PointSetItem*>(item)){
if(pointSetItem->name() == "RangeCameraPoints"){
pointSetFromRangeCamera = pointSetItem->pointSet();
mv->putln("RTCVisionSensorSamplePlugin: Detected PointSetItem \"RangeCameraPoints\"");
visionSensorSampleRTC->setPointSetFromRangeCamera(pointSetFromRangeCamera);
} else if(pointSetItem->name() == "RangeSensorPoints"){
pointSetFromRangeSensor = pointSetItem->pointSet();
mv->putln("RTCVisionSensorSamplePlugin: Detected PointSetItem \"RangeSensorPoints\"");
visionSensorSampleRTC->setPointSetFromRangeSensor(pointSetFromRangeSensor);
}
}
}
bool GLVisionSimulatorItemImpl::initializeSimulation(SimulatorItem* simulatorItem)
{
if(!QGLPixelBuffer::hasOpenGLPbuffers()){
os << (format(_("The vision sensor simulation by %1% cannot be performed because the OpenGL pbuffer is not available."))
% self->name()) << endl;
return false;
}
glFormat = QGLFormat::defaultFormat();
glFormat.setDoubleBuffer(false);
this->simulatorItem = simulatorItem;
worldTimeStep = simulatorItem->worldTimeStep();
currentTime = 0;
visionRenderers.clear();
useThread = useThreadProperty;
if(useThread){
if(useThreadsForSensorsProperty){
useQueueThreadForAllSensors = false;
useThreadsForSensors = true;
} else {
useQueueThreadForAllSensors = true;
useThreadsForSensors = false;
}
} else {
useQueueThreadForAllSensors = false;
useThreadsForSensors = false;
}
isBestEffortMode = isBestEffortModeProperty;
renderersInRendering.clear();
cloneMap.clear();
#ifdef CNOID_REFERENCED_USE_ATOMIC_COUNTER
cloneMap.setNonNodeCloning(false);
cout << "cloneMap.setNonNodeCloning(false);" << endl;
#else
cloneMap.setNonNodeCloning(true);
cout << "cloneMap.setNonNodeCloning(true);" << endl;
#endif
std::set<string> bodyNameSet;
for(size_t i=0; i < bodyNames.size(); ++i){
bodyNameSet.insert(bodyNames[i]);
}
std::set<string> sensorNameSet;
for(size_t i=0; i < sensorNames.size(); ++i){
sensorNameSet.insert(sensorNames[i]);
}
const vector<SimulationBody*>& simBodies = simulatorItem->simulationBodies();
for(size_t i=0; i < simBodies.size(); ++i){
SimulationBody* simBody = simBodies[i];
Body* body = simBody->body();
if(bodyNameSet.empty() || bodyNameSet.find(body->name()) != bodyNameSet.end()){
for(size_t j=0; j < body->numDevices(); ++j){
Device* device = body->device(j);
if(dynamic_cast<Camera*>(device) || dynamic_cast<RangeSensor*>(device)){
if(sensorNameSet.empty() || sensorNameSet.find(device->name()) != sensorNameSet.end()){
addTargetSensor(simBody, i, device);
}
}
}
}
}
if(visionRenderers.empty()){
os << (format(_("%1% has no target sensors")) % self->name()) << endl;
return false;
}
#ifdef Q_OS_LINUX
/**
The following code is neccessary to avoid a crash when a view which has a widget such as
QPlainTextEdit and has not been focused yet is first focused (clikced) during the camera
image simulation processed by GLVisionSimulatorItem. The crash only occurs in Linux with
the nVidia proprietary X driver. If the user clicks such a view to give the focus before
the simulation started, the crash doesn't occur, so here the focus is forced to be given
by the following code.
*/
if(QWidget* textEdit = MessageView::instance()->findChild<QWidget*>("TextEdit")){
textEdit->setFocus();
//! todo restore the previous focus here
}
#endif
vector<VisionRendererPtr>::iterator p = visionRenderers.begin();
while(p != visionRenderers.end()){
VisionRenderer* renderer = p->get();
if(renderer->initialize(simBodies)){
++p;
} else {
os << (format(_("%1%: Target sensor \"%2%\" cannot be initialized."))
% self->name() % renderer->device->name()) << endl;
p = visionRenderers.erase(p);
}
}
if(!visionRenderers.empty()){
simulatorItem->addPreDynamicsFunction(boost::bind(&GLVisionSimulatorItemImpl::onPreDynamics, this));
simulatorItem->addPostDynamicsFunction(boost::bind(&GLVisionSimulatorItemImpl::onPostDynamics, this));
if(useQueueThreadForAllSensors){
while(!rendererQueue.empty()){
rendererQueue.pop();
}
isQueueRenderingTerminationRequested = false;
queueThread = boost::thread(
boost::bind(&GLVisionSimulatorItemImpl::queueRenderingLoop, this));
}
return true;
}
return false;
}
