From 67dfe89c8fa160abc22527fb1398ddfa9ce3136c Mon Sep 17 00:00:00 2001 From: X-lanni Date: Fri, 6 Jun 2025 16:15:33 +0800 Subject: [PATCH] add teleop_twist_keyboard --- teleop_twist_keyboard/CHANGELOG.rst | 66 +++++ teleop_twist_keyboard/CONTRIBUTING.md | 3 + teleop_twist_keyboard/LICENSE | 30 +++ teleop_twist_keyboard/README.md | 52 ++++ teleop_twist_keyboard/package.xml | 27 ++ .../resource/teleop_twist_keyboard | 0 teleop_twist_keyboard/setup.cfg | 4 + teleop_twist_keyboard/setup.py | 38 +++ .../teleop_twist_keyboard.py | 231 ++++++++++++++++++ teleop_twist_keyboard/test/test_copyright.py | 20 ++ teleop_twist_keyboard/test/test_flake8.py | 20 ++ teleop_twist_keyboard/test/test_pep257.py | 20 ++ 12 files changed, 511 insertions(+) create mode 100644 teleop_twist_keyboard/CHANGELOG.rst create mode 100644 teleop_twist_keyboard/CONTRIBUTING.md create mode 100644 teleop_twist_keyboard/LICENSE create mode 100644 teleop_twist_keyboard/README.md create mode 100644 teleop_twist_keyboard/package.xml create mode 100644 teleop_twist_keyboard/resource/teleop_twist_keyboard create mode 100644 teleop_twist_keyboard/setup.cfg create mode 100644 teleop_twist_keyboard/setup.py create mode 100755 teleop_twist_keyboard/teleop_twist_keyboard.py create mode 100644 teleop_twist_keyboard/test/test_copyright.py create mode 100644 teleop_twist_keyboard/test/test_flake8.py create mode 100644 teleop_twist_keyboard/test/test_pep257.py diff --git a/teleop_twist_keyboard/CHANGELOG.rst b/teleop_twist_keyboard/CHANGELOG.rst new file mode 100644 index 0000000..5513f01 --- /dev/null +++ b/teleop_twist_keyboard/CHANGELOG.rst @@ -0,0 +1,66 @@ +2.4.0 (2024-03-08) +------------------ +* Fixes for flake8. (`#29 `_) +* Show command to change topic name on readme (`#27 `_) +* Remove url for ros1 on package.xml (`#28 `_) +* Added TwistStamped option (`#26 `_) +* Switch to underscores for setup.cfg. (`#25 `_) +* Contributors: Asuki Kono, Chris Lalancette, agyoungs + +2.3.2 (2020-05-12) +------------------ +* Add Windows support to teleop_twist_keyboard. (`#24 `_) +* Contributors: Chris Lalancette + +2.3.1 (2019-10-03) +------------------ +* Stop using deprecated APIs. (`#22 `_) +* Contributors: Chris Lalancette + +2.3.0 (2019-04-19) +------------------ +* changing QoS to default (`#18 `_) +* Contributors: thorstink + +2.1.1 (2018-06-26) +------------------ + +2.1.0 (2018-06-26) +------------------ +* set zip_safe to avoid warning during installation (`#14 `_) +* Update the maintainer to me. (`#13 `_) +* fix data files install path (`#12 `_) +* remove test_suite, add pytest as test_requires (`#11 `_) +* use ros2 run to launch whichever the installation used (`#8 `_) +* Fix teleop_twist_keyboard to have a setup.cfg. (`#9 `_) +* Make sure to add teleop_twist_keyboard to ament index. (`#6 `_) +* use OSI website as reference for license (`#2 `_) +* ROS 2 port (`#1 `_) +* Contributors: Adam Allevato, Austin, Chris Lalancette, Dirk Thomas, Mikael Arguedas + +0.6.1 (2018-05-02) +------------------ +* Merge pull request `#11 `_ from MatthijsBurgh/patch-1 + Correct exception handling; Python3 print compatible +* import print from future +* Print python3 compatible +* correct Exception handling +* Merge pull request `#7 `_ from lucasw/speed_params + set linear and turn speed via rosparams +* Using tabs instead of spaces to match rest of file +* set linear and turn speed via rosparams +* Contributors: Austin, Lucas Walter, Matthijs van der Burgh + +0.6.0 (2016-03-21) +------------------ +* Better instruction formatting +* support holonomic base, send commmand with keyboard down +* Fixing queue_size warning +* Create README.md +* Update the description string in package.xml +* Contributors: Austin, Kei Okada, LiohAu, Mike Purvis, kk6axq, trainman419 + +0.5.0 (2014-02-11) +------------------ +* Initial import from brown_remotelab +* Convert to catkin diff --git a/teleop_twist_keyboard/CONTRIBUTING.md b/teleop_twist_keyboard/CONTRIBUTING.md new file mode 100644 index 0000000..5e190bd --- /dev/null +++ b/teleop_twist_keyboard/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the BSD license 2.0, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/teleop_twist_keyboard/LICENSE b/teleop_twist_keyboard/LICENSE new file mode 100644 index 0000000..24d43da --- /dev/null +++ b/teleop_twist_keyboard/LICENSE @@ -0,0 +1,30 @@ +All rights reserved. + +Software License Agreement (BSD License 2.0) + +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 the copyright holder 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. diff --git a/teleop_twist_keyboard/README.md b/teleop_twist_keyboard/README.md new file mode 100644 index 0000000..72d36e7 --- /dev/null +++ b/teleop_twist_keyboard/README.md @@ -0,0 +1,52 @@ +# teleop_twist_keyboard +Generic Keyboard Teleoperation for ROS + +## Run + +```sh +ros2 run teleop_twist_keyboard teleop_twist_keyboard +``` + +Publishing to a different topic (in this case `my_cmd_vel`). +```sh +ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=my_cmd_vel +``` + +## Usage + +``` +This node takes keypresses from the keyboard and publishes them as Twist +messages. It works best with a US keyboard layout. +--------------------------- +Moving around: + u i o + j k l + m , . + +For Holonomic mode (strafing), hold down the shift key: +--------------------------- + U I O + J K L + M < > + +t : up (+z) +b : down (-z) + +anything else : stop + +q/z : increase/decrease max speeds by 10% +w/x : increase/decrease only linear speed by 10% +e/c : increase/decrease only angular speed by 10% + +CTRL-C to quit +``` + +## Parameters +- `stamped (bool, default: false)` + - If false (the default), publish a `geometry_msgs/msg/Twist` message. If true, publish a `geometry_msgs/msg/TwistStamped` message. +- `frame_id (string, default: '')` + - When `stamped` is true, the frame_id to use when publishing the `geometry_msgs/msg/TwistStamped` message. +- `speed (double, default: 0.25)` + - The speed the node starts with by default. +- `turn (double, default: 0.5)` + - The turn rate (rad/s) the node starts with by default. diff --git a/teleop_twist_keyboard/package.xml b/teleop_twist_keyboard/package.xml new file mode 100644 index 0000000..b6d57f5 --- /dev/null +++ b/teleop_twist_keyboard/package.xml @@ -0,0 +1,27 @@ + + + teleop_twist_keyboard + 2.4.0 + + A robot-agnostic teleoperation node to convert keyboard commands to Twist + messages. + + + Chris Lalancette + + BSD License 2.0 + + Austin Hendrix + Graylin Trevor Jay + + geometry_msgs + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/teleop_twist_keyboard/resource/teleop_twist_keyboard b/teleop_twist_keyboard/resource/teleop_twist_keyboard new file mode 100644 index 0000000..e69de29 diff --git a/teleop_twist_keyboard/setup.cfg b/teleop_twist_keyboard/setup.cfg new file mode 100644 index 0000000..09fd871 --- /dev/null +++ b/teleop_twist_keyboard/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/teleop_twist_keyboard +[install] +install_scripts=$base/lib/teleop_twist_keyboard diff --git a/teleop_twist_keyboard/setup.py b/teleop_twist_keyboard/setup.py new file mode 100644 index 0000000..a97a6a7 --- /dev/null +++ b/teleop_twist_keyboard/setup.py @@ -0,0 +1,38 @@ +from setuptools import setup + +package_name = 'teleop_twist_keyboard' + +setup( + name=package_name, + version='2.4.0', + packages=[], + py_modules=[ + 'teleop_twist_keyboard' + ], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Chris Lalancette', + maintainer_email='clalancette@openrobotics.org', + author='Graylin Trevor Jay, Austin Hendrix', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: BSD', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='A robot-agnostic teleoperation node to convert keyboard' + 'commands to Twist messages.', + license='BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'teleop_twist_keyboard = teleop_twist_keyboard:main' + ], + }, +) diff --git a/teleop_twist_keyboard/teleop_twist_keyboard.py b/teleop_twist_keyboard/teleop_twist_keyboard.py new file mode 100755 index 0000000..c4574f7 --- /dev/null +++ b/teleop_twist_keyboard/teleop_twist_keyboard.py @@ -0,0 +1,231 @@ +# Copyright 2011 Brown University Robotics. +# Copyright 2017 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 the Willow Garage 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 sys +import threading + +import geometry_msgs.msg +import rclpy + +if sys.platform == 'win32': + import msvcrt +else: + import termios + import tty + + +msg = """ +This node takes keypresses from the keyboard and publishes them +as Twist/TwistStamped messages. It works best with a US keyboard layout. +--------------------------- +Moving around: + u i o + j k l + m , . + +For Holonomic mode (strafing), hold down the shift key: +--------------------------- + U I O + J K L + M < > + +t : up (+z) +b : down (-z) + +anything else : stop + +q/z : increase/decrease max speeds by 10% +w/x : increase/decrease only linear speed by 10% +e/c : increase/decrease only angular speed by 10% + +CTRL-C to quit +""" + +moveBindings = { + 'i': (1, 0, 0, 0), + 'o': (1, 0, 0, -1), + 'j': (0, 0, 0, 1), + 'l': (0, 0, 0, -1), + 'u': (1, 0, 0, 1), + ',': (-1, 0, 0, 0), + '.': (-1, 0, 0, 1), + 'm': (-1, 0, 0, -1), + 'O': (1, -1, 0, 0), + 'I': (1, 0, 0, 0), + 'J': (0, 1, 0, 0), + 'L': (0, -1, 0, 0), + 'U': (1, 1, 0, 0), + '<': (-1, 0, 0, 0), + '>': (-1, -1, 0, 0), + 'M': (-1, 1, 0, 0), + 't': (0, 0, 1, 0), + 'b': (0, 0, -1, 0), +} + +speedBindings = { + 'q': (1.1, 1.1), + 'z': (.9, .9), + 'w': (1.1, 1), + 'x': (.9, 1), + 'e': (1, 1.1), + 'c': (1, .9), +} + + +def getKey(settings): + if sys.platform == 'win32': + # getwch() returns a string on Windows + key = msvcrt.getwch() + else: + tty.setraw(sys.stdin.fileno()) + # sys.stdin.read() returns a string on Linux + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + + +def saveTerminalSettings(): + if sys.platform == 'win32': + return None + return termios.tcgetattr(sys.stdin) + + +def restoreTerminalSettings(old_settings): + if sys.platform == 'win32': + return + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + + +def vels(speed, turn): + return 'currently:\tspeed %s\tturn %s ' % (speed, turn) + + +def main(): + settings = saveTerminalSettings() + + rclpy.init() + + node = rclpy.create_node('teleop_twist_keyboard') + + # parameters + stamped = node.declare_parameter('stamped', False).value + frame_id = node.declare_parameter('frame_id', '').value + speed = node.declare_parameter('speed', 0.25).value + turn = node.declare_parameter('turn', 0.5).value + if not stamped and frame_id: + raise Exception("'frame_id' can only be set when 'stamped' is True") + + if stamped: + TwistMsg = geometry_msgs.msg.TwistStamped + else: + TwistMsg = geometry_msgs.msg.Twist + + pub = node.create_publisher(TwistMsg, 'cmd_vel', 10) + + spinner = threading.Thread(target=rclpy.spin, args=(node,)) + spinner.start() + + x = 0.0 + y = 0.0 + z = 0.0 + th = 0.0 + status = 0.0 + + twist_msg = TwistMsg() + + if stamped: + twist = twist_msg.twist + twist_msg.header.stamp = node.get_clock().now().to_msg() + twist_msg.header.frame_id = frame_id + else: + twist = twist_msg + + try: + print(msg) + print(vels(speed, turn)) + while True: + key = getKey(settings) + if key in moveBindings.keys(): + x = moveBindings[key][0] + y = moveBindings[key][1] + z = moveBindings[key][2] + th = moveBindings[key][3] + elif key in speedBindings.keys(): + speed = speed * speedBindings[key][0] + turn = turn * speedBindings[key][1] + + print(vels(speed, turn)) + if (status == 14): + print(msg) + status = (status + 1) % 15 + else: + x = 0.0 + y = 0.0 + z = 0.0 + th = 0.0 + if (key == '\x03'): + break + + if stamped: + twist_msg.header.stamp = node.get_clock().now().to_msg() + + twist.linear.x = x * speed + twist.linear.y = y * speed + twist.linear.z = z * speed + twist.angular.x = 0.0 + twist.angular.y = 0.0 + twist.angular.z = th * turn + pub.publish(twist_msg) + + except Exception as e: + print(e) + + finally: + if stamped: + twist_msg.header.stamp = node.get_clock().now().to_msg() + + twist.linear.x = 0.0 + twist.linear.y = 0.0 + twist.linear.z = 0.0 + twist.angular.x = 0.0 + twist.angular.y = 0.0 + twist.angular.z = 0.0 + pub.publish(twist_msg) + rclpy.shutdown() + spinner.join() + + restoreTerminalSettings(settings) + + +if __name__ == '__main__': + main() diff --git a/teleop_twist_keyboard/test/test_copyright.py b/teleop_twist_keyboard/test/test_copyright.py new file mode 100644 index 0000000..061fcfd --- /dev/null +++ b/teleop_twist_keyboard/test/test_copyright.py @@ -0,0 +1,20 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main + + +def test_copyright(): + rc = main(argv=['.']) + assert rc == 0, 'Found errors' diff --git a/teleop_twist_keyboard/test/test_flake8.py b/teleop_twist_keyboard/test/test_flake8.py new file mode 100644 index 0000000..ca8aa2c --- /dev/null +++ b/teleop_twist_keyboard/test/test_flake8.py @@ -0,0 +1,20 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main + + +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/teleop_twist_keyboard/test/test_pep257.py b/teleop_twist_keyboard/test/test_pep257.py new file mode 100644 index 0000000..5fbe08d --- /dev/null +++ b/teleop_twist_keyboard/test/test_pep257.py @@ -0,0 +1,20 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main + + +def test_pep257(): + rc = main(argv=['.']) + assert rc == 0, 'Found code style errors / warnings'