void MyHapticsGLWidget::hideEvent(QHideEvent *event)
{
MyHapticsThread *hthread = MyHapticsThread::instance();
hthread->pause();
if (m_scene) m_scene->clearForces();
// remove the renderer's FPS estimate indicator to the main window status bar
if (QMainWindow *mainWindow = qobject_cast<QMainWindow *>(window()))
mainWindow->statusBar()->removeWidget(hthread->hpsWidget());
// stop the rendering timer if mode is timer driven
killTimer(m_timerID);
}
void MyHapticsGLWidget::showEvent(QShowEvent *event)
{
MyHapticsThread *hthread = MyHapticsThread::instance();
// add the renderer's FPS estimate indicator to the main window status bar
if (QMainWindow *mainWindow = qobject_cast<QMainWindow *>(window())) {
mainWindow->statusBar()->addWidget(hthread->hpsWidget());
hthread->hpsWidget()->show();
}
if (m_isosurface) m_isosurface->reset();
// switch back to this widget's haptic scene and resume
hthread->selectScene(m_sceneIndex);
hthread->resume();
// we have to (re)start the rendering timer if mode is timer driven
m_timerID = startTimer(30);
}
int main (int argc, char* argv[])
{
printf("Starting...\n");
ros::init(argc, argv, "pr2_interactive_gripper_pose_action");
ros::NodeHandle pnh("~");
bool use_haptics = false;
pnh.param<bool>("use_haptics", use_haptics, false);
// create and start a thread for haptics
ROS_INFO("Creating haptics thread...");
MyHapticsThread *hthread = MyHapticsThread::instance();
hthread->setHapticRate(2000);
if(use_haptics)
{
hthread->start(QThread::TimeCriticalPriority);
}
// // run the application
// MyMainWindow *window = MyMainWindow::instance();
// window->show();
// int result = application.exec();
// HapticGhostedGripper* haptics = new HapticGhostedGripper();
//// ros::MultiThreadedSpinner spinner(3); // Use 4 threads
//// spinner.spin();
// ros::spin();
ROS_INFO("Creating GhostedGripperActionServer...");
GripperPoseAction ggas(use_haptics);
ROS_INFO("...spinning!");
ros::spin();
// stop the haptic servo thread and destroy it
hthread->quit();
hthread->resume();
//hthread->wait();
//delete hthread;
//sleep(1);
return (0);
}
void MyHapticsGLWidget::initializeGL()
{
// initialize the scene with our own private test volume instance
Volume *volume = m_data->getVolume();
Volume *mask = m_data->getMask();
// m_sampler = new VolumeSampler(volume, mask);
m_sampler = new PointSampler();
PointSampler *ps = dynamic_cast<PointSampler*>(m_sampler);
//ps->createSphericalShell(0.5, 0.05, -1, 1, -M_PI, M_PI, 0.0);
ps->createPlane();
ps->applyLastCloud();
//volume = m_sampler->getVolume();
// create a distance shader for starting/terminating rays
m_distanceShader = new QGLShaderProgram(this);
if (!m_distanceShader->addShaderFromSourceFile(QGLShader::Vertex, ":/Shaders/Distance.vert"))
QMessageBox::critical(this, "OpenGL Shader Compile Error", m_distanceShader->log());
if (!m_distanceShader->addShaderFromSourceFile(QGLShader::Fragment, ":/Shaders/Distance.frag"))
QMessageBox::critical(this, "OpenGL Shader Compile Error", m_distanceShader->log());
if (!m_distanceShader->link())
QMessageBox::critical(this, "OpenGL Shader Link Error", m_distanceShader->log());
// set up the visual renderer
m_renderer->initialize();
m_renderer->setVolume(volume, VolumeRenderer::vrPrimary);
m_renderer->setVolume(mask, VolumeRenderer::vrMask);
m_renderer->setIsosurfaceValue(0.5);
m_renderer->setIsosurfaceColor(colour(1.0, 1.0, 0.5, 0.5));
// create a haptics scene with all the haptic displays in it
m_scene = new HapticScene();
if (m_devicesModel)
{
int n = m_devicesModel->rowCount();
for (int i = 0; i < n; ++i) {
cGenericHapticDevice *device =
m_devicesModel->data(m_devicesModel->index(i, 2))
.value<cGenericHapticDevice *>();
if (device)
{
HapticDisplay *display = m_scene->addHapticDisplay(device);
m_displays.append(display);
// center the device at (.5, .5, .5) where the volume will be
matrix44f_c center, orient = identity_4x4();
matrix_translation(center, -.5f, -.75f, -.5f);
matrix_set_basis_vectors(orient, x_axis_3D(), -z_axis_3D(), y_axis_3D());
display->setTransform(orient * center);
// TODO: increase stiffness when speed problem is solved
double k = display->stiffness();
// display->setStiffness(0.25 * k);
}
}
}
// save obj models to disk so we can load them back
QString location = expandModelResources();
string directory = location.toStdString();
m_drill = new MeshGLM("drill", directory + "/drill.obj",
MeshGLM::k_useTexture | MeshGLM::k_useMaterial);
m_burr = new MeshGLM("burr", directory + "/burr1.obj",
MeshGLM::k_useMaterial);
// add a haptic isosurface to the scene
// m_isosurface = new HapticIsosurface(volume, mask, 0.5);
m_isosurface = new PointShellIsosurface(m_sampler, 0.5);
loadPointShell(location);
m_scene->addNode(m_isosurface);
// clean up the temporary model files from the disk
removeModelResources(location);
// add the scene to the haptics thread and select it for rendering
MyHapticsThread *hthread = MyHapticsThread::instance();
m_sceneIndex = hthread->addScene(m_scene);
hthread->selectScene(m_sceneIndex);
}
