/**
* This function will check if the enemy is in the limited scenario,
* so it will triggered. Happens normally when the Object is seen on the screen.
*/
bool CVorticonSpriteObject::checkforScenario()
{
if ( !exists || m_type==OBJ_PLAYER ) return false;
if( m_type==OBJ_EXPLOSION || m_type==OBJ_EARTHCHUNK
|| m_type == OBJ_BRIDGE || m_type == OBJ_NONE
|| m_type == OBJ_TELEPORTER ) return true;
// Check if enemy is near enough. If he isn't, don't make him perform. Exception is on the map
if(!mp_Map->m_worldmap)
{
if(!calcVisibility()) return false;
}
onscreen = true;
if (hasbeenonscreen ||
m_type==OBJ_RAY || m_type==OBJ_ROPE ||
m_type==OBJ_ICECANNON ||
m_type==OBJ_ICECHUNK || m_type==OBJ_PLATFORM ||
m_type==OBJ_PLATVERT || m_type==OBJ_YORP ||
m_type==OBJ_FOOB || m_type==OBJ_SCRUB ||
m_type == OBJ_SECTOREFFECTOR)
{
return true;
}
return false;
}
static void ovalDraw(XtPointer cd) {
MedmOval *po = (MedmOval *)cd;
Record *pR = po->records?po->records[0]:NULL;
DisplayInfo *displayInfo = po->updateTask->displayInfo;
XGCValues gcValues;
unsigned long gcValueMask;
Display *display = XtDisplay(po->updateTask->displayInfo->drawingArea);
DlOval *dlOval = po->dlElement->structure.oval;
#if DEBUG_VISIBILITY
print("ovalDraw: \n");
#endif
if(isConnected(po->records)) {
gcValueMask = GCForeground|GCLineWidth|GCLineStyle;
switch (dlOval->dynAttr.clr) {
case STATIC :
case DISCRETE:
gcValues.foreground = displayInfo->colormap[dlOval->attr.clr];
break;
case ALARM :
gcValues.foreground = alarmColor(pR->severity);
break;
default :
gcValues.foreground = displayInfo->colormap[dlOval->attr.clr];
medmPrintf(1,"\novalDraw: Unknown attribute\n");
break;
}
gcValues.line_width = dlOval->attr.width;
gcValues.line_style = ((dlOval->attr.style == SOLID) ?
LineSolid : LineOnOffDash);
XChangeGC(display,displayInfo->gc,gcValueMask,&gcValues);
/* Draw depending on visibility */
if(calcVisibility(&dlOval->dynAttr, po->records))
drawOval(po);
if(!pR->readAccess) {
drawBlackRectangle(po->updateTask);
}
} else if(isStaticDynamic(&dlOval->dynAttr, True)) {
/* clr and vis are both static */
gcValueMask = GCForeground|GCLineWidth|GCLineStyle;
gcValues.foreground = displayInfo->colormap[dlOval->attr.clr];
gcValues.line_width = dlOval->attr.width;
gcValues.line_style = ((dlOval->attr.style == SOLID) ?
LineSolid : LineOnOffDash);
XChangeGC(display,displayInfo->gc,gcValueMask,&gcValues);
drawOval(po);
} else {
gcValueMask = GCForeground|GCLineWidth|GCLineStyle;
gcValues.foreground = WhitePixel(display,DefaultScreen(display));
gcValues.line_width = dlOval->attr.width;
gcValues.line_style = ((dlOval->attr.style == SOLID) ?
LineSolid : LineOnOffDash);
XChangeGC(display,displayInfo->gc,gcValueMask,&gcValues);
drawOval(po);
}
}
int main(int argc, char *argv[]) {
// command line options
GeneralConfig::scale = 10;
SceneConfig::enableIK = SceneConfig::enableHaptics = false;
SceneConfig::enableRobot = true;
Parser parser;
parser.addGroup(TrackingConfig());
parser.addGroup(RecordingConfig());
parser.addGroup(GeneralConfig());
parser.addGroup(BulletConfig());
parser.addGroup(SceneConfig());
parser.read(argc,argv);
// comm stuff
initComm();
FileSubscriber pcSub("kinect","pcd");
CloudMessage cloudMsg;
FileSubscriber ropeSub("rope_pts","pcd");
CloudMessage ropeMsg;
FileSubscriber labelSub("labels","png");
ImageMessage labelMsg;
FileSubscriber endSub("rope_ends","txt");
VecVecMessage<float> endMsg;
FileSubscriber jointSub("joint_states","txt");
Retimer<VectorMessage<double> > retimer(&jointSub);
Scene scene;
PR2Manager pr2m(scene);
MonitorForGrabbing lMonitor(pr2m.pr2->robot->GetManipulators()[5], scene.env->bullet->dynamicsWorld);
MonitorForGrabbing rMonitor(pr2m.pr2->robot->GetManipulators()[7], scene.env->bullet->dynamicsWorld);
vector<double> firstJoints = doubleVecFromFile(filePath("data000000000000.txt", "joint_states").string());
ValuesInds vi = getValuesInds(firstJoints);
pr2m.pr2->setDOFValues(vi.second, vi.first);
// get kinect transform
KinectTrans kinectTrans(pr2m.pr2->robot);
kinectTrans.calibrate(btTransform(btQuaternion(0.669785, -0.668418, 0.222562, -0.234671), btVector3(0.263565, -0.038203, 1.762524)));
CoordinateTransformer CT(kinectTrans.getKinectTrans());
// load table
/////////////// load table
vector<btVector3> tableCornersCam = toBulletVectors(floatMatFromFile(onceFile("table_corners.txt").string()));
vector<btVector3> tableCornersWorld = CT.toWorldFromCamN(tableCornersCam);
BulletObject::Ptr table = makeTable(tableCornersWorld, .1*GeneralConfig::scale);
table->setColor(1,1,1,.25);
// load rope
vector<btVector3> ropePtsCam = toBulletVectors(floatMatFromFile(onceFile("init_rope.txt").string()));
CapsuleRope::Ptr rope(new CapsuleRope(CT.toWorldFromCamN(ropePtsCam), .0075*METERS));
// plots
PointCloudPlot::Ptr kinectPts(new PointCloudPlot(2));
CorrPlots corrPlots;
// setup scene
if (TrackingConfig::showKinect) scene.env->add(kinectPts);
scene.env->add(rope);
scene.env->add(table);
if (TrackingConfig:: showLines) scene.env->add(corrPlots.m_lines);
lMonitor.setBodies(rope->children);
rMonitor.setBodies(rope->children);
// recording
ScreenRecorder* rec;
if (RecordingConfig::record != DONT_RECORD){
rec = new ScreenRecorder(scene.viewer);
}
// end tracker
vector<RigidBodyPtr> rope_ends;
rope_ends.push_back(rope->bodies[0]);
rope_ends.push_back(rope->bodies[rope->bodies.size()-1]);
MultiPointTrackerRigid endTracker(rope_ends,scene.env->bullet->dynamicsWorld);
TrackerPlotter trackerPlotter(endTracker);
//scene.env->add(trackerPlotter.m_fakeObjects[0]);
//scene.env->add(trackerPlotter.m_fakeObjects[1]);
scene.startViewer();
scene.setSyncTime(true);
scene.idle(true);
vector<double> oldvals, newvals;
int count=0;
while (pcSub.recv(cloudMsg)) {
ENSURE(ropeSub.recv(ropeMsg));
vector<btVector3> obsPts = CT.toWorldFromCamN(toBulletVectors(ropeMsg.m_data));
ENSURE(labelSub.recv(labelMsg));
cv::Mat labels = toSingleChannel(labelMsg.m_data);
ENSURE(endSub.recv(endMsg));
vector<btVector3> newEnds = CT.toWorldFromCamN(toBulletVectors(endMsg.m_data));
endTracker.update(newEnds);
trackerPlotter.update();
ColorCloudPtr cloudCam = cloudMsg.m_data;
ColorCloudPtr cloudWorld(new ColorCloud());
pcl::transformPointCloud(*cloudCam, *cloudWorld, CT.worldFromCamEigen);
kinectPts->setPoints1(cloudWorld);
cout << "loaded cloud " << count << endl;
count++;
VectorMessage<double>* jointMsgPtr = retimer.msgAt(cloudMsg.getTime());
vector<double> currentJoints = jointMsgPtr->m_data;
ValuesInds vi = getValuesInds(currentJoints);
newvals = vi.first;
if (oldvals.size()==0) {
cout << "first one" << endl;
oldvals = newvals;
}
CT.reset(kinectTrans.getKinectTrans());
cv::Mat ropeMask = toSingleChannel(labels) == 1;
for (int iter=0; iter<TrackingConfig::nIter; iter++) {
cout << "iteration " << iter << endl;
pr2m.pr2->setDOFValues(vi.second, interpolateBetween(oldvals, newvals, (iter+0.00001)/TrackingConfig::nIter));
vector<btVector3> estPts = rope->getNodes();
Eigen::MatrixXf ropePtsCam = toEigenMatrix(CT.toCamFromWorldN(estPts));
vector<float> pVis = calcVisibility(rope->bodies, scene.env->bullet->dynamicsWorld, CT.worldFromCamUnscaled.getOrigin()*METERS, TrackingConfig::sigA*METERS, TrackingConfig::nSamples);
colorByVisibility(rope, pVis);
SparseArray corr = toSparseArray(calcCorrProb(toEigenMatrix(estPts), toEigenMatrix(obsPts), toVectorXf(pVis), TrackingConfig::sigB*METERS, TrackingConfig::outlierParam),TrackingConfig::cutoff);
corrPlots.update(estPts, obsPts, corr);
vector<btVector3> impulses = calcImpulsesSimple(estPts, obsPts, corr, TrackingConfig::impulseSize);
applyImpulses(impulses, rope);
if (RecordingConfig::record == EVERY_ITERATION ||
RecordingConfig::record == FINAL_ITERATION && iter==TrackingConfig::nIter-1)
rec->snapshot();
lMonitor.update();
rMonitor.update();
scene.step(DT);
}
oldvals = newvals;
}
}
