From c041ac26f0ca013f2944e85f1285b47ac739392f Mon Sep 17 00:00:00 2001 From: farrell2017 Date: Mon, 6 Sep 2021 19:00:41 -0400 Subject: [PATCH] Added joystick and platform packages --- src/cobot_platform/cobot_platform/__init__.py | 0 src/cobot_platform/cobot_platform/move.py | 60 +++++++++++++++++++ src/cobot_platform/package.xml | 18 ++++++ src/cobot_platform/resource/cobot_platform | 0 src/cobot_platform/setup.cfg | 4 ++ src/cobot_platform/setup.py | 26 ++++++++ src/cobot_platform/test/test_copyright.py | 23 +++++++ src/cobot_platform/test/test_flake8.py | 25 ++++++++ src/cobot_platform/test/test_pep257.py | 23 +++++++ src/joystickmove/joystickmove/__init__.py | 0 src/joystickmove/joystickmove/move.py | 33 ++++++++++ src/joystickmove/package.xml | 18 ++++++ src/joystickmove/resource/joystickmove | 0 src/joystickmove/setup.cfg | 4 ++ src/joystickmove/setup.py | 26 ++++++++ src/joystickmove/test/test_copyright.py | 23 +++++++ src/joystickmove/test/test_flake8.py | 25 ++++++++ src/joystickmove/test/test_pep257.py | 23 +++++++ 18 files changed, 331 insertions(+) create mode 100644 src/cobot_platform/cobot_platform/__init__.py create mode 100644 src/cobot_platform/cobot_platform/move.py create mode 100644 src/cobot_platform/package.xml create mode 100644 src/cobot_platform/resource/cobot_platform create mode 100644 src/cobot_platform/setup.cfg create mode 100644 src/cobot_platform/setup.py create mode 100644 src/cobot_platform/test/test_copyright.py create mode 100644 src/cobot_platform/test/test_flake8.py create mode 100644 src/cobot_platform/test/test_pep257.py create mode 100644 src/joystickmove/joystickmove/__init__.py create mode 100644 src/joystickmove/joystickmove/move.py create mode 100644 src/joystickmove/package.xml create mode 100644 src/joystickmove/resource/joystickmove create mode 100644 src/joystickmove/setup.cfg create mode 100644 src/joystickmove/setup.py create mode 100644 src/joystickmove/test/test_copyright.py create mode 100644 src/joystickmove/test/test_flake8.py create mode 100644 src/joystickmove/test/test_pep257.py diff --git a/src/cobot_platform/cobot_platform/__init__.py b/src/cobot_platform/cobot_platform/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py new file mode 100644 index 0000000..34e564a --- /dev/null +++ b/src/cobot_platform/cobot_platform/move.py @@ -0,0 +1,60 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 + +from serial import Serial + +from enum import Enum + +class Servos(Enum): + BODY = 0 + ARM = 1 + HAND = 2 + +class CobotMove(Node): + def __init__(self): + super().__init__('cobot_move') + self._logger = self.get_logger() + self._logger.info('COBOT init') + + self._serial = Serial('/dev/ttyACM0', 115200) + + # self._buf = [90, 90, 90] # 3 servos + # self._prev_buf = [90, 90, 90] + + base_topic = 'move/' + + self.create_subscription(Float32, base_topic+'body', self._move_body_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'arm', self._move_arm_cb, rclpy.qos.qos_profile_sensor_data) + self.create_subscription(Float32, base_topic+'hand', self._move_hand_cb, rclpy.qos.qos_profile_sensor_data) + + def _move_body_cb(self, msg): + self._move_servo(Servos.BODY, msg.data) + + def _move_arm_cb(self, msg): + self._move_servo(Servos.ARM, msg.data) + + def _move_hand_cb(self, msg): + self._move_servo(Servos.HAND, msg.data) + + def _move_servo(self, serv_num, pos): + cmd = 'SV' + # self._buf[serv_num] = pos + self._logger.info('COBOT: Servo {serv_num} to {pos}') + self._serial.write(f'{cmd}V{serv_num}P{pos}\n'.encode('UTF-8')) + + # def _push ? + +def main(args=None): + rclpy.init(args=args) + + cobot_move = CobotMove() + rclpy.spin(cobot_move) + + cobot_move.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/cobot_platform/package.xml b/src/cobot_platform/package.xml new file mode 100644 index 0000000..f9f29fe --- /dev/null +++ b/src/cobot_platform/package.xml @@ -0,0 +1,18 @@ + + + + cobot_platform + 1.0.0 + Driver for the cobot platform + John Farrell + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/cobot_platform/resource/cobot_platform b/src/cobot_platform/resource/cobot_platform new file mode 100644 index 0000000..e69de29 diff --git a/src/cobot_platform/setup.cfg b/src/cobot_platform/setup.cfg new file mode 100644 index 0000000..b1fcef0 --- /dev/null +++ b/src/cobot_platform/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/cobot_platform +[install] +install-scripts=$base/lib/cobot_platform diff --git a/src/cobot_platform/setup.py b/src/cobot_platform/setup.py new file mode 100644 index 0000000..926a3f7 --- /dev/null +++ b/src/cobot_platform/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'cobot_platform' + +setup( + name=package_name, + version='1.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='John Farrell', + maintainer_email='farrell2017@my.fit.edu', + description='Driver for the cobot platform', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'move = cobot_platform.move:main' + ], + }, +) diff --git a/src/cobot_platform/test/test_copyright.py b/src/cobot_platform/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/cobot_platform/test/test_copyright.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_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/cobot_platform/test/test_flake8.py b/src/cobot_platform/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/cobot_platform/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/src/cobot_platform/test/test_pep257.py b/src/cobot_platform/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/cobot_platform/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' diff --git a/src/joystickmove/joystickmove/__init__.py b/src/joystickmove/joystickmove/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py new file mode 100644 index 0000000..692356f --- /dev/null +++ b/src/joystickmove/joystickmove/move.py @@ -0,0 +1,33 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32 +from sensor_msgs.msg import Joy + +class JoyMove(Node): + def __init__(self): + super().__init__('joystickmove') + self._logger = self.get_logger() + self._logger.info('joystick init') + + self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data) + + self.base_pub = self.create_publisher(Float32, '/move/body', 10) + + def _joy_cb(self, msg): + asd = (msg.axes[1] + 1) / 2 * 180.0 + self.base_pub.publish(Float32(data=asd)) + + +def main(args=None): + rclpy.init(args=args) + + joy_move = JoyMove() + rclpy.spin(joy_move) + + joy_move.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/joystickmove/package.xml b/src/joystickmove/package.xml new file mode 100644 index 0000000..5878684 --- /dev/null +++ b/src/joystickmove/package.xml @@ -0,0 +1,18 @@ + + + + joystickmove + 0.0.0 + Joystick memes + John Farrell + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/joystickmove/resource/joystickmove b/src/joystickmove/resource/joystickmove new file mode 100644 index 0000000..e69de29 diff --git a/src/joystickmove/setup.cfg b/src/joystickmove/setup.cfg new file mode 100644 index 0000000..148efe4 --- /dev/null +++ b/src/joystickmove/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/joystickmove +[install] +install-scripts=$base/lib/joystickmove diff --git a/src/joystickmove/setup.py b/src/joystickmove/setup.py new file mode 100644 index 0000000..09fd878 --- /dev/null +++ b/src/joystickmove/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'joystickmove' + +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='John Farrell', + maintainer_email='farrell2017@my.fit.edu', + description='Joystick memes', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'joystickmove = joystickmove.move:main' + ], + }, +) diff --git a/src/joystickmove/test/test_copyright.py b/src/joystickmove/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/joystickmove/test/test_copyright.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_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/joystickmove/test/test_flake8.py b/src/joystickmove/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/joystickmove/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/src/joystickmove/test/test_pep257.py b/src/joystickmove/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/joystickmove/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'