├── config ├── ps3-holonomic.config.yaml ├── ps3.config.yaml ├── joy-con-r.config.yaml ├── atk3.config.yaml ├── xd3.config.yaml └── xbox.config.yaml ├── README.md ├── test ├── no_enable_joy.test ├── differential_joy.test ├── only_turbo_joy.test ├── holonomic_joy.test ├── turbo_enable_joy.test ├── turbo_angular_enable_joy.test ├── turbo_angular_enable_joy_with_rosparam_map.test ├── six_dof_joy.test └── test_joy_twist.py ├── launch └── teleop.launch ├── .gitignore ├── package.xml ├── .travis.yml ├── CHANGELOG.rst ├── LICENSE.txt ├── CMakeLists.txt ├── src ├── teleop_node.cpp └── teleop_twist_joy.cpp └── include └── teleop_twist_joy └── teleop_twist_joy.h /config/ps3-holonomic.config.yaml: -------------------------------------------------------------------------------- 1 | axis_linear: 2 | x: 1 3 | y: 0 4 | scale_linear: 5 | x: 0.4 6 | y: 0.4 7 | axis_angular: 8 | yaw: 2 9 | scale_angular: 10 | yaw: 0.5 11 | enable_button: 10 12 | enable_turbo_button: 8 13 | -------------------------------------------------------------------------------- /config/ps3.config.yaml: -------------------------------------------------------------------------------- 1 | axis_linear: 1 2 | scale_linear: 0.7 3 | scale_linear_turbo: 1.5 4 | 5 | axis_angular: 0 6 | scale_angular: 0.4 7 | 8 | enable_button: 8 # L2 shoulder button 9 | enable_turbo_button: 10 # L1 shoulder button 10 | -------------------------------------------------------------------------------- /config/joy-con-r.config.yaml: -------------------------------------------------------------------------------- 1 | # Nintendo Switch Joy-Con(R) controller 2 | axis_linear: 4 3 | scale_linear: 0.7 4 | scale_linear_turbo: 1.5 5 | 6 | axis_angular: 5 7 | scale_angular: 0.4 8 | 9 | enable_button: 15 # ZR button 10 | enable_turbo_button: 14 # R button 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | teleop_twist_joy [![Build Status](https://travis-ci.org/ros-teleop/teleop_twist_joy.svg?branch=indigo-devel)](https://travis-ci.org/ros-teleop/teleop_twist_joy) 2 | ================ 3 | 4 | Simple joystick teleop for twist robots. See [ROS Wiki](http://wiki.ros.org/teleop_twist_joy) 5 | -------------------------------------------------------------------------------- /config/atk3.config.yaml: -------------------------------------------------------------------------------- 1 | # Logitech Attack3 2 | axis_linear: 1 # Forward/Back 3 | scale_linear: 0.7 4 | scale_linear_turbo: 1.5 5 | 6 | axis_angular: 0 # Left/Right 7 | scale_angular: 0.4 8 | 9 | enable_button: 0 # Trigger 10 | enable_turbo_button: 1 # Button 2 aka thumb button 11 | -------------------------------------------------------------------------------- /config/xd3.config.yaml: -------------------------------------------------------------------------------- 1 | # Logitech Extreme 3D Pro 2 | axis_linear: 1 # Forward/Back 3 | scale_linear: 0.7 4 | scale_linear_turbo: 1.5 5 | 6 | axis_angular: 2 # Twist 7 | scale_angular: 0.4 8 | 9 | enable_button: 0 # Trigger 10 | enable_turbo_button: 1 # Button 2 aka thumb button 11 | -------------------------------------------------------------------------------- /config/xbox.config.yaml: -------------------------------------------------------------------------------- 1 | axis_linear: 1 # Left thumb stick vertical 2 | scale_linear: 0.7 3 | scale_linear_turbo: 1.5 4 | 5 | axis_angular: 0 # Left thumb stick horizontal 6 | scale_angular: 0.4 7 | 8 | enable_button: 2 # Left trigger button 9 | enable_turbo_button: 5 # Right trigger button 10 | -------------------------------------------------------------------------------- /test/no_enable_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 1 5 | axis_angular: 0 6 | scale_linear: 2.0 7 | scale_angular: 3.0 8 | enable_button: 0 9 | 10 | 11 | 12 | 13 | 14 | publish_joy: 15 | axes: [ 0.3, 0.4 ] 16 | buttons: [ 0 ] 17 | expect_cmd_vel: 18 | linear: { x: 0, y: 0, z: 0 } 19 | angular: { x: 0, y: 0, z: 0 } 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /test/differential_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 1 5 | axis_angular: 0 6 | scale_linear: 2.0 7 | scale_angular: 3.0 8 | enable_button: 0 9 | 10 | 11 | 12 | 13 | 14 | publish_joy: 15 | axes: [ 0.3, 0.4 ] 16 | buttons: [ 1 ] 17 | expect_cmd_vel: 18 | linear: { x: 0.8, y: 0, z: 0 } 19 | angular: { x: 0, y: 0, z: 0.9 } 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /test/only_turbo_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 1 5 | axis_angular: 0 6 | scale_linear: 1.0 7 | scale_linear_turbo: 2.0 8 | scale_angular: 3.0 9 | enable_button: 0 10 | enable_turbo_button: 1 11 | 12 | 13 | 14 | 15 | 16 | publish_joy: 17 | axes: [ 0.3, 0.4 ] 18 | buttons: [ 0, 1 ] 19 | expect_cmd_vel: 20 | linear: { x: 0.8, y: 0, z: 0 } 21 | angular: { x: 0, y: 0, z: 0.9 } 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /test/holonomic_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 5 | x: 1 6 | y: 2 7 | scale_linear: 8 | x: 1.0 9 | y: 4.0 10 | axis_angular: 0 11 | scale_angular: 3.0 12 | enable_button: 0 13 | 14 | 15 | 16 | 17 | 18 | publish_joy: 19 | axes: [ 0.3, 0.4, 0.5 ] 20 | buttons: [ 1 ] 21 | expect_cmd_vel: 22 | linear: { x: 0.4, y: 2.0, z: 0 } 23 | angular: { x: 0, y: 0, z: 0.9 } 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /test/turbo_enable_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 1 5 | axis_angular: 0 6 | scale_linear: 1.0 7 | scale_linear_turbo: 2.0 8 | scale_angular: 3.0 9 | enable_button: 0 10 | enable_turbo_button: 1 11 | 12 | 13 | 14 | 15 | 16 | publish_joy: 17 | axes: [ 0.3, 0.4 ] 18 | buttons: [ 1, 1 ] 19 | expect_cmd_vel: 20 | linear: { x: 0.8, y: 0, z: 0 } 21 | angular: { x: 0, y: 0, z: 0.9 } 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /test/turbo_angular_enable_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 1 5 | axis_angular: 0 6 | scale_linear: 1.0 7 | scale_linear_turbo: 2.0 8 | scale_angular: 1.0 9 | scale_angular_turbo: 2.0 10 | enable_button: 0 11 | enable_turbo_button: 1 12 | 13 | 14 | 15 | 16 | 17 | publish_joy: 18 | axes: [ 0.3, 0.4 ] 19 | buttons: [ 1, 1 ] 20 | expect_cmd_vel: 21 | linear: { x: 0.8, y: 0, z: 0 } 22 | angular: { x: 0, y: 0, z: 0.6 } 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ~$ 2 | .swp$ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/.*Action\.msg$ 9 | msg/.*ActionFeedback\.msg$ 10 | msg/.*ActionGoal\.msg$ 11 | msg/.*ActionResult\.msg$ 12 | msg/.*Feedback\.msg$ 13 | msg/.*Goal\.msg$ 14 | msg/.*Result\.msg$ 15 | msg/_.*\.py$ 16 | 17 | \.pcd$ 18 | .pyc$ 19 | 20 | # Generated by dynamic reconfigure 21 | \.cfgc$ 22 | /cfg/cpp/ 23 | /cfg/.*\.py$ 24 | 25 | # Ignore generated docs 26 | .dox$ 27 | .wikidoc$ 28 | 29 | # eclipse stuff 30 | .project 31 | .cproject 32 | 33 | # qcreator stuff 34 | CMakeLists.txt.user 35 | 36 | srv/_.*\.py$ 37 | \.pcd$ 38 | .pyc$ 39 | qtcreator-* 40 | *.user 41 | 42 | /planning/cfg 43 | /planning/docs 44 | /planning/src 45 | 46 | *~$ 47 | 48 | # Emacs 49 | .#* 50 | 51 | # Catkin custom files 52 | CATKIN_IGNORE 53 | -------------------------------------------------------------------------------- /test/turbo_angular_enable_joy_with_rosparam_map.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 5 | x: 1 6 | axis_angular: 7 | yaw: 0 8 | scale_linear: 9 | x: 1.0 10 | scale_linear_turbo: 11 | x: 2.0 12 | scale_angular: 13 | yaw: 1.0 14 | scale_angular_turbo: 15 | yaw: 2.0 16 | enable_button: 0 17 | enable_turbo_button: 1 18 | 19 | 20 | 21 | 22 | 23 | publish_joy: 24 | axes: [ 0.3, 0.4 ] 25 | buttons: [ 1, 1 ] 26 | expect_cmd_vel: 27 | linear: { x: 0.8, y: 0, z: 0 } 28 | angular: { x: 0, y: 0, z: 0.6 } 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /test/six_dof_joy.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | axis_linear: 5 | x: 1 6 | y: 2 7 | z: 3 8 | scale_linear: 9 | x: 1.0 10 | y: 4.0 11 | z: 2.0 12 | axis_angular: 13 | roll: 4 14 | pitch: 5 15 | yaw: 0 16 | scale_angular: 17 | roll: 3.0 18 | pitch: 2.0 19 | yaw: 1.0 20 | enable_button: 0 21 | 22 | 23 | 24 | 25 | 26 | publish_joy: 27 | axes: [ 0.3, 0.4, 0.5, 0.6, 0.7, 0.8 ] 28 | buttons: [ 1 ] 29 | expect_cmd_vel: 30 | linear: { x: 0.4, y: 2.0, z: 1.2 } 31 | angular: { x: 2.1, y: 1.6, z: 0.3 } 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | teleop_twist_joy 4 | 0.1.3 5 | Generic joystick teleop for twist robots. 6 | Mike Purvis 7 | 8 | BSD 9 | 10 | http://wiki.ros.org/teleop_twist_joy 11 | Mike Purvis 12 | 13 | catkin 14 | geometry_msgs 15 | joy 16 | roscpp 17 | roslaunch 18 | roslint 19 | rostest 20 | sensor_msgs 21 | geometry_msgs 22 | joy 23 | roscpp 24 | sensor_msgs 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | language: generic # optional, just removes the language badge 5 | 6 | services: 7 | - docker 8 | 9 | # include the following block if the C/C++ build artifacts should get cached by Travis, 10 | # CCACHE_DIR needs to get set as well to actually fill the cache 11 | cache: 12 | directories: 13 | - $HOME/.ccache 14 | 15 | git: 16 | quiet: true # optional, silences the cloning of the target repository 17 | 18 | # configure the build environment(s) 19 | # https://github.com/ros-industrial/industrial_ci/blob/master/doc/index.rst#variables-you-can-configure 20 | env: 21 | global: # global settings for all jobs 22 | - ROS_REPO=ros 23 | - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci 24 | matrix: # each line is a job 25 | - ROS_DISTRO="indigo" 26 | - ROS_DISTRO="kinetic" 27 | - ROS_DISTRO="melodic" 28 | 29 | # clone and run industrial_ci 30 | install: 31 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci 32 | script: 33 | - .industrial_ci/travis.sh -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package teleop_twist_joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.3 (2019-01-21) 6 | ------------------ 7 | * Use industrial ci 8 | * Don't crash with invalid axes. 9 | * Make sure to not crash when the Joy message buttons is too small. 10 | * Don't get the axis twice. 11 | * Add ROS Wiki link (`#26 `_) 12 | * Contributors: Chris Lalancette, Julian Gaal, Rousseau Vincent, vincentrou 13 | 14 | 0.1.2 (2016-08-31) 15 | ------------------ 16 | * Fixed incorrect key. (`#21 `_) 17 | * Allow custom config file from location outside of this package 18 | * Setting scale_angular_turbo if axis_angular is set so that turning works when turbo is pressed. 19 | * Added turbo scale for angular velocities and accompanying test. 20 | * Add LICENSE.txt. 21 | * Contributors: Daniel Aden, Isaac I.Y. Saito, Mike Purvis, Tony Baltovski 22 | 23 | 0.1.1 (2015-06-27) 24 | ------------------ 25 | * Add rostests. 26 | * Added maps to allow multi-dof velocity publishing. 27 | * Added Xbox 360 controller example. 28 | * Contributors: Mike Purvis, Tony Baltovski 29 | 30 | 0.1.0 (2014-07-25) 31 | ------------------ 32 | * Added configurations for Logitech Attack3 and Extreme 3D Pro joysticks. 33 | * Initial version, with example config for PS3 joystick. 34 | * Contributors: Mike Purvis, Tony Baltovski 35 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD) 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 4 | the following conditions are met: 5 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 6 | following disclaimer. 7 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 8 | following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 10 | products derived from this software without specific prior written permission. 11 | 12 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 13 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 14 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 15 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 16 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 17 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 18 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE 19 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(teleop_twist_joy) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp sensor_msgs) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | LIBRARIES ${PROJECT_NAME} 9 | CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs 10 | ) 11 | 12 | include_directories(include ${catkin_INCLUDE_DIRS}) 13 | 14 | add_library(${PROJECT_NAME} src/${PROJECT_NAME}) 15 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 16 | 17 | add_executable(${PROJECT_NAME}_node src/teleop_node.cpp) 18 | target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) 19 | set_target_properties(${PROJECT_NAME}_node 20 | PROPERTIES OUTPUT_NAME teleop_node PREFIX "") 21 | 22 | install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 23 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 26 | 27 | install(DIRECTORY include/${PROJECT_NAME}/ 28 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 29 | 30 | install(DIRECTORY launch config 31 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 32 | 33 | if (CATKIN_ENABLE_TESTING) 34 | find_package(roslint REQUIRED) 35 | roslint_cpp(include/ src/) 36 | roslint_python() 37 | roslint_add_test() 38 | 39 | find_package(roslaunch REQUIRED) 40 | roslaunch_add_file_check(launch/teleop.launch) 41 | 42 | find_package(rostest REQUIRED) 43 | include_directories(include ${catkin_INCLUDE_DIRS}) 44 | 45 | # Check axes and scaling. 46 | add_rostest(test/differential_joy.test) 47 | add_rostest(test/holonomic_joy.test) 48 | add_rostest(test/six_dof_joy.test) 49 | 50 | # Check enable and turbo button logic. 51 | add_rostest(test/no_enable_joy.test) 52 | add_rostest(test/turbo_enable_joy.test) 53 | add_rostest(test/only_turbo_joy.test) 54 | add_rostest(test/turbo_angular_enable_joy.test) 55 | add_rostest(test/turbo_angular_enable_joy_with_rosparam_map.test) 56 | endif() 57 | -------------------------------------------------------------------------------- /src/teleop_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Software License Agreement (BSD) 3 | 4 | \file teleop_node.cpp 5 | \authors Mike Purvis 6 | \copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 7 | 8 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 9 | the following conditions are met: 10 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 11 | following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 13 | following disclaimer in the documentation and/or other materials provided with the distribution. 14 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 15 | products derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 18 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 19 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 20 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 21 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include "ros/ros.h" 27 | #include "teleop_twist_joy/teleop_twist_joy.h" 28 | 29 | int main(int argc, char *argv[]) 30 | { 31 | ros::init(argc, argv, "teleop_twist_joy_node"); 32 | 33 | ros::NodeHandle nh(""), nh_param("~"); 34 | teleop_twist_joy::TeleopTwistJoy joy_teleop(&nh, &nh_param); 35 | 36 | ros::spin(); 37 | } 38 | -------------------------------------------------------------------------------- /include/teleop_twist_joy/teleop_twist_joy.h: -------------------------------------------------------------------------------- 1 | /** 2 | Software License Agreement (BSD) 3 | 4 | \authors Mike Purvis 5 | \copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 8 | the following conditions are met: 9 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | following disclaimer in the documentation and/or other materials provided with the distribution. 13 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 14 | products derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 17 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 19 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 20 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 22 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | */ 24 | 25 | #ifndef TELEOP_TWIST_JOY_TELEOP_TWIST_JOY_H 26 | #define TELEOP_TWIST_JOY_TELEOP_TWIST_JOY_H 27 | 28 | namespace ros { class NodeHandle; } 29 | 30 | namespace teleop_twist_joy 31 | { 32 | 33 | /** 34 | * Class implementing a basic Joy -> Twist translation. 35 | */ 36 | class TeleopTwistJoy 37 | { 38 | public: 39 | TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param); 40 | 41 | private: 42 | struct Impl; 43 | Impl* pimpl_; 44 | }; 45 | 46 | } // namespace teleop_twist_joy 47 | 48 | #endif // TELEOP_TWIST_JOY_TELEOP_TWIST_JOY_H 49 | -------------------------------------------------------------------------------- /test/test_joy_twist.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD) 3 | # 4 | # @author Tony Baltovski 5 | # @copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without modification, are permitted provided that 8 | # the following conditions are met: 9 | # * Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | # following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | # following disclaimer in the documentation and/or other materials provided with the distribution. 13 | # * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or 14 | # promote products derived from this software without specific prior written permission. 15 | # 16 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 17 | # WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 | # PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 19 | # ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 20 | # TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21 | # HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 22 | # NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 23 | # POSSIBILITY OF SUCH DAMAGE. 24 | 25 | import sys 26 | import unittest 27 | import time 28 | import rostest 29 | import rospy 30 | import geometry_msgs.msg 31 | import sensor_msgs.msg 32 | 33 | 34 | class TestJoyTwist(unittest.TestCase): 35 | 36 | def setUp(self): 37 | rospy.init_node('test_joy_twist_node', anonymous=True) 38 | self.pub = rospy.Publisher('joy', sensor_msgs.msg.Joy) 39 | rospy.Subscriber('cmd_vel', geometry_msgs.msg.Twist, callback=self.callback) 40 | 41 | while (not rospy.has_param("~publish_joy")) and (not rospy.get_param("~expect_cmd_vel")): 42 | time.sleep(0.1) 43 | 44 | self.expect_cmd_vel = rospy.get_param("~expect_cmd_vel") 45 | self.joy_msg = rospy.get_param("~publish_joy") 46 | self.received_cmd_vel = None 47 | 48 | def test_expected(self): 49 | pub_joy = sensor_msgs.msg.Joy() 50 | pub_joy.axes.extend(self.joy_msg['axes']) 51 | pub_joy.buttons.extend(self.joy_msg['buttons']) 52 | while self.received_cmd_vel is None: 53 | self.pub.publish(pub_joy) 54 | time.sleep(0.1) 55 | 56 | self.assertAlmostEqual(self.received_cmd_vel.linear.x, self.expect_cmd_vel['linear']['x']) 57 | self.assertAlmostEqual(self.received_cmd_vel.linear.y, self.expect_cmd_vel['linear']['y']) 58 | self.assertAlmostEqual(self.received_cmd_vel.linear.z, self.expect_cmd_vel['linear']['z']) 59 | self.assertAlmostEqual(self.received_cmd_vel.angular.x, self.expect_cmd_vel['angular']['x']) 60 | self.assertAlmostEqual(self.received_cmd_vel.angular.y, self.expect_cmd_vel['angular']['y']) 61 | self.assertAlmostEqual(self.received_cmd_vel.angular.z, self.expect_cmd_vel['angular']['z']) 62 | 63 | def callback(self, msg): 64 | self.received_cmd_vel = geometry_msgs.msg.Twist() 65 | self.received_cmd_vel = msg 66 | 67 | 68 | if __name__ == '__main__': 69 | rostest.rosrun('teleop_twist_joy', 'test_joy_twist', TestJoyTwist) 70 | -------------------------------------------------------------------------------- /src/teleop_twist_joy.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Software License Agreement (BSD) 3 | 4 | \authors Mike Purvis 5 | \copyright Copyright (c) 2014, Clearpath Robotics, Inc., All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that 8 | the following conditions are met: 9 | * Redistributions of source code must retain the above copyright notice, this list of conditions and the 10 | following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the 12 | following disclaimer in the documentation and/or other materials provided with the distribution. 13 | * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote 14 | products derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- 17 | RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 18 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- 19 | DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 20 | OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 22 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | */ 24 | 25 | #include "geometry_msgs/Twist.h" 26 | #include "ros/ros.h" 27 | #include "sensor_msgs/Joy.h" 28 | #include "teleop_twist_joy/teleop_twist_joy.h" 29 | 30 | #include 31 | #include 32 | 33 | 34 | namespace teleop_twist_joy 35 | { 36 | 37 | /** 38 | * Internal members of class. This is the pimpl idiom, and allows more flexibility in adding 39 | * parameters later without breaking ABI compatibility, for robots which link TeleopTwistJoy 40 | * directly into base nodes. 41 | */ 42 | struct TeleopTwistJoy::Impl 43 | { 44 | void joyCallback(const sensor_msgs::Joy::ConstPtr& joy); 45 | void sendCmdVelMsg(const sensor_msgs::Joy::ConstPtr& joy_msg, const std::string& which_map); 46 | 47 | ros::Subscriber joy_sub; 48 | ros::Publisher cmd_vel_pub; 49 | 50 | int enable_button; 51 | int enable_turbo_button; 52 | 53 | std::map axis_linear_map; 54 | std::map< std::string, std::map > scale_linear_map; 55 | 56 | std::map axis_angular_map; 57 | std::map< std::string, std::map > scale_angular_map; 58 | 59 | bool sent_disable_msg; 60 | }; 61 | 62 | /** 63 | * Constructs TeleopTwistJoy. 64 | * \param nh NodeHandle to use for setting up the publisher and subscriber. 65 | * \param nh_param NodeHandle to use for searching for configuration parameters. 66 | */ 67 | TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param) 68 | { 69 | pimpl_ = new Impl; 70 | 71 | pimpl_->cmd_vel_pub = nh->advertise("cmd_vel", 1, true); 72 | pimpl_->joy_sub = nh->subscribe("joy", 1, &TeleopTwistJoy::Impl::joyCallback, pimpl_); 73 | 74 | nh_param->param("enable_button", pimpl_->enable_button, 0); 75 | nh_param->param("enable_turbo_button", pimpl_->enable_turbo_button, -1); 76 | 77 | if (nh_param->getParam("axis_linear", pimpl_->axis_linear_map)) 78 | { 79 | nh_param->getParam("scale_linear", pimpl_->scale_linear_map["normal"]); 80 | nh_param->getParam("scale_linear_turbo", pimpl_->scale_linear_map["turbo"]); 81 | } 82 | else 83 | { 84 | nh_param->param("axis_linear", pimpl_->axis_linear_map["x"], 1); 85 | nh_param->param("scale_linear", pimpl_->scale_linear_map["normal"]["x"], 0.5); 86 | nh_param->param("scale_linear_turbo", pimpl_->scale_linear_map["turbo"]["x"], 1.0); 87 | } 88 | 89 | if (nh_param->getParam("axis_angular", pimpl_->axis_angular_map)) 90 | { 91 | nh_param->getParam("scale_angular", pimpl_->scale_angular_map["normal"]); 92 | nh_param->getParam("scale_angular_turbo", pimpl_->scale_angular_map["turbo"]); 93 | } 94 | else 95 | { 96 | nh_param->param("axis_angular", pimpl_->axis_angular_map["yaw"], 0); 97 | nh_param->param("scale_angular", pimpl_->scale_angular_map["normal"]["yaw"], 0.5); 98 | nh_param->param("scale_angular_turbo", 99 | pimpl_->scale_angular_map["turbo"]["yaw"], pimpl_->scale_angular_map["normal"]["yaw"]); 100 | } 101 | 102 | ROS_INFO_NAMED("TeleopTwistJoy", "Teleop enable button %i.", pimpl_->enable_button); 103 | ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", 104 | "Turbo on button %i.", pimpl_->enable_turbo_button); 105 | 106 | for (std::map::iterator it = pimpl_->axis_linear_map.begin(); 107 | it != pimpl_->axis_linear_map.end(); ++it) 108 | { 109 | ROS_INFO_NAMED("TeleopTwistJoy", "Linear axis %s on %i at scale %f.", 110 | it->first.c_str(), it->second, pimpl_->scale_linear_map["normal"][it->first]); 111 | ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", 112 | "Turbo for linear axis %s is scale %f.", it->first.c_str(), pimpl_->scale_linear_map["turbo"][it->first]); 113 | } 114 | 115 | for (std::map::iterator it = pimpl_->axis_angular_map.begin(); 116 | it != pimpl_->axis_angular_map.end(); ++it) 117 | { 118 | ROS_INFO_NAMED("TeleopTwistJoy", "Angular axis %s on %i at scale %f.", 119 | it->first.c_str(), it->second, pimpl_->scale_angular_map["normal"][it->first]); 120 | ROS_INFO_COND_NAMED(pimpl_->enable_turbo_button >= 0, "TeleopTwistJoy", 121 | "Turbo for angular axis %s is scale %f.", it->first.c_str(), pimpl_->scale_angular_map["turbo"][it->first]); 122 | } 123 | 124 | pimpl_->sent_disable_msg = false; 125 | } 126 | 127 | double getVal(const sensor_msgs::Joy::ConstPtr& joy_msg, const std::map& axis_map, 128 | const std::map& scale_map, const std::string& fieldname) 129 | { 130 | if (axis_map.find(fieldname) == axis_map.end() || 131 | scale_map.find(fieldname) == scale_map.end() || 132 | joy_msg->axes.size() <= axis_map.at(fieldname)) 133 | { 134 | return 0.0; 135 | } 136 | 137 | return joy_msg->axes[axis_map.at(fieldname)] * scale_map.at(fieldname); 138 | } 139 | 140 | void TeleopTwistJoy::Impl::sendCmdVelMsg(const sensor_msgs::Joy::ConstPtr& joy_msg, 141 | const std::string& which_map) 142 | { 143 | // Initializes with zeros by default. 144 | geometry_msgs::Twist cmd_vel_msg; 145 | 146 | cmd_vel_msg.linear.x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x"); 147 | cmd_vel_msg.linear.y = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "y"); 148 | cmd_vel_msg.linear.z = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "z"); 149 | cmd_vel_msg.angular.z = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "yaw"); 150 | cmd_vel_msg.angular.y = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "pitch"); 151 | cmd_vel_msg.angular.x = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "roll"); 152 | 153 | cmd_vel_pub.publish(cmd_vel_msg); 154 | sent_disable_msg = false; 155 | } 156 | 157 | void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg) 158 | { 159 | if (enable_turbo_button >= 0 && 160 | joy_msg->buttons.size() > enable_turbo_button && 161 | joy_msg->buttons[enable_turbo_button]) 162 | { 163 | sendCmdVelMsg(joy_msg, "turbo"); 164 | } 165 | else if (joy_msg->buttons.size() > enable_button && 166 | joy_msg->buttons[enable_button]) 167 | { 168 | sendCmdVelMsg(joy_msg, "normal"); 169 | } 170 | else 171 | { 172 | // When enable button is released, immediately send a single no-motion command 173 | // in order to stop the robot. 174 | if (!sent_disable_msg) 175 | { 176 | // Initializes with zeros by default. 177 | geometry_msgs::Twist cmd_vel_msg; 178 | cmd_vel_pub.publish(cmd_vel_msg); 179 | sent_disable_msg = true; 180 | } 181 | } 182 | } 183 | 184 | } // namespace teleop_twist_joy 185 | --------------------------------------------------------------------------------