GLWidget::GLWidget(QWidget *p_parent)
: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), p_parent)
, m_Light()
, m_World()
, m_GlView()
, m_MoverController()
, m_ShuttleBoundingBox()
, m_MotionTimer()
{
connect(&m_GlView, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
m_Light.setPosition(4000.0, 40000.0, 80000.0);
//m_GlView.setBackgroundColor(Qt::white);
m_Light.setAmbientColor(Qt::lightGray);
m_GlView.cameraHandle()->setDefaultUpVector(glc::Z_AXIS);
m_GlView.cameraHandle()->setIsoView();
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 1.0);
m_MoverController= GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
createScene();
// Signal and slot connection
connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(rotateView()));
qDebug() << glc::X_AXIS.signedAngleWithVect(-glc::Y_AXIS, glc::Z_AXIS);
qDebug() << fmod(glc::X_AXIS.signedAngleWithVect(-glc::Y_AXIS, glc::Z_AXIS), 2.0 * glc::PI);
}
ModelViewGadgetWidget::ModelViewGadgetWidget(QWidget *parent)
: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent)
, m_Light()
, m_World()
, m_GlView()
, m_MoverController()
, m_ModelBoundingBox()
, m_MotionTimer()
, acFilename()
, bgFilename()
, vboEnable(false)
{
connect(&m_GlView, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding);
m_Light.setPosition(4000.0, 40000.0, 80000.0);
// m_GlView.setBackgroundColor(Qt::white);
m_Light.setAmbientColor(Qt::lightGray);
m_GlView.cameraHandle()->setDefaultUpVector(glc::Z_AXIS);
m_GlView.cameraHandle()->setRearView();
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 0.0);
m_MoverController = GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
CreateScene();
// Get required UAVObjects
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
attState = AttitudeActual::GetInstance(objManager);
connect(&m_MotionTimer, SIGNAL(timeout()), this, SLOT(updateAttitude()));
}
GLWidget::GLWidget(QWidget *p_parent)
: QGLWidget(new QGLContext(QGLFormat(QGL::SampleBuffers)), p_parent)
, m_Light()
, m_GlView()
, m_MoverController()
{
//////////////////////////// GLC specific///////////////////////////////////////
connect(&m_GlView, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
m_Light.setPosition(1.0, 1.0, 1.0);
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 1.0);
m_MoverController= GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
// Create objects to display
CreateScene();
//////////////////////////End GLC specific/////////////////////////////////////
}
GLWidget::GLWidget(QWidget *parent)
: QGLWidget(new GLC_Context(QGLFormat(QGL::SampleBuffers)), parent)
, _glc_view()
, _glc_mover_controller()
, _glc_light()
,_glc_world()
{
connect(&_glc_view, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
_glc_light.setPosition(4000.0, 40000.0, 80000.0);
_glc_light.setAmbientColor(QColor(0.1,0.1,0.8));
_glc_view.cameraHandle()->setDefaultUpVector(glc::Z_AXIS);
_glc_view.cameraHandle()->setIsoView();//set camera view
_glc_mover_controller = GLC_Factory::instance()->createDefaultMoverController(Qt::green, &_glc_view);//create move controller
isSet = false;
//create or load scene
//createScene();
}
GLWidget::GLWidget(QWidget *p_parent)
: QGLWidget(new QGLContext(QGLFormat(QGL::SampleBuffers)),p_parent)
, m_Cylinder(GLC_Factory::instance()->createCylinder(1.0, 2.0)) // Cylinder definition.
, m_Collection()
, m_light()
, m_GlView()
, m_MoverController()
{
connect(&m_GlView, SIGNAL(updateOpenGL()), this, SLOT(updateGL()));
// Set cylinder material
QColor matBlue;
matBlue.setRgbF(0.5, 0.8, 1.0, 1.0);
m_Cylinder.geomAt(0)->addMaterial(new GLC_Material(matBlue));
m_Collection.add(m_Cylinder);
// Set up mover controller
QColor repColor;
repColor.setRgbF(1.0, 0.11372, 0.11372, 1.0);
m_MoverController= GLC_Factory::instance()->createDefaultMoverController(repColor, &m_GlView);
// Set iso view
m_GlView.cameraHandle()->setIsoView();
}
/*****************************************************************************
******************************************************************************
Function Name : read_user_sensors
Date : Dec 1997
Remarks:
gets sensor readings from the robot and converts them into standard
units
******************************************************************************
Parameters: (i/o = input/output)
raw (o): the raw sensor readings
misc_raw(o): the raw miscellaneous sensor readings
*****************************************************************************/
int read_user_sensors(SL_Jstate *raw, double *misc_raw) {
int rc;
if( (rc =rt_mutex_acquire(&gdc_mutex, rt_timer_ns2ticks(GDC_RT_MUTEX_TIMEOUT))) )
{
printf("MOTOR SERVO>> Error cannot acquire gdc mutex, error code %d\n", rc);
return FALSE;
}
//we just need to update the file with what we received
gdc_network.checkForReceivedMessages();
#ifdef HAS_LOWER_BODY
if(use_feet)
gdc_network.checkFootSensorReceivedMessages();
#endif
//now we translate things into SL happy data
gdc_sl_interface.translateJointStates(gdc_network.gdc_card_states_, raw);
if(use_feet)
gdc_sl_interface.translateMiscSensors(gdc_network.foot_sensor_states_, misc_raw);
//read imu
if(IMU_MODE_DONT_USE == imu_mode)
{
misc_raw[B_Q0_IMU] = 0.5;
misc_raw[B_Q1_IMU] = 0.5;
misc_raw[B_Q2_IMU] = 0.5;
misc_raw[B_Q3_IMU] = -0.5;
}
else
{
SL_quat orientation;
SL_Cstate position;
double unstab_acc[3];
// unstab_acc[0] = 0.0;
// unstab_acc[1] = 0.0;
// unstab_acc[2] = 0.0;
switch (imu_mode) {
case IMU_MODE_RT_STREAM:
double imu_time;
if (!imu_interface_rt.readDataThreadSafe(unstab_acc,
&orientation.ad[1], imu_time)) {
printf("Could not get data from imu\n");
}
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
misc_raw[B_TSTAMP_IMU] = imu_time;
break;
case IMU_MODE_NONRT:
imu_interface_nonrt.readPose(orientation, position, unstab_acc);
memcpy(&(misc_raw[B_XACC_IMU]), &(position.xdd[_X_]), 3 * sizeof(double));
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
memcpy(&(misc_raw[B_Q0_IMU]), &(orientation.q[_Q0_]), 4 * sizeof(double));
break;
case IMU_MODE_NONRT_STREAM:
imu_interface_nonrt_stream.readData(unstab_acc, &orientation.ad[1],
imu_time);
std::cout << "acc " << unstab_acc[0] << "orient " << orientation.ad[1] << std::endl;
memcpy(&(misc_raw[B_XACC_UNSTAB_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
misc_raw[B_TSTAMP_IMU] = imu_time;
break;
case IMU_MODE_PF_NONRT_STREAM:
pf_imu_interface_nonrt_stream.getAG(unstab_acc, &orientation.ad[1]);
memcpy(&(misc_raw[B_XACC_IMU]), unstab_acc, 3 * sizeof(double));
memcpy(&(misc_raw[B_AD_A_IMU]), &(orientation.ad[_A_]), 3 * sizeof(double));
break;
default:
rt_printf("bad imu_mode\n");
return false;
}
}
//TODO set a default SL value for inactive joints
if( (rc = rt_mutex_release(&gdc_mutex)) )
{
printf("Error cannot release mutex, error code %d\n", rc);
return FALSE;
}
if(!updateOpenGL(raw, misc_raw))
{
///////////////////////////////////////////////////////////////////////
///////////////To avoid output
////////////////////////////////////////////////////////////////////
//printf("ERROR>>SL_user_sensor_proc: cannot update OpenGL\n");
}
return TRUE;
}
