void MotionCore::init() {
if (type_ == CORE_INIT) {
FrameInfoBlock *frame_info;
memory_.getBlockByName(frame_info,"frame_info");
if (frame_info->source == MEMORY_SIM) {
std::cout << "MOTION CORE: SIM" << std::endl;
type_ = CORE_SIM;
} else if (frame_info->source == MEMORY_ROBOT) {
std::cout << "MOTION CORE: ROBOT" << std::endl;
type_ = CORE_ROBOT;
} else {
std::cerr << "Unknown memory type when init vision core" << std::endl;
exit(1);
}
}
inst_ = this;
setMemoryVariables();
initMemory();
initModules();
fps_time_ = frame_info_->seconds_since_start;
time_motion_started_ = frame_info_->seconds_since_start;
walk_request_->walk_type_ = INVALID_WALK;
loadCalibration();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "refill_checker");
ros::NodeHandle nh;
pub_ = nh.advertise<std_msgs::Bool>("/needs_refill", 1);
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("camera/rgb/image_raw", 1, imageCallback);
std_msgs::Bool bee;
bee.data = false;
loadCalibration();
while(ros::ok())
{
double ratio = bad / big_ol_size;
if (ratio > EMPTY_THRESHOLD)
{
bee.data = true;
pub_.publish(bee);
}
else
{
bee.data = false;
pub_.publish(bee);
}
ros::spinOnce();
}
return 0;
}
//--------------------------------------------------------------
void testApp::setup()
{
//the pirmary listener can't yet be set by default, so even if you only have one window, you need to call this line
ofxFensterManager::get()->getPrimaryWindow()->addListener(this);
ofSetFrameRate(30);
ofBackground(0, 0, 0);
ofEnableAlphaBlending();
ofSetVerticalSync(true);
ofSetWindowTitle("Control Panel");
vout.setup();
ofxFenster* win=ofxFensterManager::get()->createFenster(500,0,1024,768,OF_WINDOW);
win->addListener(&vout);
win->setWindowTitle("Output");
if(calibFile.open("calibration")){
printf("Calibration file found\n");
calibFile.close();
loadCalibration();
}else{
printf("Calibration file not found: using defaults\n");
skHratio = ofGetWidth()/640.0;
skVratio = ofGetHeight()/480.0;
skelHoffset = 0;
skelVoffset = 0;
spdWings = 0;
hgtWings = 0;
sclWings = 1;
saveCalibration();
}
bKinect = true;
if(bKinect){
context.setupUsingXMLFile(ofToDataPath("openni/config/ofxopenni_config.xml",true));
context.getXnContext().SetGlobalMirror(false);
depth.setup(&context);
user.setup(&context, &depth);
}
wing.setup();
wLa.set(ofGetWidth()/2,ofGetHeight()/2);
wRa.set(ofGetWidth()/2,ofGetHeight()/2);
head.setup();
bDebug = true;
bCalib = false;
bMouseMode = false;
bWingsGrown= false;
trkEase = 0.6;
scene = 0;
growAmtR = 0;
growAmtL = 0;
oldHandL = 0;
oldHandR = 0;
cellEmitter.clear();
font.loadFont("verdana.ttf",60);
curS = 0;
}
int setCal(char *buf, int len) {
int retVal = writeFile(CAL_FILE, buf, len);
loadCalibration();
return retVal;
}
//--------------------------------------------------------------
void calibrationManager::update(){
calibrationRectangle.x = 0;
calibrationRectangle.y = 0;
calibrationRectangle.width = ofGetWidth();
calibrationRectangle.height = ofGetHeight();
float widthPad = ofGetWidth() * 0.025f;
float heightPad = ofGetHeight() * 0.025f;
calibrationRectangle.x += widthPad;
calibrationRectangle.y += heightPad;
calibrationRectangle.width -= widthPad*2;
calibrationRectangle.height -= heightPad*2;
panel.update();
nDivisionsWidth = panel.getValueI("N_DIV_W");
nDivisionsHeight = panel.getValueI("N_DIV_H");;
preTimePerDot = panel.getValueF("PRE_RECORD_TIME");;
recordTimePerDot = panel.getValueF("RECORD_TIME");;
smoothing = panel.getValueF("AMOUNT_SMOOTHING");
totalTimePerDot = preTimePerDot + recordTimePerDot;
if (panel.getValueB("START_AUTO")){
bPreAutomatic = true;
panel.setValueB("START_AUTO", false);
}
if (panel.getValueB("RESET_CALIB")){
clear();
panel.setValueB("RESET_CALIB", false);
}
if (panel.getValueB("SAVE_CALIB")){
if (bBeenFit){
saveCalibration();
}
panel.setValueB("SAVE_CALIB", false);
}
if (panel.getValueB("LOAD_CALIB")){
loadCalibration();
panel.setValueB("LOAD_CALIB", false);
}
if ((bAutomatic == true && bAmInAutodrive == true) || bPreAutomatic){
menuEnergy = 0.94f * menuEnergy + 0.06f * 0.0f;
} else {
menuEnergy = 0.94f * menuEnergy + 0.06f * 1.0f;
}
// do the auto:
if (bAutomatic == true && bAmInAutodrive == true){
int nPts = nDivisionsWidth * nDivisionsHeight;
float totalTime = totalTimePerDot * nPts;
if (ofGetElapsedTimef() - startTime > totalTime){
bAmInAutodrive = false;
bInAutoRecording = false;
calculate();
} else {
float diffTime = ofGetElapsedTimef() - startTime ;
int pt = (int)(diffTime / totalTimePerDot);
float diffDotTime = diffTime - pt * totalTimePerDot;
//cout << diffTime << " " << pt << " " << diffDotTime << endl;
if (diffDotTime < preTimePerDot){
autoPct = (diffDotTime / preTimePerDot);
bInAutoRecording = false;
} else{
autoPct = (diffDotTime - preTimePerDot) / recordTimePerDot;
bInAutoRecording = true;
}
pos = pt;
}
}
inputEnergy *= 0.98f;
int xx = (pos % nDivisionsWidth);
int yy = (pos / nDivisionsWidth);
bool bEven = false;
if (yy % 2 == 0) bEven = true;
//xp = ((float)ofGetWidth() / (float)(nDivisionsWidth-1)) * xx;
//yp = ofGetHeight() - ((float)ofGetHeight() / (float)(nDivisionsHeight-1)) * yy;
xp = bEven ? calibrationRectangle.x + ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx :
calibrationRectangle.x + (calibrationRectangle.width - ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx);
yp = calibrationRectangle.y + calibrationRectangle.height - ((float)calibrationRectangle.height / (float)(nDivisionsHeight-1)) * yy;
}
bool MainController::setup()
{
pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
Volume::get(ConfigArgs::get().volumeSize);
Stopwatch::get().setCustomSignature(43543534);
cudaSafeCall(cudaSetDevice(ConfigArgs::get().gpu));
loadCalibration();
std::cout << "Point resolution: " << ((int)((Volume::get().getVoxelSizeMeters().x * 1000.0f) * 10.0f)) / 10.0f << " millimetres" << std::endl;
if(ConfigArgs::get().logFile.size())
{
rawRead = new RawLogReader;
logRead = static_cast<LogReader *>(rawRead);
}
else
{
liveRead = new LiveLogReader;
logRead = static_cast<LogReader *>(liveRead);
}
ThreadDataPack::get();
trackerInterface = new TrackerInterface(logRead, depthIntrinsics);
if(ConfigArgs::get().trajectoryFile.size())
{
std::cout << "Load trajectory: " << ConfigArgs::get().trajectoryFile << std::endl;
trackerInterface->loadTrajectory(ConfigArgs::get().trajectoryFile);
}
systemComponents.push_back(trackerInterface);
ThreadDataPack::get().assignFrontend(trackerInterface->getFrontend());
cloudSliceProcessor = new CloudSliceProcessor();
systemComponents.push_back(cloudSliceProcessor);
if(ConfigArgs::get().extractOverlap)
{
trackerInterface->enableOverlap();
}
if(!ConfigArgs::get().incrementalMesh && ConfigArgs::get().enableMeshGenerator)
{
meshGenerator = new MeshGenerator();
systemComponents.push_back(meshGenerator);
}
else
{
ThreadDataPack::get().meshGeneratorFinished.assignValue(true);
}
if(ConfigArgs::get().vocabFile.size() && ConfigArgs::get().onlineDeformation)
{
deformation = new Deformation;
placeRecognition = new PlaceRecognition(depthIntrinsics);
systemComponents.push_back(deformation);
systemComponents.push_back(placeRecognition);
}
else
{
ThreadDataPack::get().deformationFinished.assignValue(true);
ThreadDataPack::get().placeRecognitionFinished.assignValue(true);
}
pangoVis = new PangoVis(depthIntrinsics);
return true;
}
// --------------------------------------------------- LOAD & SAVE
bool ofxMapaMok::loadMesh(string _daeModel, int _textWidth, int _textHeight){
bool fileLoaded = false;
// Cargamos el modelo
//
ofxAssimpModelLoader model;
if (model.loadModel(_daeModel)){
textWidth = _textWidth;
textHeight = _textHeight;
fileLoaded = true;
// store model name, will be used later to save xml settings
//
modelFile = _daeModel;
//objectMesh = model.getMesh( model.getNumMeshes() - 1 );
for (int i=model.getNumMeshes()-1; i>=0; i--){
ofMesh m = model.getMesh(i);
objectMesh.append( m );
}
// Check to see if vertices match texture coordinates
//
int n = objectMesh.getNumVertices();
if ( n != objectMesh.getNumTexCoords() ){
ofLogError() << "[ofxMapamok]: not same amount of texCoords for all vertices";
}
// Hm: this checks for normalized texcoords. Maybe if non pow2 don't multiply??
bool bNormalized = true;
for(int i = 0; i < objectMesh.getNumVertices(); i++){
if ( objectMesh.getNumTexCoords() > i ){
float x = objectMesh.getTexCoords()[i].x;
float y = objectMesh.getTexCoords()[i].y;
if ( (x > 1) || (y > 1) || (x < -1) || (y < -1)){
ofLog(OF_LOG_ERROR,"TexCoord " + ofToString(i) + " it's out of normalized values");
ofLogError()<<x<<":"<<y<<endl;
//bNormalized = false;
}
objectMesh.getTexCoords()[i] = ofVec2f( bNormalized ? x*textWidth : x, bNormalized ? y*textHeight : y);
} else {
objectMesh.addTexCoord(ofVec2f(0,0));
}
}
// Create & assign values to vectors based on model's points
//
objectPoints.resize(n);
imagePoints.resize(n);
referencePoints.resize(n, false);
for(int i = 0; i < n; i++) {
objectPoints[i] = ofxCv::toCv(objectMesh.getVertex(i));
}
loadCalibration( modelFile.substr( 0, modelFile.length() - 4) + ".xml");
return true;
}
return false;
}
MainController::MainController(int argc, char * argv[])
: good(true),
eFusion(0),
gui(0),
groundTruthOdometry(0),
logReader(0),
framesToSkip(0),
resetButton(false),
resizeStream(0)
{
std::string empty;
iclnuim = Parse::get().arg(argc, argv, "-icl", empty) > -1;
std::string calibrationFile;
Parse::get().arg(argc, argv, "-cal", calibrationFile);
Resolution::getInstance(640, 480);
if(calibrationFile.length())
{
loadCalibration(calibrationFile);
}
else
{
Intrinsics::getInstance(528, 528, 320, 240);
}
Parse::get().arg(argc, argv, "-l", logFile);
if(logFile.length())
{
logReader = new RawLogReader(logFile, Parse::get().arg(argc, argv, "-f", empty) > -1);
}
else
{
logReader = new LiveLogReader(logFile, Parse::get().arg(argc, argv, "-f", empty) > -1);
good = ((LiveLogReader *)logReader)->asus->ok();
}
if(Parse::get().arg(argc, argv, "-p", poseFile) > 0)
{
groundTruthOdometry = new GroundTruthOdometry(poseFile);
}
confidence = 10.0f;
depth = 3.0f;
icp = 10.0f;
icpErrThresh = 5e-05;
covThresh = 1e-05;
photoThresh = 115;
fernThresh = 0.3095f;
timeDelta = 200;
icpCountThresh = 35000;
start = 1;
so3 = !(Parse::get().arg(argc, argv, "-nso", empty) > -1);
end = std::numeric_limits<unsigned short>::max(); //Funny bound, since we predict times in this format really!
Parse::get().arg(argc, argv, "-c", confidence);
Parse::get().arg(argc, argv, "-d", depth);
Parse::get().arg(argc, argv, "-i", icp);
Parse::get().arg(argc, argv, "-ie", icpErrThresh);
Parse::get().arg(argc, argv, "-cv", covThresh);
Parse::get().arg(argc, argv, "-pt", photoThresh);
Parse::get().arg(argc, argv, "-ft", fernThresh);
Parse::get().arg(argc, argv, "-t", timeDelta);
Parse::get().arg(argc, argv, "-ic", icpCountThresh);
Parse::get().arg(argc, argv, "-s", start);
Parse::get().arg(argc, argv, "-e", end);
logReader->flipColors = Parse::get().arg(argc, argv, "-f", empty) > -1;
openLoop = !groundTruthOdometry && Parse::get().arg(argc, argv, "-o", empty) > -1;
reloc = Parse::get().arg(argc, argv, "-rl", empty) > -1;
frameskip = Parse::get().arg(argc, argv, "-fs", empty) > -1;
quiet = Parse::get().arg(argc, argv, "-q", empty) > -1;
fastOdom = Parse::get().arg(argc, argv, "-fo", empty) > -1;
rewind = Parse::get().arg(argc, argv, "-r", empty) > -1;
frameToFrameRGB = Parse::get().arg(argc, argv, "-ftf", empty) > -1;
gui = new GUI(logFile.length() == 0, Parse::get().arg(argc, argv, "-sc", empty) > -1);
gui->flipColors->Ref().Set(logReader->flipColors);
gui->rgbOnly->Ref().Set(false);
gui->pyramid->Ref().Set(true);
gui->fastOdom->Ref().Set(fastOdom);
gui->confidenceThreshold->Ref().Set(confidence);
gui->depthCutoff->Ref().Set(depth);
gui->icpWeight->Ref().Set(icp);
gui->so3->Ref().Set(so3);
gui->frameToFrameRGB->Ref().Set(frameToFrameRGB);
resizeStream = new Resize(Resolution::getInstance().width(),
Resolution::getInstance().height(),
Resolution::getInstance().width() / 2,
Resolution::getInstance().height() / 2);
}
Drive::Drive(PWM *pwm, Accelerometer *accel, Gyroscope *gyro, int frontleft,
int frontright, int rearright, int rearleft, int update_rate,
int smoothing) {
mAccelerometer = accel;
mGyroscope = gyro;
mUpdateRate = update_rate;
mTimerID = 0;
mTimerEnabled = false;
mSmoothing = smoothing;
mAccelValue = new Vector3<float>[mSmoothing];
for (int i = 0; i < mSmoothing; ++i) {
mAccelValue[i].x = 0.0f;
mAccelValue[i].y = 0.0f;
mAccelValue[i].z = 0.0f;
}
mGyroValue = new Vector3<float>[mSmoothing];
for (int i = 0; i < mSmoothing; ++i) {
mGyroValue[i].x = 0.0f;
mGyroValue[i].y = 0.0f;
mGyroValue[i].z = 0.0f;
}
mRotate = 0.0f;
mTranslate.x = 0.0f;
mTranslate.y = 0.0f;
mTranslate.z = 0.0f;
mRoll = 0.0f;
mPitch = 0.0f;
mYaw = 0.0f;
mTargetRoll = 0.0f;
mTargetPitch = 0.0f;
mTargetYaw = 0.0f;
mPIDRollAngle = new PIDController(mTargetRoll, 0.00f, 0.00f, 0.00f, 5);
mPIDPitchAngle = new PIDController(mTargetPitch, 0.00f, 0.00f, 0.00f, 5);
mPIDYawAngle = new PIDController(mTargetYaw, 0.00f, 0.00f, 0.00f, 5);
mPIDRollRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5);
mPIDPitchRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5);
mPIDYawRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5);
mMotors[0] = new Motor(pwm, frontleft, 1.26f, 1.6f);
mMotors[1] = new Motor(pwm, frontright, 1.26f, 1.6f);
mMotors[2] = new Motor(pwm, rearright, 1.26f, 1.6f);
mMotors[3] = new Motor(pwm, rearleft, 1.26f, 1.6f);
mAccelOffset.x = 0.0f;
mAccelOffset.y = 0.0f;
mAccelOffset.z = 0.0f;
mGyroOffset.x = 0.0f;
mGyroOffset.y = 0.0f;
mGyroOffset.z = 0.0f;
try {
loadCalibration("calibration.ini");
} catch (CalibrationException &e) {
std::cout << "WARNING: Continuimg without calibration" << std::endl;
}
// Wait for motors to prime
usleep(3000000);
gettimeofday(&mLastUpdate, NULL);
// Pre-populate mAccelValue and mGyroValue arrays
Vector3<float> aval;
Vector3<float> gval;
for (int i = 0; i < mSmoothing; ++i) {
aval = mAccelerometer->read();
gval = mGyroscope->read();
mAccelValue[i] = aval;
mGyroValue[i] = gval;
usleep(10000); // 10,000us = 100Hz
}
mAccelValueCurrent = 0;
mGyroValueCurrent = 0;
Vector3<float> accelAvg = averageAccelerometer();
Vector3<float> gyroAvg = averageGyroscope();
calculateOrientation(0.0f, accelAvg, gyroAvg);
// Make sure the motors are resting to start
stop();
}
