spacenav_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 /*
31  * Author: Stuart Glaser
32  */
33 
34 #include <stdio.h>
35 #include <vector>
36 #include "ros/node_handle.h"
37 #include "ros/param.h"
38 #include "spnav.h"
39 #include "geometry_msgs/Vector3.h"
40 #include "geometry_msgs/Twist.h"
41 #include "sensor_msgs/Joy.h"
42 
50 bool ensureThreeComponents(std::vector<double>& param)
51 {
52  if (param.size() == 0)
53  {
54  param.push_back(1);
55  param.push_back(1);
56  param.push_back(1);
57  return True;
58  }
59  if (param.size() == 3)
60  {
61  return True;
62  }
63  if (param.size() == 1)
64  {
65  param.push_back(param[0]);
66  param.push_back(param[0]);
67  return True;
68  }
69  return False;
70 }
71 
72 int main(int argc, char **argv)
73 {
74  ros::init(argc, argv, "spacenav");
75 
76  ros::NodeHandle node_handle;
77  ros::Publisher offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2);
78  ros::Publisher rot_offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2);
79  ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("spacenav/twist", 2);
80  ros::Publisher joy_pub = node_handle.advertise<sensor_msgs::Joy>("spacenav/joy", 2);
81 
82  // Used to scale joystick output to be in [-1, 1]. Estimated from data, and not necessarily correct.
83  ros::NodeHandle private_nh("~");
84  double full_scale;
85  private_nh.param<double>("full_scale", full_scale, 512);
86  if (full_scale < 1e-10)
87  {
88  full_scale = 512;
89  }
90  // Scale factors for the different axes. End output will be within [-scale, +scale], provided
91  // full_scale normalizes to within [-1, 1].
92  std::vector<double> linear_scale;
93  std::vector<double> angular_scale;
94  private_nh.param<std::vector<double> >("linear_scale", linear_scale, std::vector<double>(3, 1));
95  private_nh.param<std::vector<double> >("angular_scale", angular_scale, std::vector<double>(3, 1));
96 
97  if (!ensureThreeComponents(linear_scale))
98  {
99  ROS_ERROR_STREAM("Parameter " << private_nh.getNamespace() << "/linear_scale must have either one or three components.");
100  exit(EXIT_FAILURE);
101  }
102  if (!ensureThreeComponents(angular_scale))
103  {
104  ROS_ERROR_STREAM("Parameter " << private_nh.getNamespace() << "/angular_scale must have either one or three components.");
105  exit(EXIT_FAILURE);
106  }
107  ROS_DEBUG("linear_scale: %.3f %.3f %.3f", linear_scale[0], linear_scale[1], linear_scale[2]);
108  ROS_DEBUG("angular_scale: %.3f %.3f %.3f", angular_scale[0], angular_scale[1], angular_scale[2]);
109 
110  if (spnav_open() == -1)
111  {
112  ROS_ERROR("Could not open the space navigator device. Did you remember to run spacenavd (as root)?");
113 
114  return 1;
115  }
116 
117  // Parameters
118  // The number of polls needed to be done before the device is considered "static"
119  int static_count_threshold = 30;
120  ros::param::get("~/static_count_threshold",static_count_threshold);
121 
122  // If true, the node will zero the output when the device is "static"
123  bool zero_when_static = true;
124  ros::param::get("~/zero_when_static",zero_when_static);
125 
126  // If the device is considered "static" and each trans, rot normed component
127  // is below the deadband, it will output zeros in either rotation,
128  // translation, or both.
129  double static_trans_deadband = 0.1;
130  double static_rot_deadband = 0.1;
131  ros::param::get("~/static_trans_deadband", static_trans_deadband);
132  ros::param::get("~/static_rot_deadband", static_rot_deadband);
133 
134  sensor_msgs::Joy joystick_msg;
135  joystick_msg.axes.resize(6);
136  joystick_msg.buttons.resize(2);
137 
138  spnav_event sev;
139  int no_motion_count = 0;
140  bool motion_stale = false;
141  double normed_vx = 0;
142  double normed_vy = 0;
143  double normed_vz = 0;
144  double normed_wx = 0;
145  double normed_wy = 0;
146  double normed_wz = 0;
147  while (node_handle.ok())
148  {
149  bool joy_stale = false;
150  bool queue_empty = false;
151 
152  // Sleep when the queue is empty.
153  // If the queue is empty 30 times in a row output zeros.
154  // Output changes each time a button event happens, or when a motion
155  // event happens and the queue is empty.
156  joystick_msg.header.stamp = ros::Time().now();
157  switch (spnav_poll_event(&sev))
158  {
159  case 0:
160  queue_empty = true;
161  if (++no_motion_count > static_count_threshold)
162  {
163  if ( zero_when_static ||
164  ( fabs(normed_vx) < static_trans_deadband &&
165  fabs(normed_vy) < static_trans_deadband &&
166  fabs(normed_vz) < static_trans_deadband)
167  )
168  {
169  normed_vx = normed_vy = normed_vz = 0;
170  }
171 
172  if ( zero_when_static ||
173  ( fabs(normed_wx) < static_rot_deadband &&
174  fabs(normed_wy) < static_rot_deadband &&
175  fabs(normed_wz) < static_rot_deadband )
176  )
177  {
178  normed_wx = normed_wy = normed_wz = 0;
179  }
180 
181  no_motion_count = 0;
182  motion_stale = true;
183  }
184  break;
185 
186  case SPNAV_EVENT_MOTION:
187  normed_vx = sev.motion.z / full_scale;
188  normed_vy = -sev.motion.x / full_scale;
189  normed_vz = sev.motion.y / full_scale;
190 
191  normed_wx = sev.motion.rz / full_scale;
192  normed_wy = -sev.motion.rx / full_scale;
193  normed_wz = sev.motion.ry / full_scale;
194 
195  motion_stale = true;
196  break;
197 
198  case SPNAV_EVENT_BUTTON:
199  //printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum);
200  joystick_msg.buttons[sev.button.bnum] = sev.button.press;
201 
202  joy_stale = true;
203  break;
204 
205  default:
206  ROS_WARN("Unknown message type in spacenav. This should never happen.");
207  break;
208  }
209 
210  if (motion_stale && (queue_empty || joy_stale))
211  {
212  // The offset and rot_offset are scaled.
213  geometry_msgs::Vector3 offset_msg;
214  offset_msg.x = normed_vx * linear_scale[0];
215  offset_msg.y = normed_vy * linear_scale[1];
216  offset_msg.z = normed_vz * linear_scale[2];
217  offset_pub.publish(offset_msg);
218  geometry_msgs::Vector3 rot_offset_msg;
219  rot_offset_msg.x = normed_wx * angular_scale[0];
220  rot_offset_msg.y = normed_wy * angular_scale[1];
221  rot_offset_msg.z = normed_wz * angular_scale[2];
222  rot_offset_pub.publish(rot_offset_msg);
223 
224  geometry_msgs::Twist twist_msg;
225  twist_msg.linear = offset_msg;
226  twist_msg.angular = rot_offset_msg;
227  twist_pub.publish(twist_msg);
228 
229  // The joystick.axes are normalized within [-1, 1].
230  joystick_msg.axes[0] = normed_vx;
231  joystick_msg.axes[1] = normed_vy;
232  joystick_msg.axes[2] = normed_vz;
233  joystick_msg.axes[3] = normed_wx;
234  joystick_msg.axes[4] = normed_wy;
235  joystick_msg.axes[5] = normed_wz;
236 
237  no_motion_count = 0;
238  motion_stale = false;
239  joy_stale = true;
240  }
241 
242  if (joy_stale)
243  {
244  joy_pub.publish(joystick_msg);
245  }
246 
247  if (queue_empty) {
248  usleep(1000);
249  }
250  }
251 
252  spnav_close();
253 
254  return 0;
255 }
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
int main(int argc, char **argv)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static Time now()
bool ok() const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
bool ensureThreeComponents(std::vector< double > &param)
#define ROS_DEBUG(...)


spacenav_node
Author(s): Stuart Glaser, Blaise Gassend
autogenerated on Mon Jun 10 2019 13:42:42