Commit d6117b98 authored by CactusSoyeux's avatar CactusSoyeux
Browse files

change_node debug and add of joy

parent be90810b
^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package joy
^^^^^^^^^^^^^^^^^^^^^^^^^
1.15.1 (2021-06-07)
-------------------
1.15.0 (2020-10-12)
-------------------
* Added autodetection for force-feedback devices. (`#169 <https://github.com/ros-drivers/joystick_drivers/issues/169>`_)
* Added autodetection for force-feedback devices.
* RAII for closedir
* joy: Little fixes for force feedback. (`#167 <https://github.com/ros-drivers/joystick_drivers/issues/167>`_)
This commit increases the maximum magnitude of the FF effects to double the previous maximum.
* Print out joystick name on initialization. (`#168 <https://github.com/ros-drivers/joystick_drivers/issues/168>`_)
This helps figuring out what string to give to the `dev_name` parameter.
* Contributors: Martin Pecka
1.14.0 (2020-07-07)
-------------------
* frame_id in the header of the joystick msg (`#166 <https://github.com/ros-drivers/joystick_drivers/issues/166>`_)
* roslint and Generic Clean-Up (`#161 <https://github.com/ros-drivers/joystick_drivers/issues/161>`_)
* Merge pull request `#158 <https://github.com/ros-drivers/joystick_drivers/issues/158>`_ from clalancette/ros1-cleanups
ROS1 joy cleanups
* Greatly simplify the sticky_buttons support.
* Small fixes to rumble support.
* Use C++ style casts.
* Use empty instead of length.
* joy_def_ff -> joy_dev_ff
* Cleanup header includes.
* Use size_t appropriately.
* NULL -> nullptr everywhere.
* Style cleanup in joy_node.cpp.
* Merge pull request `#154 <https://github.com/ros-drivers/joystick_drivers/issues/154>`_ from zchen24/master
Minor: moved default to right indent level
* Contributors: Chris Lalancette, Joshua Whitley, Mamoun Gharbi, Zihan Chen
1.13.0 (2019-06-24)
-------------------
* Merge pull request `#120 <https://github.com/ros-drivers/joystick_drivers/issues/120>`_ from furushchev/remap
add joy_remap and its sample
* Merge pull request `#128 <https://github.com/ros-drivers/joystick_drivers/issues/128>`_ from ros-drivers/fix/tab_errors
Cleaning up Python indentation.
* Merge pull request `#111 <https://github.com/ros-drivers/joystick_drivers/issues/111>`_ from matt-attack/indigo-devel
Add Basic Force Feedback Support
* Merge pull request `#126 <https://github.com/ros-drivers/joystick_drivers/issues/126>`_ from clalancette/minor-formatting
* Put brackets around ROS\_* macros.
In some circumstances they may be defined to empty, so we need
to have brackets to ensure that they are syntactically valid.
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
* Merge pull request `#122 <https://github.com/ros-drivers/joystick_drivers/issues/122>`_ from lbucklandAS/fix-publish-timestamp
Add timestamp to all joy messages
* Change error messages and set ps3 as default controller
* Better handle device loss
Allow for loss and redetection of device with force feedback
* Add basic force feedback over usb
Addresses `#89 <https://github.com/ros-drivers/joystick_drivers/issues/89>`_
* Contributors: Chris Lalancette, Furushchev, Joshua Whitley, Lucas Buckland, Matthew, Matthew Bries
1.12.0 (2018-06-11)
-------------------
* Update timestamp when using autorepeat_rate
* Added dev_name parameter to select joystick by name
* Added Readme for joy package with description of new device name parameter
* Fixed numerous outstanding PRs.
* Added sticky buttons
* Changed package xml to format 2
* Fixed issue when the joystick data did not got send until changed.
* Changed messaging to better reflect what the script is doing
* Contributors: Dino Hüllmann, Jonathan Bohren, Joshua Whitley, Miklos Marton, Naoki Mizuno, jprod123, psimona
1.11.0 (2017-02-10)
-------------------
* fixed joy/Cmakelists for osx
* Update dependencies to remove warnings
* Contributors: Marynel Vazquez, Mark D Horn
1.10.1 (2015-05-24)
-------------------
* Remove stray architechture_independent flags
* Contributors: Jonathan Bohren, Scott K Logan
1.10.0 (2014-06-26)
-------------------
* First indigo release
cmake_minimum_required(VERSION 3.5)
project(joy)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Load catkin and all dependencies required for this package
set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint)
find_package(catkin REQUIRED ${CATKIN_DEPS})
roslint_cpp()
catkin_package(DEPENDS sensor_msgs)
# Look for <linux/joystick.h>
include(CheckIncludeFiles)
check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H)
if(CATKIN_ENABLE_TESTING)
roslint_add_test()
endif()
if(HAVE_LINUX_JOYSTICK_H)
include_directories(msg/cpp ${catkin_INCLUDE_DIRS})
add_executable(joy_node src/joy_node.cpp)
target_link_libraries(joy_node ${catkin_LIBRARIES})
# Install targets
install(TARGETS joy_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
else()
message("Warning: no <linux/joystick.h>; won't build joy node")
endif()
install(DIRECTORY migration_rules scripts config launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS)
# ROS Driver for Generic Linux Joysticks
The joy package contains joy_node, a node that interfaces a generic Linux joystick to ROS. This node publishes a "Joy" message, which contains the current state of each one of the joystick's buttons and axes.
## Supported Hardware
This node should work with any joystick that is supported by Linux.
## Published Topics
* joy ([sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html)): outputs the joystick state.
## Device Selection
There are two parameters controlling which device should be used:
* ~dev (string, default: "/dev/input/js0")
* ~dev_name (string, default: "" (empty = disabled))
If ~dev_name is empty, ~dev defines the Linux joystick device from which to read joystick events.
If ~dev_name is defined, the node enumerates all available joysticks, i.e. /dev/input/js*. The first joystick matching ~dev_name is opened. If no joystick matches the desired device name, the device specified by ~dev is used as a fallback.
To get a list of the names of all connected joysticks, an invalid ~dev_name can be specified. For example:
`rosrun joy joy_node _dev_name:="*"`
The output might look like this:
```
[ INFO]: Found joystick: ST LIS3LV02DL Accelerometer (/dev/input/js1).
[ INFO]: Found joystick: Microsoft X-Box 360 pad (/dev/input/js0).
[ERROR]: Couldn't find a joystick with name *. Falling back to default device.
```
Then the node can be started with the device name given in the list. For example:
`rosrun joy joy_node _dev_name:="Microsoft X-Box 360 pad"`
## Advanced Parameters
* ~deadzone (double, default: 0.05)
* Amount by which the joystick has to move before it is considered to be off-center. This parameter is specified relative to an axis normalized between -1 and 1. Thus, 0.1 means that the joystick has to move 10% of the way to the edge of an axis's range before that axis will output a non-zero value. Linux does its own deadzone processing, so in many cases this value can be set to zero.
* ~autorepeat_rate (double, default: 0.0 (disabled))
* Rate in Hz at which a joystick that has a non-changing state will resend the previously sent message.
* ~coalesce_interval (double, default: 0.001)
* Axis events that are received within coalesce_interval (seconds) of each other are sent out in a single ROS message. Since the kernel sends each axis motion as a separate event, coalescing greatly reduces the rate at which messages are sent. This option can also be used to limit the rate of outgoing messages. Button events are always sent out immediately to avoid missing button presses.
## Further Information
For further information have a look at the [Wiki page](http://wiki.ros.org/joy).
\ No newline at end of file
mappings:
axes:
- axes[0]
- axes[1]
- axes[2]
- axes[5]
- axes[6] * 2.0
- axes[7] * 2.0
- axes[8] * -2.0
- 0.0
- max(axes[10], 0.0) * -1.0
- min(axes[9], 0.0)
- min(axes[10], 0.0)
- max(axes[9], 0.0) * -1.0
- (axes[3] - 1.0) * 0.5
- (axes[4] - 1.0) * 0.5
- buttons[4] * -1.0
- buttons[5] * -1.0
- buttons[3] * -1.0
- buttons[2] * -1.0
- buttons[1] * -1.0
- buttons[0] * -1.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
buttons:
- buttons[8]
- buttons[10]
- buttons[11]
- buttons[9]
- max(axes[10], 0.0)
- min(axes[9], 0.0) * -1.0
- min(axes[10], 0.0) * -1.0
- max(axes[9], 0.0)
- buttons[6]
- buttons[7]
- buttons[4]
- buttons[5]
- buttons[3]
- buttons[2]
- buttons[1]
- buttons[0]
- buttons[12]
<launch>
<!-- assumes ds4drv is running -->
<node name="joy_node" pkg="joy" type="joy_node">
<remap from="joy" to="joy_orig"/>
</node>
<!-- remap joy to emulate ps3joy mappings -->
<node name="joy_remap" pkg="joy" type="joy_remap.py">
<remap from="joy_in" to="joy_orig"/>
<remap from="joy_out" to="joy"/>
<rosparam command="load" file="$(find joy)/config/ps4joy.yaml"/>
</node>
</launch>
/**
\mainpage
\htmlinclude manifest.html
\b joy ROS joystick drivers for Linux. This package works with Logitech joysticks and PS3 SIXAXIS/DUALSHOCK devices. This package will only build is linux/joystick.h
\section rosapi ROS API
This package contains the message "Joy", which carries data from the joystick's axes and buttons. It also has the joy_node, which reads from a given joystick port and publishes "Joy" messages.
List of nodes:
- \b joy_node
Deprecated nodes:
- \b txjoy
- \b joy
The deprecated nodes are the same as the joy_node, but they will warn users when used. They are actually just bash scripts that call "joy_node" and print a warning.
<hr>
\subsection joy joy
\b joy ROS joystick driver for all linux joysticks. The driver will poll a given port until it can read from it, the publish Joy messages of the joystick state. If the port closes, or it reads an error, it will reopen the port. All axes are in the range [-1, 1], and all buttons are 0 (off) or 1 (on).
Since the driver will poll for the joystick port, and automatically reopen the port if it's closed, the joy_node should be "on" whenever possible. It is typically part of the robot's launch file.
\subsubsection autorepeat Auto-Repeat/Signal Loss
The joy_node takes an "~autorepeat_rate" parameter. If the linux kernal receives no events during the autorepeat interval, it will automatically repeat the last value of the joystick. This is an important safety feature, and allows users to recover from a joystick that has timed out.
\subsubsection usage Usage
\verbatim
$ joy [standard ROS args]
\endverbatim
\subsubsection topic ROS topics
Subscribes to (name / type):
- None
Publishes to (name / type):
- \b "joy/Joy" : Joystick output. Axes are [-1, 1], buttons are 0 or 1 (depressed).
\subsubsection parameters ROS parameters
- \b "~dev" : Input device for joystick. Default: /dev/input/js0
- \b "~deadzone" : Output is zero for axis in deadzone. Range: [-0.9, 0.9]. Default 0.05
- \b "~autorepeat_rate" : If no events, repeats last known state at this rate. Defualt: 0 (disabled)
- \b "~coalesce_interval" : Waits for this interval (seconds) after receiving an event. If multiple events happen in this interval, only one message will be sent. Reduces number of messages. Default: 0.001.
<hr>
*/
class update_joy_Joy_e3ef016fcdf22397038b36036c66f7c8(MessageUpdateRule):
old_type = "joy/Joy"
old_full_text = """
float32[] axes
int32[] buttons
"""
new_type = "sensor_msgs/Joy"
new_full_text = """
# Reports the state of a joysticks axes and buttons.
Header header # timestamp in the header is the time the data is received from the joystick
float32[] axes # the axes measurements from a joystick
int32[] buttons # the buttons measurements from a joystick
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
"""
order = 0
migrated_types = []
valid = True
def update(self, old_msg, new_msg):
#No matching field name in old message
new_msg.header = self.get_new_class('Header')()
new_msg.axes = old_msg.axes
new_msg.buttons = old_msg.buttons
<package format="2">
<name>joy</name>
<version>1.15.1</version>
<license>BSD</license>
<description>
ROS driver for a generic Linux joystick.
The joy package contains joy_node, a node that interfaces a
generic Linux joystick to ROS. This node publishes a "Joy"
message, which contains the current state of each one of the
joystick's buttons and axes.
</description>
<maintainer email="jbo@jhu.edu">Jonathan Bohren</maintainer>
<author>Morgan Quigley</author>
<author>Brian Gerkey</author>
<author>Kevin Watts</author>
<author>Blaise Gassend</author>
<url type="website">http://www.ros.org/wiki/joy</url>
<url type="development">https://github.com/ros-drivers/joystick_drivers</url>
<url type="bugtracker">https://github.com/ros-drivers/joystick_drivers/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslint</build_depend>
<depend>diagnostic_updater</depend>
<depend>joystick</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<test_depend>rosbag</test_depend>
<export>
<rosbag migration_rule_file="migration_rules/Joy.bmr"/>
</export>
</package>
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Author: furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
import ast
import operator as op
import rospy
import traceback
from sensor_msgs.msg import Joy
class RestrictedEvaluator(object):
def __init__(self):
self.operators = {
ast.Add: op.add,
ast.Sub: op.sub,
ast.Mult: op.mul,
ast.Div: op.truediv,
ast.BitXor: op.xor,
ast.USub: op.neg,
}
self.functions = {
'abs': lambda x: abs(x),
'max': lambda *x: max(*x),
'min': lambda *x: min(*x),
}
def _reval_impl(self, node, variables):
if isinstance(node, ast.Num):
return node.n
elif isinstance(node, ast.BinOp):
op = self.operators[type(node.op)]
return op(self._reval_impl(node.left, variables),
self._reval_impl(node.right, variables))
elif isinstance(node, ast.UnaryOp):
op = self.operators[type(node.op)]
return op(self._reval_impl(node.operand, variables))
elif isinstance(node, ast.Call) and node.func.id in self.functions:
func = self.functions[node.func.id]
args = [self._reval_impl(n, variables) for n in node.args]
return func(*args)
elif isinstance(node, ast.Name) and node.id in variables:
return variables[node.id]
elif isinstance(node, ast.Subscript) and node.value.id in variables:
var = variables[node.value.id]
idx = node.slice.value.n
try:
return var[idx]
except IndexError:
raise IndexError("Variable '%s' out of range: %d >= %d" % (node.value.id, idx, len(var)))
else:
raise TypeError("Unsupported operation: %s" % node)
def reval(self, expr, variables):
expr = str(expr)
if len(expr) > 1000:
raise ValueError("The length of an expression must not be more than 1000 characters")
try:
return self._reval_impl(ast.parse(expr, mode='eval').body, variables)
except Exception as e:
rospy.logerr(traceback.format_exc())
raise e
class JoyRemap(object):
def __init__(self):
self.evaluator = RestrictedEvaluator()
self.mappings = self.load_mappings("~mappings")
self.warn_remap("joy_out")
self.pub = rospy.Publisher(
"joy_out", Joy, queue_size=1)
self.warn_remap("joy_in")
self.sub = rospy.Subscriber(
"joy_in", Joy, self.callback,
queue_size=rospy.get_param("~queue_size", None))
def load_mappings(self, ns):
btn_remap = rospy.get_param(ns + "/buttons", [])
axes_remap = rospy.get_param(ns + "/axes", [])
rospy.loginfo("loaded remapping: %d buttons, %d axes" % (len(btn_remap), len(axes_remap)))
return {"buttons": btn_remap, "axes": axes_remap}
def warn_remap(self, name):
if name == rospy.remap_name(name):
rospy.logwarn("topic '%s' is not remapped" % name)
def callback(self, in_msg):
out_msg = Joy(header=in_msg.header)
map_axes = self.mappings["axes"]
map_btns = self.mappings["buttons"]
out_msg.axes = [0.0] * len(map_axes)
out_msg.buttons = [0] * len(map_btns)
in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons}
for i, exp in enumerate(map_axes):
try:
out_msg.axes[i] = self.evaluator.reval(exp, in_dic)
except NameError as e:
rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e)
except UnboundLocalError as e:
rospy.logerr("Wrong form: %s" % e)
except Exception as e:
raise e
for i, exp in enumerate(map_btns):
try:
if self.evaluator.reval(exp, in_dic) > 0:
out_msg.buttons[i] = 1
except NameError as e:
rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e)
except UnboundLocalError as e:
rospy.logerr("Wrong form: %s" % e)
except Exception as e:
raise e
self.pub.publish(out_msg)
if __name__ == '__main__':
rospy.init_node("joy_remap")
n = JoyRemap()
rospy.spin()
This diff is collapsed.
[joy/Joy]:
float32[] axes
int32[] buttons
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
import roslib
roslib.load_manifest('joy')
import sys
import struct
import unittest
import rostest
import rosbag
import rosbagmigration
import re
from cStringIO import StringIO
import os
import rospy
migrator = rosbagmigration.MessageMigrator()
def repack(x):
return struct.unpack('<f',struct.pack('<f',x))[0]
class TestJoyMsgsMigration(unittest.TestCase):
# (*) Joy.saved
########### Joy ###############
def get_old_joy(self):
joy_classes = self.load_saved_classes('Joy.saved')
joy = joy_classes['joy/Joy']
return joy([0.1,0.2,0.3,0.4,0.5],[0,1,0,1,0])
def get_new_joy(self):
from sensor_msgs.msg import Joy
from roslib.msg import Header
return Joy(Header(),[0.1,0.2,0.3,0.4,0.5],[0,1,0,1,0])
def test_joy(self):
self.do_test('joy', self.get_old_joy, self.get_new_joy)
########### Helper functions ###########
def setUp(self):
self.pkg_dir = roslib.packages.get_pkg_dir("joy")
def load_saved_classes(self,saved_msg):
f = open("%s/test/saved/%s"%(self.pkg_dir,saved_msg), 'r')
type_line = f.readline()
pat = re.compile(r"\[(.*)]:")
type_match = pat.match(type_line)
self.assertTrue(type_match is not None, "Full definition file malformed. First line should be: '[my_package/my_msg]:'")
saved_type = type_match.groups()[0]
saved_full_text = f.read()
saved_classes = roslib.genpy.generate_dynamic(saved_type,saved_full_text)
self.assertTrue(saved_classes is not None, "Could not generate class from full definition file.")
self.assertTrue(saved_classes.has_key(saved_type), "Could not generate class from full definition file.")
return saved_classes
def do_test(self, name, old_msg, new_msg):
# Name the bags
oldbag = "%s/test/%s_old.bag"%(self.pkg_dir,name)
newbag = "%s/test/%s_new.bag"%(self.pkg_dir,name)
# Create an old message
bag = rosbag.Bag(oldbag, 'w')
bag.write("topic", old_msg(), roslib.rostime.Time())
bag.close()
# Check and migrate
res = rosbagmigration.checkbag(migrator, oldbag)