void fgSpaceballHandleXEvent(const XEvent *xev)
{
spnav_event sev;
if(!sball_initialized) {
fgInitialiseSpaceball();
if(!sball_initialized) {
return;
}
}
if(spnav_x11_event(xev, &sev)) {
switch(sev.type) {
case SPNAV_EVENT_MOTION:
if(sev.motion.x | sev.motion.y | sev.motion.z) {
INVOKE_WCB(*spnav_win, SpaceMotion, (sev.motion.x, sev.motion.y, sev.motion.z));
}
if(sev.motion.rx | sev.motion.ry | sev.motion.rz) {
INVOKE_WCB(*spnav_win, SpaceRotation, (sev.motion.rx, sev.motion.ry, sev.motion.rz));
}
spnav_remove_events(SPNAV_EVENT_MOTION);
break;
case SPNAV_EVENT_BUTTON:
INVOKE_WCB(*spnav_win, SpaceButton, (sev.button.bnum, sev.button.press ? GLUT_DOWN : GLUT_UP));
break;
default:
break;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv);
ros::node node("spacenav");
node.advertise<std_msgs::Vector3>("/spacenav/offset", 2);
node.advertise<std_msgs::Vector3>("/spacenav/rot_offset", 2);
node.advertise<joy::Joy>("/spacenav/joy", 2);
if (spnav_open() == -1)
{
fprintf(stderr, "Error: Could not open the space navigator device\n");
fprintf(stderr, "Did you remember to run spacenavd (as root)?\n");
ros::fini();
return 1;
}
spnav_event sev;
int ret;
int no_motion_count = 0;
joy::Joy joystick_msg;
joystick_msg.axes.resize(6);
joystick_msg.buttons.resize(2);
while (node.ok())
{
ret = spnav_poll_event(&sev);
spnav_remove_events(SPNAV_EVENT_MOTION);
if (ret == 0)
{
if (++no_motion_count > 30)
{
no_motion_count = 0;
std_msgs::Vector3 offset_msg;
offset_msg.x = offset_msg.y = offset_msg.z = 0;
node.publish("/spacenav/offset", offset_msg);
std_msgs::Vector3 rot_offset_msg;
rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0;
node.publish("/spacenav/rot_offset", rot_offset_msg);
joystick_msg.axes[0] = offset_msg.x / FULL_SCALE;
joystick_msg.axes[1] = offset_msg.y / FULL_SCALE;
joystick_msg.axes[2] = offset_msg.z / FULL_SCALE;
joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE;
joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE;
joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE;
}
}
if (sev.type == SPNAV_EVENT_MOTION)
{
std_msgs::Vector3 offset_msg;
offset_msg.x = sev.motion.z;
offset_msg.y = -sev.motion.x;
offset_msg.z = sev.motion.y;
node.publish("/spacenav/offset", offset_msg);
std_msgs::Vector3 rot_offset_msg;
rot_offset_msg.x = sev.motion.rz;
rot_offset_msg.y = -sev.motion.rx;
rot_offset_msg.z = sev.motion.ry;
//printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y, rot_offset_msg.z);
node.publish("/spacenav/rot_offset", rot_offset_msg);
joystick_msg.axes[0] = offset_msg.x / FULL_SCALE;
joystick_msg.axes[1] = offset_msg.y / FULL_SCALE;
joystick_msg.axes[2] = offset_msg.z / FULL_SCALE;
joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE;
joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE;
joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE;
no_motion_count = 0;
}
else if (sev.type == SPNAV_EVENT_BUTTON)
{
//printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum);
joystick_msg.buttons[sev.button.bnum] = sev.button.press;
}
node.publish("/spacenav/joy", joystick_msg);
usleep(1000);
}
node.unadvertise("/spacenav/offset");
ros::fini();
return 0;
}
void SpaceNavigatorPollingThread::run()
{
m_stopped = false;
if(spnav_open() == -1)
return;
qreal posfactor = 0.1;
int currX = 0, currY = 0, currZ = 0;
int currRX = 0, currRY = 0, currRZ = 0;
Qt::MouseButtons buttons = Qt::NoButton;
kDebug() << "started spacenavigator polling thread";
while( ! m_stopped )
{
spnav_event event;
if( spnav_poll_event( &event ) )
{
if( event.type == SPNAV_EVENT_MOTION )
{
/*
* The coordinate system of the space navigator is like the following:
* x-axis : from left to right
* y-axis : from down to up
* z-axis : from front to back
* We probably want to make sure that the x- and y-axis match Qt widget
* coordinate system:
* x-axis : from left to right
* y-axis : from back to front
* The z-axis would then go from up to down in a right handed coordinate system.
* z-axis : from up to down
*/
//kDebug() << "got motion event: t("<< event.motion.x << event.motion.y << event.motion.z << ") r(" << event.motion.rx << event.motion.ry << event.motion.rz << ")";
currX = static_cast<int>( posfactor * event.motion.x );
currY = -static_cast<int>( posfactor * event.motion.z );
currZ = -static_cast<int>( posfactor * event.motion.y );
currRX = static_cast<int>( posfactor * event.motion.rx );
currRY = static_cast<int>( -posfactor * event.motion.rz );
currRZ = static_cast<int>( -posfactor * event.motion.ry );
emit moveEvent( currX, currY, currZ, currRX, currRY, currRZ, buttons );
}
else
{
/* SPNAV_EVENT_BUTTON */
Qt::MouseButton button = event.button.bnum == 0 ? Qt::LeftButton : Qt::RightButton;
KoDeviceEvent::Type type;
if( event.button.press )
{
//kDebug() << "got button press event b(" << event.button.bnum << ")";
buttons |= button;
type = KoDeviceEvent::ButtonPressed;
}
else
{
//kDebug() << "got button release event b(" << event.button.bnum << ")";
buttons &= ~button;
type = KoDeviceEvent::ButtonReleased;
}
emit buttonEvent( currX, currY, currZ, currRX, currRY, currRZ, buttons, button, type );
}
spnav_remove_events( event.type );
}
msleep(10);
}
kDebug() << "finished spacenavigator polling thread";
}
