GHOST_NDOFManagerUnix::GHOST_NDOFManagerUnix(GHOST_System& sys)
: GHOST_NDOFManager(sys),
m_available(false)
{
if (spnav_open() != -1) {
m_available = true;
/* determine exactly which device (if any) is plugged in */
#define MAX_LINE_LENGTH 100
/* look for USB devices with Logitech or 3Dconnexion's vendor ID */
FILE *command_output = popen("lsusb | grep '046d:\\|256f:'", "r");
if (command_output) {
char line[MAX_LINE_LENGTH] = {0};
while (fgets(line, MAX_LINE_LENGTH, command_output)) {
unsigned short vendor_id = 0, product_id = 0;
if (sscanf(line, "Bus %*d Device %*d: ID %hx:%hx", &vendor_id, &product_id) == 2)
if (setDevice(vendor_id, product_id)) {
break; /* stop looking once the first 3D mouse is found */
}
}
pclose(command_output);
}
}
else {
#ifdef DEBUG
/* annoying for official builds, just adds noise and most people don't own these */
puts("ndof: spacenavd not found");
/* This isn't a hard error, just means the user doesn't have a 3D mouse. */
#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;
}
events()
{
static k3d::bool_t initialized = false;
if(!initialized)
{
initialized = true;
if(-1 == spnav_open())
{
k3d::log() << error << "Error opening connection to spnav daemon." << std::endl;
return;
}
if(!Glib::thread_supported())
Glib::thread_init();
g_dispatcher.reset(new Glib::Dispatcher());
g_dispatcher->connect(sigc::ptr_fun(handle_event));
g_mutex.reset(new Glib::Mutex());
Glib::Thread::create(sigc::ptr_fun(&event_loop), false);
}
}
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;
}
QSpaceNavigator::QSpaceNavigator() {
if(spnav_open() != -1) {
spaceSock = new QSocketNotifier(spnav_fd(),QSocketNotifier::Read, this);
connect(spaceSock, SIGNAL(activated(int)), this, SLOT(poll()));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ardrone3dnav");
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>("cmd_vel", 2);
// ros::Publisher joy_pub = node_handle.advertise<joy::Joy>("spacenav/joy",
// 2);
ros::Publisher land_pub = node_handle.advertise<std_msgs::Empty>("ardrone/land", 2);
ros::Publisher takeoff_pub = node_handle.advertise<std_msgs::Empty>("ardrone/takeoff", 2);
ros::Publisher reset_pub = node_handle.advertise<std_msgs::Empty>("ardrone/reset", 2);
if (spnav_open() == -1)
{
ROS_ERROR("Could not open the space navigator device. Did you remember to "
"run spacenavd (as root)?");
return 1;
}
else
ROS_INFO("Properly connected to the space navigator device.");
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;
std_msgs::Empty empty_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.
switch (spnav_poll_event(&sev))
{
case 0:
queue_empty = true;
if (++no_motion_count > 30)
{
offset_msg.x = offset_msg.y = offset_msg.z = 0;
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 / FULL_SCALE;
offset_msg.y = -sev.motion.x / FULL_SCALE;
offset_msg.z = sev.motion.y / FULL_SCALE;
rot_offset_msg.x = sev.motion.rz / FULL_SCALE;
rot_offset_msg.y = -sev.motion.rx / FULL_SCALE;
rot_offset_msg.z = sev.motion.ry / FULL_SCALE;
// printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y,
// rot_offset_msg.z);
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;
if (sev.button.press == 1)
{
switch (sev.button.bnum)
{
case 0:
takeoff_pub.publish(empty_msg);
ROS_INFO("The drone is taking off!");
break;
case 1:
land_pub.publish(empty_msg);
ROS_INFO("The drone is landing");
break;
case 6:
reset_pub.publish(empty_msg);
break;
}
}
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);
}
return 0;
}
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 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";
}
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;
}
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 > 30)
{
offset_msg.x = offset_msg.y = offset_msg.z = 0;
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;
//printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y, rot_offset_msg.z);
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);
}
return 0;
}
