void HydraManager::Initialize()
{
sixenseInit();
sixenseUtils::ControllerManager* controllerManager = sixenseUtils::getTheControllerManager();
sixenseUtils::getTheControllerManager()->setGameType( sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER );
sixenseUtils::getTheControllerManager()->registerSetupCallback(controlManagerSetupCallback);
}
SixenseManager::SixenseManager() {
#ifdef HAVE_SIXENSE
_lastMovement = 0;
_calibrationState = CALIBRATION_STATE_IDLE;
// By default we assume the _neckBase (in orb frame) is as high above the orb
// as the "torso" is below it.
_neckBase = glm::vec3(NECK_X, -NECK_Y, NECK_Z);
sixenseInit();
#endif
}
openni::Status openSixenseDevice() {
if (sixenseInit() != SIXENSE_SUCCESS) {
printf("couldn't init Sixense library\n");
return openni::STATUS_ERROR;
}
// Init the controller manager. This makes sure the controllers are present, assigned to left and right hands, and that
// the hemisphere calibration is complete.
sixenseUtils::getTheControllerManager()->setGameType( sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER );
// sixenseUtils::getTheControllerManager()->registerSetupCallback( controller_manager_setup_callback );
printf("done all sixense init!\n");
return openni::STATUS_OK;
}
void Hydra_Tracker::StartTracker(QFrame* videoFrame)
{
//QMessageBox::warning(0,"FaceTrackNoIR Notification", "Tracking loading settings...",QMessageBox::Ok,QMessageBox::NoButton);
loadSettings();
// Init sixense
//QMessageBox::warning(0,"OpenTrack Info", "sixense init",QMessageBox::Ok,QMessageBox::NoButton);
sixenseInit();
//QMessageBox::warning(0,"OpenTrack Info", "sixense init complete, setting controller manager",QMessageBox::Ok,QMessageBox::NoButton);
// Init the controller manager. This makes sure the controllers are present, assigned to left and right hands, and that
// the hemisphere calibration is complete.
//sixenseUtils::getTheControllerManager()->setGameType( sixenseUtils::ControllerManager::ONE_PLAYER_TWO_CONTROLLER );
//sixenseUtils::getTheControllerManager()->registerSetupCallback( controller_manager_setup_callback );
//QMessageBox::warning(0,"OpenTrack Info", "controller manager callback registered",QMessageBox::Ok,QMessageBox::NoButton);
return;
}
dev::sixense::sixense() :
status(false),
flying(false)
{
if ((status = (sixenseInit() == SIXENSE_SUCCESS &&
sixenseSetActiveBase(0) == SIXENSE_SUCCESS)))
{
float nr = ::conf->get_f("sixense_filter_near_range", 500.00);
float nv = ::conf->get_f("sixense_filter_near_value", 0.95);
float fr = ::conf->get_f("sixense_filter_far_range", 1600.00);
float fv = ::conf->get_f("sixense_filter_far_value", 0.99);
sixenseSetFilterParams(nr, nv, fr, fv);
sixenseSetFilterEnabled(1);
}
move_rate = ::conf->get_f("sixense_move_rate", 30.0);
turn_rate = ::conf->get_f("sixense_turn_rate", 120.0);
hand_controller = ::conf->get_i("sixense_hand_controller", 0);
fly_button = ::conf->get_i("sixense_fly_button ", 1);
}
SixenseManager::SixenseManager() {
#ifdef HAVE_SIXENSE
_lastMovement = 0;
_amountMoved = glm::vec3(0.0f);
_lowVelocityFilter = false;
_calibrationState = CALIBRATION_STATE_IDLE;
// By default we assume the _neckBase (in orb frame) is as high above the orb
// as the "torso" is below it.
_neckBase = glm::vec3(NECK_X, -NECK_Y, NECK_Z);
sixenseInit();
#endif
_hydrasConnected = false;
_triggerPressed[0] = false;
_bumperPressed[0] = false;
_oldX[0] = -1;
_oldY[0] = -1;
_triggerPressed[1] = false;
_bumperPressed[1] = false;
_oldX[1] = -1;
_oldY[1] = -1;
}
vr::EVRInitError CServerDriver_Hydra::Init( vr::IDriverLog * pDriverLog, vr::IServerDriverHost * pDriverHost, const char * pchUserDriverConfigDir, const char * pchDriverInstallDir )
{
InitDriverLog( pDriverLog );
m_pDriverHost = pDriverHost;
m_strDriverInstallDir = pchDriverInstallDir;
if ( sixenseInit() != SIXENSE_SUCCESS )
return vr::VRInitError_Driver_Failed;
// Will not immediately detect controllers at this point. Sixense driver must be initializing
// in its own thread... It's okay to dynamically detect devices later, but if controllers are
// the only devices (e.g. requireHmd=false) we must have GetTrackedDeviceCount() != 0 before returning.
for ( int i = 0; i < 20; ++i )
{
ScanForNewControllers( false );
if ( GetTrackedDeviceCount() )
break;
Sleep( 100 );
}
m_Thread = std::thread( ThreadEntry, this );
return vr::VRInitError_None;
}
void Hydra_Tracker::StartTracker(QFrame*)
{
sixenseInit();
}
int main(int argc, char **argv)
{
ArgsParser arg(argc, argv);
// get help
if (argc < 2
|| 0 == strcasecmp(argv[1], "-h")
|| 0 == strcasecmp(argv[1], "--help"))
{
displayHelp(argv[0]);
exit(-1);
}
const char *target = arg.getOpt("-t", "--target", "localhost:7777");
const char *rateStr = arg.getOpt("-r", "--rate", "20");
const char *sendRate = arg.getOpt("-s", "--sendrate", "16");
// get port on which the dtrack sends the data
float rate = atof(rateStr);
// verbose max. once per second
int sendsPerVerbose;
if (rate < 1)
sendsPerVerbose = 1;
else
sendsPerVerbose = (int)rate;
// select() delay record
rate = 1.0 / rate;
struct timeval delay;
delay.tv_sec = (int)rate;
delay.tv_usec = (int)(1e6 * (rate - delay.tv_sec));
// +++++++++++++++ prepare mapping +++++++++++++++
int artID[MAXSENSORS], coverID[MAXSENSORS]; // COVER IDs for sensor[i]
if (arg.numArgs() > MAXSENSORS)
{
std::cerr << "Only " << MAXSENSORS << " sensors allowed" << std::endl;
}
int i;
int numSensors = 0;
for (i = 0; i < arg.numArgs(); i++)
{
coverID[numSensors] = i;
bool ok = splitOpt(arg[i], artID[numSensors], coverID[numSensors]);
if (ok)
{
numSensors++;
}
else
{
std::cerr << "Illegal device selection: " << arg[i] << std::endl;
exit(0);
}
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Show what we will do
printf("\n");
printf(" +-----------------------------------------------------+\n");
printf(" + VRC HydraServer %-10s (C) 2005 HLRS +\n", HydraServerVersion);
printf(" +-----------------------------------------------------+\n");
printf(" + Settings: +\n");
printf(" + UDP Target: %-30s +\n", target);
printf(" + Send Rate: %-3.1f Packets/s +\n", 1.0 / rate);
printf(" +-----------------------------------------------------+\n");
printf(" + Mapping: +\n");
for (i = 0; i < numSensors; i++)
{
printf(" + Target %c%-2d --> COVER ID %-2d +\n",
((artID[i] >= 10) ? 'F' : 'B'),
(artID[i] % 10) + 1,
coverID[i]);
}
printf(" +-----------------------------------------------------+\n\n");
/// ++++++++++++++++++++++++++ All parameters set - start work +++++++++++++++++++
signal(SIGINT, sigHandler);
#ifndef WIN32
signal(SIGPIPE, sigHandler);
signal(SIGCHLD, sigHandler);
#endif
signal(SIGTERM, sigHandler);
// create udp socket
UDP_Sender sender(target);
if (sender.isBad())
{
std::cerr << "Could not start UDP server to "
<< target << " : "
<< sender.errorMessage() << std::endl;
return -1;
}
int init = sixenseInit();
int activebase = sixenseSetActiveBase(0);
int basecolor = sixenseSetBaseColor(255, 0, 0);
sixenseAllControllerData acd;
int reshigh = sixenseSetHighPriorityBindingEnabled(1);
int autoenable = sixenseAutoEnableHemisphereTracking(1);
int frame = 0;
while (1)
{
sixenseGetAllNewestData(&acd);
Sleep((int)*sendRate);
/*fprintf(stderr,"left pos: x = %f\ty = %f\tz = %f\nright pos: x = %f\ty = %f\t z = %f\n\n",
acd.controllers[0].pos[0],acd.controllers[0].pos[1],acd.controllers[0].pos[2],
acd.controllers[1].pos[0],acd.controllers[1].pos[1],acd.controllers[1].pos[2]); */
for (i = 0; i < numSensors; i++)
{
char sendbuffer[2048];
sprintf(sendbuffer, "VRC %d %3d [%5.1f %5.1f %5.1f] - [%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f] - [ %d %d %s]",
coverID[i], acd.controllers[i].buttons, acd.controllers[i].pos[0], acd.controllers[i].pos[1], acd.controllers[i].pos[2],
acd.controllers[i].rot_mat[0][0], acd.controllers[i].rot_mat[0][1], acd.controllers[i].rot_mat[0][2],
acd.controllers[i].rot_mat[1][0], acd.controllers[i].rot_mat[1][1], acd.controllers[i].rot_mat[1][2],
acd.controllers[i].rot_mat[2][0], acd.controllers[i].rot_mat[2][1], acd.controllers[i].rot_mat[2][2],
acd.controllers[i].joystick_x, acd.controllers[i].joystick_y, compatibilityString);
sender.send(sendbuffer);
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "%s\n", sendbuffer);
}
}
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "---\n");
}
frame++;
}
sixenseExit();
return 0;
}
void FlyingMouse::Init()
{
#ifdef USE_SIXENSE
sixenseInit();
#endif
}
//===========================================================================
cHydraDevice::cHydraDevice(unsigned int a_deviceNumber):
a_deviceNumber(a_deviceNumber)
{
// the connection to your device has not yet been established.
m_deviceReady = false;
/************************************************************************
STEP 1:
Here you should complete the specifications of your device.
These values only need to be estimates. Since haptic devices often perform
differently depending of their configuration withing their workspace,
simply use average values.
*************************************************************************/
//-----------------------------------------------------------------------
// NAME:
//-----------------------------------------------------------------------
// haptic device model (see file "CGenericHapticDevice.h")
m_specifications.m_model = C_HAPTIC_DEVICE_HYDRA;
// name of the device manufacturer, research lab, university.
m_specifications.m_manufacturerName = "Razor";
// name of your device
m_specifications.m_modelName = "Hydra";
//-----------------------------------------------------------------------
// CHARACTERISTICS: (The following values must be positif or equal to zero)
//-----------------------------------------------------------------------
// the maximum force [N] the device can produce along the x,y,z axis.
m_specifications.m_maxLinearForce = 0.0; // [N]
// the maximum amount of torque your device can provide arround its
// rotation degrees of freedom.
m_specifications.m_maxAngularTorque = 0.0; // [N*m]
// the maximum amount of torque which can be provided by your gripper
m_specifications.m_maxGripperForce = 0.0; // [N]
// the maximum closed loop linear stiffness in [N/m] along the x,y,z axis
m_specifications.m_maxLinearStiffness = 0.0; // [N/m]
// the maximum amount of angular stiffness
m_specifications.m_maxAngularStiffness = 0.0; // [N*m/Rad]
// the maximum amount of stiffness supported by the gripper
m_specifications.m_maxGripperLinearStiffness = 0; // [N*m]
// the radius of the physiqual workspace of the device (x,y,z axis)
m_specifications.m_workspaceRadius = 0.3; // [m]
// DAMPING PROPERTIES:
// Start with small values as damping terms can be high;y sensitive to
// the quality of your velocity signal and the spatial resolution of your
// device. Try gradually increasing the values by using example "01-devices" and by
// enabling viscosity with key command "2".
// Maximum recommended linear damping factor Kv
m_specifications.m_maxLinearDamping = 0.0; // [N/(m/s)]
//! Maximum recommended angular damping factor Kv (if actuated torques are available)
m_specifications.m_maxAngularDamping = 0.0; // [N*m/(Rad/s)]
//! Maximum recommended angular damping factor Kv for the force gripper. (if actuated gripper is available)
m_specifications.m_maxGripperAngularDamping = 0.0; // [N*m/(Rad/s)]
//-----------------------------------------------------------------------
// CHARACTERISTICS: (The following are of boolean type: (true or false)
//-----------------------------------------------------------------------
// does your device provide sensed position (x,y,z axis)?
m_specifications.m_sensedPosition = true;
// does your device provide sensed rotations (i.e stylus)?
m_specifications.m_sensedRotation = true;
// does your device provide a gripper which can be sensed?
m_specifications.m_sensedGripper = true;
// is you device actuated on the translation degrees of freedom?
m_specifications.m_actuatedPosition = false;
// is your device actuated on the rotation degrees of freedom?
m_specifications.m_actuatedRotation = false;
// is the gripper of your device actuated?
m_specifications.m_actuatedGripper = false;
// can the device be used with the left hand?
m_specifications.m_leftHand = true;
// can the device be used with the right hand?
m_specifications.m_rightHand = true;
/************************************************************************
STEP 2:
Here, you should implement code which tells the application if your
device is actually connected to your computer and can be accessed.
In practice this may be consist in checking if your I/O board
is active or if your drivers are available.
If your device can be accessed, set:
m_systemAvailable = true;
Otherwise set:
m_systemAvailable = false;
Your actual code may look like:
bool result = checkIfMyDeviceIsAvailable()
m_systemAvailable = result;
*************************************************************************/
// *** INSERT YOUR CODE HERE ***
/* Initiate sixsense */
int r = sixenseInit();
std::cout << "a_deviceNumber " << this->a_deviceNumber
<< ": sixenseInit() returns " << r << std::endl;
sixenseSetHemisphereTrackingMode(0,1);
sixenseSetHemisphereTrackingMode(1,1);
sixenseSetActiveBase(0);
m_deviceAvailable = true; // default value.
}
