spacenav_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Stuart Glaser
00032  */
00033 
00034 #include <stdio.h>
00035 #include "ros/node_handle.h"
00036 #include "ros/param.h"
00037 #include "spnav.h"
00038 #include "geometry_msgs/Vector3.h"
00039 #include "geometry_msgs/Twist.h"
00040 #include "sensor_msgs/Joy.h"
00041 
00042 #define FULL_SCALE (512.0)
00043 //Used to scale joystick output to be in [-1, 1].  Estimated from data, and not necessarily correct.
00044 
00045 int main(int argc, char **argv)
00046 {
00047   ros::init(argc, argv, "spacenav");
00048 
00049   ros::NodeHandle node_handle;
00050   ros::Publisher offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2);
00051   ros::Publisher rot_offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2);
00052   ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("spacenav/twist", 2);
00053   ros::Publisher joy_pub = node_handle.advertise<sensor_msgs::Joy>("spacenav/joy", 2);
00054 
00055   if (spnav_open() == -1)
00056   {
00057     ROS_ERROR("Could not open the space navigator device.  Did you remember to run spacenavd (as root)?");
00058 
00059     return 1;
00060   }
00061 
00062   // Parameters
00063   // The number of polls needed to be done before the device is considered "static"
00064   int static_count_threshold = 30;
00065   ros::param::get("~/static_count_threshold",static_count_threshold);
00066 
00067   // If true, the node will zero the output when the device is "static"
00068   bool zero_when_static = true;
00069   ros::param::get("~/zero_when_static",zero_when_static);
00070 
00071   // If the device is considered "static" and each trans, rot component is
00072   // below the deadband, it will output zeros in either rotation, translation,
00073   // or both
00074   double static_trans_deadband = 50,
00075          static_rot_deadband = 50;
00076   ros::param::get("~/static_trans_deadband",static_trans_deadband);
00077   ros::param::get("~/static_rot_deadband",static_rot_deadband);
00078 
00079   sensor_msgs::Joy joystick_msg;
00080   joystick_msg.axes.resize(6);
00081   joystick_msg.buttons.resize(2);
00082   
00083   spnav_event sev;
00084   int no_motion_count = 0;
00085   bool motion_stale = false;
00086   geometry_msgs::Vector3 offset_msg;
00087   geometry_msgs::Vector3 rot_offset_msg;
00088   geometry_msgs::Twist twist_msg;
00089   while (node_handle.ok())
00090   {
00091     bool joy_stale = false;
00092     bool queue_empty = false;
00093     
00094     // Sleep when the queue is empty.
00095     // If the queue is empty 30 times in a row output zeros.
00096     // Output changes each time a button event happens, or when a motion
00097     // event happens and the queue is empty.
00098     joystick_msg.header.stamp = ros::Time().now();
00099     switch (spnav_poll_event(&sev))
00100     {
00101       case 0:
00102         queue_empty = true;
00103         if (++no_motion_count > static_count_threshold)
00104         {
00105           if ( zero_when_static || 
00106               ( fabs(offset_msg.x) < static_trans_deadband &&
00107                 fabs(offset_msg.y) < static_trans_deadband &&
00108                 fabs(offset_msg.z) < static_trans_deadband)
00109              )
00110           {
00111             offset_msg.x = offset_msg.y = offset_msg.z = 0;
00112           }
00113 
00114           if ( zero_when_static || 
00115               ( fabs(rot_offset_msg.x) < static_rot_deadband &&
00116                 fabs(rot_offset_msg.y) < static_rot_deadband &&
00117                 fabs(rot_offset_msg.z) < static_rot_deadband )
00118              )
00119           {
00120             rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0;
00121           }
00122 
00123           no_motion_count = 0;
00124           motion_stale = true;
00125         }
00126         break;
00127 
00128       case SPNAV_EVENT_MOTION:
00129         offset_msg.x = sev.motion.z;
00130         offset_msg.y = -sev.motion.x;
00131         offset_msg.z = sev.motion.y;
00132 
00133         rot_offset_msg.x = sev.motion.rz;
00134         rot_offset_msg.y = -sev.motion.rx;
00135         rot_offset_msg.z = sev.motion.ry;
00136 
00137         motion_stale = true;
00138         break;
00139         
00140       case SPNAV_EVENT_BUTTON:
00141         //printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum);
00142         joystick_msg.buttons[sev.button.bnum] = sev.button.press;
00143 
00144         joy_stale = true;
00145         break;
00146 
00147       default:
00148         ROS_WARN("Unknown message type in spacenav. This should never happen.");
00149         break;
00150     }
00151   
00152     if (motion_stale && (queue_empty || joy_stale))
00153     {
00154       offset_pub.publish(offset_msg);
00155       rot_offset_pub.publish(rot_offset_msg);
00156 
00157       twist_msg.linear = offset_msg;
00158       twist_msg.angular = rot_offset_msg;
00159       twist_pub.publish(twist_msg);
00160 
00161       joystick_msg.axes[0] = offset_msg.x / FULL_SCALE;
00162       joystick_msg.axes[1] = offset_msg.y / FULL_SCALE;
00163       joystick_msg.axes[2] = offset_msg.z / FULL_SCALE;
00164       joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE;
00165       joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE;
00166       joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE;
00167 
00168       no_motion_count = 0;
00169       motion_stale = false;
00170       joy_stale = true;
00171     }
00172   
00173     if (joy_stale)
00174     {
00175       joy_pub.publish(joystick_msg);
00176     }
00177 
00178     if (queue_empty) {
00179       usleep(1000);
00180     }
00181   }
00182 
00183   spnav_close();
00184 
00185   return 0;
00186 }


spacenav_node
Author(s): Stuart Glaser, Blaise Gassend
autogenerated on Mon Oct 6 2014 01:06:34