diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..83db85b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(rmp220_teleop) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) +find_package(segway_msgs REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + rmp220_teleop/rmp220_teleop.py + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6b62b2b --- /dev/null +++ b/package.xml @@ -0,0 +1,21 @@ + + + + rmp220_teleop + 0.0.0 + TODO: Package description + bjorn + TODO: License declaration + + rclpy + segway_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/resource/rmp220_teleop b/resource/rmp220_teleop new file mode 100644 index 0000000..e69de29 diff --git a/rmp220_teleop/__init__.py b/rmp220_teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rmp220_teleop/rmp220_teleop.py b/rmp220_teleop/rmp220_teleop.py new file mode 100644 index 0000000..8f253d1 --- /dev/null +++ b/rmp220_teleop/rmp220_teleop.py @@ -0,0 +1,59 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Twist +from segway_msgs.srv import RosSetChassisEnableCmd + + +class TeleopTwistJoy(Node): + + def __init__(self): + super().__init__('teleop_twist_joy') + self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) + self.joy_sub = self.create_subscription(Joy, 'joy', self.joy_callback, 10) + self.timer = self.create_timer(0.01, self.timer_callback) + self.twist = Twist() + + def joy_callback(self, joy_msg): + self.twist.linear.x = 1 * joy_msg.axes[1] + self.twist.angular.z = 1.0 * joy_msg.axes[0] + + def timer_callback(self): + self.cmd_vel_pub.publish(self.twist) + + +def enable_chassis(node): + chassis_enable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable') + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = True + while not chassis_enable.wait_for_service(timeout_sec=1.0): + print('Service not available, waiting again...') + chassis_enable.call_async(req) + +def disable_chassis(node): + chassis_disable = node.create_client(RosSetChassisEnableCmd, '/set_chassis_enable') + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = False + while not chassis_disable.wait_for_service(timeout_sec=1.0): + print('Service not available, waiting again...') + chassis_disable.call_async(req) + + +def main(args=None): + rclpy.init(args=args) + try: + teleop_twist_joy = TeleopTwistJoy() + enable_chassis(teleop_twist_joy) # Call the function to enable the chassis + rclpy.spin(teleop_twist_joy) + except KeyboardInterrupt: + disable_chassis(teleop_twist_joy) # chassis disable for safe close + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = False + #rclpy.spin_until_future_complete(teleop_twist_joy, future) + finally: + teleop_twist_joy.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..90532a6 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmp220_teleop +[install] +install_scripts=$base/lib/rmp220_teleop diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..3ba1868 --- /dev/null +++ b/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'rmp220_teleop' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='bjorn', + maintainer_email='bjoern.ellensohn@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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 +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'