EventHandler::~EventHandler()
{
if( _magellanUsed )
{
#ifdef EQUALIZER_USE_MAGELLAN_GLX
lunchbox::ScopedFastWrite mutex( _magellan );
if( --_magellan->used == 0 )
{
if( spnav_close() == -1 )
{
LBWARN
<< "Couldn't close connection to the space navigator daemon"
<< std::endl;
}
}
#endif
_magellanUsed = false;
}
EventHandlers::iterator i = lunchbox::find( *_eventHandlers, this );
LBASSERT( i != _eventHandlers->end( ));
_eventHandlers->erase( i );
if( _eventHandlers->empty( ))
{
delete _eventHandlers.get();
_eventHandlers = 0;
}
}
Gui::GUIApplicationNativeEventAware::~GUIApplicationNativeEventAware()
{
#ifdef Q_WS_X11
#ifdef SPNAV_FOUND
if (spnav_close())
Base::Console().Log("Couldn't disconnect from spacenav daemon\n");
else
Base::Console().Log("Disconnected from spacenav daemon\n");
#endif
#endif
#ifdef _USE_3DCONNEXION_SDK
#ifdef Q_WS_WIN
if (gMouseInput == this) {
gMouseInput = 0;
}
#endif
//mac
#ifdef Q_WS_MACX
/* make sure the framework is installed */
if (InstallConnexionHandlers == NULL)
{
Base::Console().Log("3Dconnexion framework not found!\n");
return;
}
/* close the connection with the 3dx driver */
//std::cerr << "tdxClientID: " << tdxClientID << std::endl;
if (tdxClientID)
UnregisterConnexionClient(tdxClientID);
CleanupConnexionHandlers();
Base::Console().Log("Disconnected from 3DConnexion driver\n");
#endif
#endif
}
Gui::GUIApplicationNativeEventAware::~GUIApplicationNativeEventAware()
{
#ifdef Q_WS_X11
#ifdef SPNAV_FOUND
if (spnav_close())
Base::Console().Log("Couldn't disconnect from spacenav daemon\n");
else
Base::Console().Log("Disconnected from spacenav daemon\n");
#endif
#endif
#ifdef _USE_3DCONNEXION_SDK
#ifdef Q_WS_WIN
if (gMouseInput == this) {
gMouseInput = 0;
Base::Console().Log("3Dconnexion device detached.\n");
}
#endif
//mac
#ifdef Q_WS_MACX
// if 3Dconnexion library was loaded at runtime
if (InstallConnexionHandlers) {
// Close our connection with the 3dx driver
if (tdxClientID)
UnregisterConnexionClient(tdxClientID);
CleanupConnexionHandlers();
Base::Console().Log("Disconnected from 3Dconnexion driver\n");
}
#endif
#endif
}
int main(void)
{
#if defined(BUILD_X11)
Display *dpy;
Window win;
unsigned long bpix;
#endif
spnav_event sev;
signal(SIGINT, sig);
#if defined(BUILD_X11)
if(!(dpy = XOpenDisplay(0))) {
fprintf(stderr, "failed to connect to the X server\n");
return 1;
}
bpix = BlackPixel(dpy, DefaultScreen(dpy));
win = XCreateSimpleWindow(dpy, DefaultRootWindow(dpy), 0, 0, 1, 1, 0, bpix, bpix);
/* This actually registers our window with the driver for receiving
* motion/button events through the 3dxsrv-compatible X11 protocol.
*/
if(spnav_x11_open(dpy, win) == -1) {
fprintf(stderr, "failed to connect to the space navigator daemon\n");
return 1;
}
#elif defined(BUILD_AF_UNIX)
if(spnav_open()==-1) {
fprintf(stderr, "failed to connect to the space navigator daemon\n");
return 1;
}
#else
#error Unknown build type!
#endif
/* spnav_wait_event() and spnav_poll_event(), will silently ignore any non-spnav X11 events.
*
* If you need to handle other X11 events you will have to use a regular XNextEvent() loop,
* and pass any ClientMessage events to spnav_x11_event, which will return the event type or
* zero if it's not an spnav event (see spnav.h).
*/
while(spnav_wait_event(&sev)) {
if(sev.type == SPNAV_EVENT_MOTION) {
printf("got motion event: t(%d, %d, %d) ", sev.motion.x, sev.motion.y, sev.motion.z);
printf("r(%d, %d, %d)\n", sev.motion.rx, sev.motion.ry, sev.motion.rz);
} else { /* SPNAV_EVENT_BUTTON */
printf("got button %s event b(%d)\n", sev.button.press ? "press" : "release", sev.button.bnum);
}
}
spnav_close();
return 0;
}
bool SpaceNavigatorDevice::stop()
{
if( ! m_thread->isRunning() )
return true;
m_thread->stop();
if( ! m_thread->wait( 500 ) )
m_thread->terminate();
spnav_close();
return true;
}
Gui::GUIApplicationNativeEventAware::~GUIApplicationNativeEventAware()
{
#ifdef SPNAV_FOUND
if (spnav_close())
Base::Console().Log("Couldn't disconnect from spacenav daemon\n");
else
Base::Console().Log("Disconnected from spacenav daemon\n");
#endif
#ifdef _USE_3DCONNEXION_SDK
if (gMouseInput == this) {
gMouseInput = 0;
}
#endif
}
bool SpaceNavigatorDevice::stop()
{
kDebug() << "stopping spacenavigator device...";
if( ! m_thread->isRunning() )
return true;
m_thread->stop();
if( ! m_thread->wait( 500 ) )
m_thread->terminate();
spnav_close();
return true;
}
EventHandler::~EventHandler()
{
if( _magellanUsed )
{
#ifdef EQUALIZER_USE_MAGELLAN_GLX
lunchbox::ScopedFastWrite mutex( _magellan );
if( --_magellan->used == 0 )
{
if( spnav_close() == -1 )
{
LBWARN
<< "Couldn't close connection to the space navigator daemon"
<< std::endl;
}
}
#endif
_magellanUsed = false;
}
eq::Pipe* pipe = _window->getPipe();
MessagePump* messagePump =
dynamic_cast<MessagePump*>( pipe->isThreaded() ?
pipe->getMessagePump() :
pipe->getConfig()->getMessagePump( ));
if( messagePump )
{
Display* display = _window->getXDisplay();
LBASSERT( display );
messagePump->deregister( display );
}
EventHandlers::iterator i = stde::find( *_eventHandlers, this );
LBASSERT( i != _eventHandlers->end( ));
_eventHandlers->erase( i );
if( _eventHandlers->empty( ))
{
delete _eventHandlers.get();
_eventHandlers = 0;
}
}
void fgSpaceballClose(void)
{
#if TARGET_HOST_POSIX_X11
spnav_close();
#endif
}
GHOST_NDOFManagerUnix::~GHOST_NDOFManagerUnix()
{
if (m_available)
spnav_close();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "spacenav");
ros::NodeHandle node_handle;
ros::Publisher offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2);
ros::Publisher rot_offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2);
ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("spacenav/twist", 2);
ros::Publisher joy_pub = node_handle.advertise<sensor_msgs::Joy>("spacenav/joy", 2);
if (spnav_open() == -1)
{
ROS_ERROR("Could not open the space navigator device. Did you remember to run spacenavd (as root)?");
return 1;
}
// Parameters
// The number of polls needed to be done before the device is considered "static"
int static_count_threshold = 30;
ros::param::get("~/static_count_threshold",static_count_threshold);
// If true, the node will zero the output when the device is "static"
bool zero_when_static = true;
ros::param::get("~/zero_when_static",zero_when_static);
// If the device is considered "static" and each trans, rot component is
// below the deadband, it will output zeros in either rotation, translation,
// or both
double static_trans_deadband = 50,
static_rot_deadband = 50;
ros::param::get("~/static_trans_deadband",static_trans_deadband);
ros::param::get("~/static_rot_deadband",static_rot_deadband);
sensor_msgs::Joy joystick_msg;
joystick_msg.axes.resize(6);
joystick_msg.buttons.resize(2);
spnav_event sev;
int no_motion_count = 0;
bool motion_stale = false;
geometry_msgs::Vector3 offset_msg;
geometry_msgs::Vector3 rot_offset_msg;
geometry_msgs::Twist twist_msg;
while (node_handle.ok())
{
bool joy_stale = false;
bool queue_empty = false;
// Sleep when the queue is empty.
// If the queue is empty 30 times in a row output zeros.
// Output changes each time a button event happens, or when a motion
// event happens and the queue is empty.
joystick_msg.header.stamp = ros::Time().now();
switch (spnav_poll_event(&sev))
{
case 0:
queue_empty = true;
if (++no_motion_count > static_count_threshold)
{
if ( zero_when_static ||
( fabs(offset_msg.x) < static_trans_deadband &&
fabs(offset_msg.y) < static_trans_deadband &&
fabs(offset_msg.z) < static_trans_deadband)
)
{
offset_msg.x = offset_msg.y = offset_msg.z = 0;
}
if ( zero_when_static ||
( fabs(rot_offset_msg.x) < static_rot_deadband &&
fabs(rot_offset_msg.y) < static_rot_deadband &&
fabs(rot_offset_msg.z) < static_rot_deadband )
)
{
rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0;
}
no_motion_count = 0;
motion_stale = true;
}
break;
case SPNAV_EVENT_MOTION:
offset_msg.x = sev.motion.z;
offset_msg.y = -sev.motion.x;
offset_msg.z = sev.motion.y;
rot_offset_msg.x = sev.motion.rz;
rot_offset_msg.y = -sev.motion.rx;
rot_offset_msg.z = sev.motion.ry;
motion_stale = true;
break;
case 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;
joy_stale = true;
break;
default:
ROS_WARN("Unknown message type in spacenav. This should never happen.");
break;
}
if (motion_stale && (queue_empty || joy_stale))
{
offset_pub.publish(offset_msg);
rot_offset_pub.publish(rot_offset_msg);
twist_msg.linear = offset_msg;
twist_msg.angular = rot_offset_msg;
twist_pub.publish(twist_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;
motion_stale = false;
joy_stale = true;
}
if (joy_stale)
{
joy_pub.publish(joystick_msg);
}
if (queue_empty) {
usleep(1000);
}
}
spnav_close();
return 0;
}
void fgPlatformSpaceballClose(void)
{
spnav_close();
}
GHOST_NDOFManagerX11::~GHOST_NDOFManagerX11()
{
if (m_available)
spnav_close();
}
void sig(int s)
{
spnav_close();
exit(0);
}
