bool BaseGeometryPatch::run()
{
/*
Wait until the user presses a key. Meanwhile, the scheduler
runs and applies forceVecs to the device.
*/
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
/* Check for errors and abort if so. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress CR to quit.\n");
return false;
}
return true;
}
void Window::initializeGL()
{
initializeOpenGLFunctions();
connect(context(), SIGNAL(aboutToBeDestroyed()), this, SLOT(teardownGL()), Qt::DirectConnection);
printVersionInformation();
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
//Loading, compiling and linking shaders.
shaderProgram = new QOpenGLShaderProgram();
shaderProgram->addShaderFromSourceFile(QOpenGLShader::Vertex, ".\\simple.vert");
shaderProgram->addShaderFromSourceFile(QOpenGLShader::Fragment, ".\\simple.frag");
shaderProgram->link();
shaderProgram->bind();
//Creating the VAO
vertexArrayObject.create();
vertexArrayObject.bind();
//We now need to combine the data of out mesh and our interator mesh
vector<Vertex> combinedVertexBuffer;
combinedVertexBuffer.reserve(mesh->getVertexVector().size() +
interatorMesh->getVertexVector().size());
combinedVertexBuffer.insert(combinedVertexBuffer.end(), mesh->
getVertexVector().begin(), mesh->getVertexVector().end());
combinedVertexBuffer.insert(combinedVertexBuffer.end(), interatorMesh->
getVertexVector().begin(), interatorMesh->getVertexVector().end());
//Index buffer
vector<unsigned int> combinedIndexBuffer;
combinedIndexBuffer.reserve(mesh->getIndexVector().size() + interatorMesh->
getIndexVector().size());
combinedIndexBuffer.insert(combinedIndexBuffer.end(), mesh->
getIndexVector().begin(), mesh->getIndexVector().end());
unsigned int vertexVectorSize = static_cast<unsigned int>(mesh->
getVertexVector().size());
for (auto index : interatorMesh->getIndexVector()) {
combinedIndexBuffer.push_back(index + vertexVectorSize);
}
//Creating the VBO for the mesh
vertexBuffer.create();
vertexBuffer.bind();
vertexBuffer.setUsagePattern(QOpenGLBuffer::DynamicDraw);
vertexBuffer.allocate(combinedVertexBuffer.data(),
static_cast<int>(combinedVertexBuffer.size() * sizeof(Vertex)));
//Creating IBO for the mesh
indexBuffer = QOpenGLBuffer(QOpenGLBuffer::IndexBuffer);
indexBuffer.create();
indexBuffer.bind();
indexBuffer.setUsagePattern(QOpenGLBuffer::DynamicDraw);
indexBuffer.allocate(combinedIndexBuffer.data(),
static_cast<int>(combinedIndexBuffer.size() * sizeof(unsigned int)));
//Enabling attribute arrays for vertex, normal and color data.
shaderProgram->enableAttributeArray(0);
shaderProgram->enableAttributeArray(1);
shaderProgram->enableAttributeArray(2);
shaderProgram->setAttributeBuffer(0, GL_FLOAT, Vertex::positionOffset(), Vertex::PositionTupleSize, Vertex::stride());
shaderProgram->setAttributeBuffer(1, GL_FLOAT, Vertex::colorOffset(), Vertex::ColorTupleSize, Vertex::stride());
shaderProgram->setAttributeBuffer(2, GL_FLOAT, Vertex::normalOffset(), Vertex::NormalTupleSize, Vertex::stride());
vertexArrayObject.release();
vertexBuffer.release();
indexBuffer.release();
shaderProgram->release();
//Setting up model, view and projection matrices.
mvpSetup();
//Creating interator ray
interator = new Ray(vec3(0.0f, 0.0f, 0.0f), vec3(0.0f, 0.0f, 1.0f));
//Setting up the collision detector
collisionDetector.setMesh(mesh);
collisionDetector.setRay(interator);
//Initializing haptic device
if (hapticsEnabled) {
haptic.initializeHL();
haptic.updateHapticWorkspace();
haptic.setInterator(interator);
haptic.setCollisionDetector(&collisionDetector);
haptic.setCollisionPath(&path);
haptic.setMesh(mesh);
haptic.setShaderProgram(shaderProgram);
haptic.setVAO(&vertexArrayObject);
handle = hdScheduleAsynchronous(HapticInterface::mainHapticCallback,
&haptic, HD_MAX_SCHEDULER_PRIORITY);
hdStartScheduler();
}
}
/******************************************************************************
Collects statistics about the update rate of the haptic device.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
HDstring model = hdGetString(HD_DEVICE_MODEL_TYPE);
std::cout << "Initialized: " << model << std::endl;
HDSchedulerHandle hServoCallback = hdScheduleAsynchronous(
ServoSchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to schedule servoloop callback" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdSetSchedulerRate(TARGET_SERVOLOOP_RATE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to set servoloop rate" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdDisable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to start servoloop" << std::endl;
hdDisableDevice(hHD);
return -1;
}
std::cout << "Printing servoloop rate stats. All numbers are in Hz units" << std::endl;
std::cout << std::endl;
char fileName[256];
sprintf(fileName, "%s Rate Stats.txt", model);
std::ofstream fout(fileName, std::ios::out | std::ios::app);
for (int nRuns = 0; nRuns < 10 && !_kbhit(); nRuns++)
{
Sleep(1000);
hdScheduleSynchronous(CopyUpdateRateStats, 0, HD_MIN_SCHEDULER_PRIORITY);
// Prints some stats about the rate as well as log it to file.
PrintUpdateRateStats(std::cout);
PrintUpdateRateStats(fout);
if (!hdWaitForCompletion(hServoCallback, HD_WAIT_CHECK_STATUS))
{
std::cerr << "Error occurred during main loop" << std::endl;
std::cerr << "Press any key to quit." << std::endl;
getch();
break;
}
}
hdStopScheduler();
hdUnschedule(hServoCallback);
hdDisableDevice(hHD);
return 0;
}
void phantomRun()
{
hdStartScheduler();
}
int main(int argc, char** argv)
{
////////////////////////////////////////////////////////////////
// Init ROS
////////////////////////////////////////////////////////////////
ros::init(argc, argv, "phantom_node");
PhantomState state;
PhantomROS phantom_ros;
////////////////////////////////////////////////////////////////
// Init Phantom
////////////////////////////////////////////////////////////////
HDErrorInfo error;
HHD hHD;
hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
//hduPrintError(stderr, &error, "Failed to initialize haptic device");
ROS_ERROR("Failed to initialize haptic device");
//: %s", &error);
return -1;
}
ROS_INFO("Found %s", hdGetString(HD_DEVICE_MODEL_TYPE));
hdEnable(HD_FORCE_OUTPUT);
// hdEnable(HD_MAX_FORCE_CLAMPING);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
ROS_ERROR("Failed to start the scheduler");
//, &error);
return -1;
}
if(phantom_ros.init(&state))
{
hdStopScheduler();
hdDisableDevice(hHD);
return -1;
}
if(phantom_ros.calibrate_)
{
HHD_Auto_Calibration();
}
hdScheduleAsynchronous(phantom_state_callback, &state, HD_MAX_SCHEDULER_PRIORITY);
////////////////////////////////////////////////////////////////
// Loop and publish
////////////////////////////////////////////////////////////////
pthread_t publish_thread;
pthread_create(&publish_thread, NULL, ros_publish, (void*)&phantom_ros);
pthread_join(publish_thread, NULL);
ROS_INFO("Ending Session...");
hdStopScheduler();
hdDisableDevice(hHD);
return 0;
}
/*******************************************************************************
Main function.
Initializes the device, starts the schedule, creates a schedule callback
to handle gravity well forces, waits for the user to press a button, exits
the application.
*******************************************************************************/
int main(int argc, char* argv[])
{
int KeyInfo;
HDErrorInfo error;
HDSchedulerHandle hGravityWell;
/* Initialize the device, must be done before attempting to call any hd
functions. Passing in HD_DEFAULT_DEVICE causes the default device to be
initialized. */
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
hIOMutex = CreateMutex(NULL, FALSE, NULL);
kill = 0;
recording = 0;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
printf("Hello Haptic Device!\n");
printf("Found device model: %s.\n\n", hdGetString(HD_DEVICE_MODEL_TYPE));
/* Schedule the main callback that will render forces to the device. */
hGravityWell = hdScheduleAsynchronous(
gravityWellCallback, 0,
HD_MAX_SCHEDULER_PRIORITY);
hdEnable(HD_FORCE_OUTPUT);
hdStartScheduler();
_beginthread( recordingLoop );
/* Check for errors and abort if so. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to start scheduler");
fprintf(stderr, "\nPress any key to quit.\n");
return -1;
}
/* Wait until the user presses a key. Meanwhile, the scheduler
runs and applies forces to the device. */
printf("Press R to record data\n");
printf("Press Q to quit.\n\n");
count = 0;
//while (!_kbhit())
do
{
KeyInfo = _getch();
if ( tolower( KeyInfo ) == 'q')
{
kill = 1;
}
else
{
if ( tolower( KeyInfo ) == 'r' )
{
if ( recording == 0) {
printf("Data recording ON\n");
recording = 1;
}
else
{
printf("Data recording OFF\n");
recording = 0;
}
}
}
/* Periodically check if the gravity well callback has exited. */
if (!hdWaitForCompletion(hGravityWell, HD_WAIT_CHECK_STATUS))
{
kill = 1;
fprintf(stderr, "Press any key to quit.\n");
getch();
break;
}
// printf("\n%d", count);
} while (!kill);
/* For cleanup, unschedule callback and stop the scheduler. */
hdStopScheduler();
hdUnschedule(hGravityWell);
/* Disable the device. */
hdDisableDevice(hHD);
return 0;
}
