From 2c7c2fca74db98c16dd98f1974bac6213705caca Mon Sep 17 00:00:00 2001 From: enaix Date: Tue, 18 Apr 2023 13:57:57 +0300 Subject: [PATCH 01/10] Add initial control node --- packages/wbb_control/package.xml | 21 +++++++ packages/wbb_control/setup.cfg | 4 ++ packages/wbb_control/setup.py | 26 +++++++++ packages/wbb_control/wbb_control/__init__.py | 0 packages/wbb_control/wbb_control/control.py | 58 ++++++++++++++++++++ packages/wbb_msgs/CMakeLists.txt | 6 +- packages/wbb_msgs/msg/Control.msg | 2 + packages/wbb_msgs/package.xml | 3 +- 8 files changed, 118 insertions(+), 2 deletions(-) create mode 100644 packages/wbb_control/package.xml create mode 100644 packages/wbb_control/setup.cfg create mode 100644 packages/wbb_control/setup.py create mode 100644 packages/wbb_control/wbb_control/__init__.py create mode 100644 packages/wbb_control/wbb_control/control.py create mode 100644 packages/wbb_msgs/msg/Control.msg diff --git a/packages/wbb_control/package.xml b/packages/wbb_control/package.xml new file mode 100644 index 0000000..89c1911 --- /dev/null +++ b/packages/wbb_control/package.xml @@ -0,0 +1,21 @@ + + + + wbb_control + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + rclpy + python3-websockets + + + ament_python + + diff --git a/packages/wbb_control/setup.cfg b/packages/wbb_control/setup.cfg new file mode 100644 index 0000000..77d8556 --- /dev/null +++ b/packages/wbb_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/wbb_control +[install] +install_scripts=$base/lib/wbb_control diff --git a/packages/wbb_control/setup.py b/packages/wbb_control/setup.py new file mode 100644 index 0000000..ef6def7 --- /dev/null +++ b/packages/wbb_control/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'wbb_control' + +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', 'websockets'], + zip_safe=True, + maintainer='root', + maintainer_email='root@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'control = wbb_control.control:main' + ], + }, +) diff --git a/packages/wbb_control/wbb_control/__init__.py b/packages/wbb_control/wbb_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/packages/wbb_control/wbb_control/control.py b/packages/wbb_control/wbb_control/control.py new file mode 100644 index 0000000..1dc234a --- /dev/null +++ b/packages/wbb_control/wbb_control/control.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +import asyncio +from enum import IntEnum + +import rclpy +import websockets +from rclpy.node import Node + +from wbb_msgs.msg import Control + +rclpy.init() + + +class Direction(IntEnum): + FWD = 0 + BACK = 1 + STOP = 2 + + +class ControlNode(Node): + def __init__(self): + super(ControlNode, self).__init__("control") + self.declare_parameter("esp32_ip", "192.168.1.121") + self.sub = self.create_subscription(Control, "movement", self.send, 10) + + async def init_conn(self): + ip = self.get_parameter("esp32_ip").get_parameter_value().string_value + self.ws = await websockets.connect("wss://" + ip + "/ws", port=80, ssl=False) + + async def send(self, msg): + d = Direction.STOP + if msg.velocity > 0: + d = Direction.FWD + elif msg.velocity < 0: + d = Direction.BACK + + await self.ws.send( + str(int(d)) + ";" + str(round(float(msg.curvature), 6)) + ";" + ) + + +node = ControlNode() + + +async def loop(): + await node.init_conn() + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0) + await asyncio.sleep(0.0001) + + +def main(): + asyncio.run(loop()) + + +if __name__ == "__main__": + main() diff --git a/packages/wbb_msgs/CMakeLists.txt b/packages/wbb_msgs/CMakeLists.txt index 4dcd60f..e66b9cc 100644 --- a/packages/wbb_msgs/CMakeLists.txt +++ b/packages/wbb_msgs/CMakeLists.txt @@ -7,14 +7,18 @@ add_compile_options(-Wall -Wextra -Wpedantic -Werror) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} msg/ImagePose.msg msg/ImagePoint.msg msg/ImageMarkerPos.msg msg/ImageMarkerPosArray.msg + msg/Control.msg DEPENDENCIES builtin_interfaces + geometry_msgs ADD_LINTER_TESTS ) @@ -25,4 +29,4 @@ endif() ament_export_dependencies(rosidl_default_runtime) -ament_package() \ No newline at end of file +ament_package() diff --git a/packages/wbb_msgs/msg/Control.msg b/packages/wbb_msgs/msg/Control.msg new file mode 100644 index 0000000..62894d0 --- /dev/null +++ b/packages/wbb_msgs/msg/Control.msg @@ -0,0 +1,2 @@ +float64 curvature +float64 velocity diff --git a/packages/wbb_msgs/package.xml b/packages/wbb_msgs/package.xml index 61098b9..9d52296 100644 --- a/packages/wbb_msgs/package.xml +++ b/packages/wbb_msgs/package.xml @@ -7,6 +7,7 @@ Skorobogatov Gordei MIT + geometry_msgs ament_cmake rosidl_default_generators @@ -19,4 +20,4 @@ ament_cmake - \ No newline at end of file + From b0e593a8f647ce3d41febf0150423e7bd37a14ad Mon Sep 17 00:00:00 2001 From: enaix Date: Tue, 18 Apr 2023 14:04:45 +0300 Subject: [PATCH 02/10] Fill package metadata --- packages/wbb_control/package.xml | 4 ++-- packages/wbb_control/setup.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/packages/wbb_control/package.xml b/packages/wbb_control/package.xml index 89c1911..82e1ce8 100644 --- a/packages/wbb_control/package.xml +++ b/packages/wbb_control/package.xml @@ -3,8 +3,8 @@ wbb_control 0.0.0 - TODO: Package description - root + Whiteboard bot controller node + enaix TODO: License declaration ament_copyright diff --git a/packages/wbb_control/setup.py b/packages/wbb_control/setup.py index ef6def7..c4dcb9a 100644 --- a/packages/wbb_control/setup.py +++ b/packages/wbb_control/setup.py @@ -13,9 +13,9 @@ ], install_requires=['setuptools', 'websockets'], zip_safe=True, - maintainer='root', - maintainer_email='root@todo.todo', - description='TODO: Package description', + maintainer='enaix', + maintainer_email='enaix@protonmail.com', + description='Whiteboard bot control node', license='TODO: License declaration', tests_require=['pytest'], entry_points={ From f87b50a4941d5bafb5c1623858bd8573fb10385b Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 19 Apr 2023 20:21:33 +0300 Subject: [PATCH 03/10] Add joystick support (WIP) --- packages/joystick_proxy/config/ipega.yaml | 5 ++ packages/joystick_proxy/config/ps4.yaml | 5 ++ .../joystick_proxy/joystick_proxy/__init__.py | 0 .../joystick_proxy/joystick_proxy/joystick.py | 46 +++++++++++++++++++ packages/joystick_proxy/launch/joystick.yaml | 12 +++++ packages/joystick_proxy/package.xml | 23 ++++++++++ packages/joystick_proxy/setup.cfg | 4 ++ packages/joystick_proxy/setup.py | 29 ++++++++++++ .../joystick_proxy/test/test_copyright.py | 23 ++++++++++ packages/joystick_proxy/test/test_flake8.py | 25 ++++++++++ packages/joystick_proxy/test/test_pep257.py | 23 ++++++++++ packages/wbb_control/launch/control.yaml | 7 +++ packages/wbb_control/package.xml | 4 +- packages/wbb_control/test/test_copyright.py | 23 ++++++++++ packages/wbb_control/test/test_flake8.py | 25 ++++++++++ packages/wbb_control/test/test_pep257.py | 23 ++++++++++ packages/wbb_control/wbb_control/control.py | 4 +- 17 files changed, 277 insertions(+), 4 deletions(-) create mode 100644 packages/joystick_proxy/config/ipega.yaml create mode 100644 packages/joystick_proxy/config/ps4.yaml create mode 100644 packages/joystick_proxy/joystick_proxy/__init__.py create mode 100644 packages/joystick_proxy/joystick_proxy/joystick.py create mode 100644 packages/joystick_proxy/launch/joystick.yaml create mode 100644 packages/joystick_proxy/package.xml create mode 100644 packages/joystick_proxy/setup.cfg create mode 100644 packages/joystick_proxy/setup.py create mode 100644 packages/joystick_proxy/test/test_copyright.py create mode 100644 packages/joystick_proxy/test/test_flake8.py create mode 100644 packages/joystick_proxy/test/test_pep257.py create mode 100644 packages/wbb_control/launch/control.yaml create mode 100644 packages/wbb_control/test/test_copyright.py create mode 100644 packages/wbb_control/test/test_flake8.py create mode 100644 packages/wbb_control/test/test_pep257.py diff --git a/packages/joystick_proxy/config/ipega.yaml b/packages/joystick_proxy/config/ipega.yaml new file mode 100644 index 0000000..59b451e --- /dev/null +++ b/packages/joystick_proxy/config/ipega.yaml @@ -0,0 +1,5 @@ +velocity_axis: 3 +curvature_axis: 0 +off_button: 0 +remote_button: 1 +auto_button: 4 diff --git a/packages/joystick_proxy/config/ps4.yaml b/packages/joystick_proxy/config/ps4.yaml new file mode 100644 index 0000000..43c712a --- /dev/null +++ b/packages/joystick_proxy/config/ps4.yaml @@ -0,0 +1,5 @@ +velocity_axis: 4 +curvature_axis: 0 +off_button: 0 +remote_button: 1 +auto_button: 2 diff --git a/packages/joystick_proxy/joystick_proxy/__init__.py b/packages/joystick_proxy/joystick_proxy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/packages/joystick_proxy/joystick_proxy/joystick.py b/packages/joystick_proxy/joystick_proxy/joystick.py new file mode 100644 index 0000000..19ae0c1 --- /dev/null +++ b/packages/joystick_proxy/joystick_proxy/joystick.py @@ -0,0 +1,46 @@ +import rclpy +from rclpy.node import Node +from yaml import load, Loader + +from wbb_msgs.msg import Control +from sensor_msgs.msg import Joy + + +class Joystick(Node): + def __init__(self): + super(Joystick, self).__init__('joystick') + self.sub = self.create_subscription(Joy, "/joy", self.handleJoystick, 10) + self.pub = self.create_publisher(Control, "/control", 10) + + # Path to the joy control map yaml file + self.declare_parameter('control_map', None) + self.declare_parameter('max_curvature', 2.0) + self.declare_parameter('min_velocity', 0.1) + + self.max_curv = self.get_parameter('max_curvature').value + self.min_velocity = self.get_parameter('min_velocity').value + self.loadConfig() + + def loadConfig(self): + with open(self.get_parameter('control_map').value, 'r') as f: + self.joy_map = load(f, Loader=Loader) + + def handleJoystick(self, msg): + curv = msg.axes[self.joy_map["curvature_axis"]] * self.max_curv + vel = msg.axes[self.joy_map["velocity_axis"]] + if (abs(vel) < self.min_velocity): + vel = 0.0 # stop + + control = Control() + control.curvature = curv + control.velocity = vel + self.pub.publish(control) + +def main(): + rclpy.init() + joy = Joystick() + rclpy.spin(joy) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/packages/joystick_proxy/launch/joystick.yaml b/packages/joystick_proxy/launch/joystick.yaml new file mode 100644 index 0000000..afbb5de --- /dev/null +++ b/packages/joystick_proxy/launch/joystick.yaml @@ -0,0 +1,12 @@ +launch: +- arg: + name: "control_name" + default: "ps4" +- node: + pkg: "joystick_proxy" + exec: "joystick" + name: "joystick" + param: + - {name: "control_map", value: "$(find-pkg-share joystick_proxy)/config/$(var control_name).yaml"} + - {name: "max_curvature", value: 40.0} + - {name: "min_velocity", value: 0.0} diff --git a/packages/joystick_proxy/package.xml b/packages/joystick_proxy/package.xml new file mode 100644 index 0000000..6c5c537 --- /dev/null +++ b/packages/joystick_proxy/package.xml @@ -0,0 +1,23 @@ + + + + joystick_proxy + 0.0.0 + Simple joystick proxy + enaix + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + sensor_msgs + python3-yaml + wbb_msgs + rclpy + joy + + + ament_python + + diff --git a/packages/joystick_proxy/setup.cfg b/packages/joystick_proxy/setup.cfg new file mode 100644 index 0000000..abf2fa3 --- /dev/null +++ b/packages/joystick_proxy/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/joystick_proxy +[install] +install_scripts=$base/lib/joystick_proxy diff --git a/packages/joystick_proxy/setup.py b/packages/joystick_proxy/setup.py new file mode 100644 index 0000000..163b390 --- /dev/null +++ b/packages/joystick_proxy/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'joystick_proxy' + +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']), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='enaix', + maintainer_email='enaix@protonmail.com', + description='Simple joystick proxy node', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'joystick = joystick_proxy.joystick:main' + ], + }, +) diff --git a/packages/joystick_proxy/test/test_copyright.py b/packages/joystick_proxy/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/packages/joystick_proxy/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/packages/joystick_proxy/test/test_flake8.py b/packages/joystick_proxy/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/packages/joystick_proxy/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/packages/joystick_proxy/test/test_pep257.py b/packages/joystick_proxy/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/packages/joystick_proxy/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/packages/wbb_control/launch/control.yaml b/packages/wbb_control/launch/control.yaml new file mode 100644 index 0000000..22bafd8 --- /dev/null +++ b/packages/wbb_control/launch/control.yaml @@ -0,0 +1,7 @@ +launch: +- node: + pkg: "wbb_control" + exec: "control" + name: "control" + param: + - {name: "esp32_ip", value: "192.168.1.121"} diff --git a/packages/wbb_control/package.xml b/packages/wbb_control/package.xml index 82e1ce8..f39ce4e 100644 --- a/packages/wbb_control/package.xml +++ b/packages/wbb_control/package.xml @@ -11,9 +11,9 @@ ament_flake8 ament_pep257 python3-pytest - + wbb_msgs + python3-websockets rclpy - python3-websockets ament_python diff --git a/packages/wbb_control/test/test_copyright.py b/packages/wbb_control/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/packages/wbb_control/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/packages/wbb_control/test/test_flake8.py b/packages/wbb_control/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/packages/wbb_control/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/packages/wbb_control/test/test_pep257.py b/packages/wbb_control/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/packages/wbb_control/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/packages/wbb_control/wbb_control/control.py b/packages/wbb_control/wbb_control/control.py index 1dc234a..9ec5110 100644 --- a/packages/wbb_control/wbb_control/control.py +++ b/packages/wbb_control/wbb_control/control.py @@ -22,10 +22,10 @@ class ControlNode(Node): def __init__(self): super(ControlNode, self).__init__("control") self.declare_parameter("esp32_ip", "192.168.1.121") - self.sub = self.create_subscription(Control, "movement", self.send, 10) + self.sub = self.create_subscription(Control, "/control", self.send, 10) async def init_conn(self): - ip = self.get_parameter("esp32_ip").get_parameter_value().string_value + ip = self.get_parameter("esp32_ip").value self.ws = await websockets.connect("wss://" + ip + "/ws", port=80, ssl=False) async def send(self, msg): From a27e58dc229dbd73e12276a765875d3afaea2eaf Mon Sep 17 00:00:00 2001 From: Flynn Date: Thu, 27 Apr 2023 13:53:05 +0300 Subject: [PATCH 04/10] Add missing resources file --- packages/joystick_proxy/resource/joystick_proxy | 0 packages/wbb_control/resource/wbb_control | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 packages/joystick_proxy/resource/joystick_proxy create mode 100644 packages/wbb_control/resource/wbb_control diff --git a/packages/joystick_proxy/resource/joystick_proxy b/packages/joystick_proxy/resource/joystick_proxy new file mode 100644 index 0000000..e69de29 diff --git a/packages/wbb_control/resource/wbb_control b/packages/wbb_control/resource/wbb_control new file mode 100644 index 0000000..e69de29 From ec9ac8d2f8e5de5dee9a61215791e43ae5444282 Mon Sep 17 00:00:00 2001 From: enaix Date: Mon, 15 May 2023 13:29:17 +0300 Subject: [PATCH 05/10] Add ps5 control map --- packages/joystick_proxy/config/ps5.yaml | 5 +++++ packages/wbb_control/wbb_control/control.py | 11 ++++------- 2 files changed, 9 insertions(+), 7 deletions(-) create mode 100644 packages/joystick_proxy/config/ps5.yaml diff --git a/packages/joystick_proxy/config/ps5.yaml b/packages/joystick_proxy/config/ps5.yaml new file mode 100644 index 0000000..7e83899 --- /dev/null +++ b/packages/joystick_proxy/config/ps5.yaml @@ -0,0 +1,5 @@ +velocity_axis: 1 +curvature_axis: 2 +off_button: 0 # square +remote_button: 1 # X +auto_button: 2 # O diff --git a/packages/wbb_control/wbb_control/control.py b/packages/wbb_control/wbb_control/control.py index 9ec5110..a447b83 100644 --- a/packages/wbb_control/wbb_control/control.py +++ b/packages/wbb_control/wbb_control/control.py @@ -9,8 +9,6 @@ from wbb_msgs.msg import Control -rclpy.init() - class Direction(IntEnum): FWD = 0 @@ -40,10 +38,7 @@ async def send(self, msg): ) -node = ControlNode() - - -async def loop(): +async def loop(node): await node.init_conn() while rclpy.ok(): rclpy.spin_once(node, timeout_sec=0) @@ -51,7 +46,9 @@ async def loop(): def main(): - asyncio.run(loop()) + rclpy.init() + node = ControlNode() + asyncio.run(loop(node)) if __name__ == "__main__": From de6337c9c33ca38c52b90a6c2c62e68024bac1bf Mon Sep 17 00:00:00 2001 From: enaix Date: Mon, 15 May 2023 16:04:17 +0300 Subject: [PATCH 06/10] Add missing platform property in docker-compose --- docker-compose.yaml | 5 +++-- packages/joystick_proxy/README.md | 3 +++ packages/joystick_proxy/joystick_proxy/joystick.py | 4 +++- 3 files changed, 9 insertions(+), 3 deletions(-) create mode 100644 packages/joystick_proxy/README.md diff --git a/docker-compose.yaml b/docker-compose.yaml index 83256eb..a033701 100644 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -4,6 +4,7 @@ services: wbb-amd64: container_name: wbb image: cr.yandex/crp8hpfj5tuhlaodm4dl/wbb:0.1.0-amd64 + platform: linux/x86_64 stdin_open: true privileged: true tty: true @@ -17,11 +18,11 @@ services: - "9090:9090" volumes: - "${PWD}:/wbb" - - "/dev:/dev" wbb-arm64v8: container_name: wbb image: cr.yandex/crp8hpfj5tuhlaodm4dl/wbb:0.1.0-arm64v8 + platform: linux/arm64/v8 stdin_open: true privileged: true tty: true @@ -35,4 +36,4 @@ services: - "9090:9090" volumes: - "${PWD}:/wbb" - - "/dev:/dev" \ No newline at end of file + - "/dev:/dev" diff --git a/packages/joystick_proxy/README.md b/packages/joystick_proxy/README.md new file mode 100644 index 0000000..1017ed5 --- /dev/null +++ b/packages/joystick_proxy/README.md @@ -0,0 +1,3 @@ +# Joystick proxy node + +Run `ros2 run joy joy_node` to receive events from joystick \ No newline at end of file diff --git a/packages/joystick_proxy/joystick_proxy/joystick.py b/packages/joystick_proxy/joystick_proxy/joystick.py index 19ae0c1..9589cac 100644 --- a/packages/joystick_proxy/joystick_proxy/joystick.py +++ b/packages/joystick_proxy/joystick_proxy/joystick.py @@ -28,9 +28,11 @@ def loadConfig(self): def handleJoystick(self, msg): curv = msg.axes[self.joy_map["curvature_axis"]] * self.max_curv vel = msg.axes[self.joy_map["velocity_axis"]] - if (abs(vel) < self.min_velocity): + if abs(vel) < self.min_velocity: vel = 0.0 # stop + #self.get_logger().info('curv: %f; vel: %f' % (curv, vel)) + control = Control() control.curvature = curv control.velocity = vel From d8f74a810c1ed37bf3ad79430dc64a4e901f7f8e Mon Sep 17 00:00:00 2001 From: enaix Date: Tue, 16 May 2023 13:26:57 +0300 Subject: [PATCH 07/10] Update communication protocol --- .../joystick_proxy/joystick_proxy/joystick.py | 2 +- packages/wbb_control/wbb_control/control.py | 37 +++++++++++++------ packages/wbb_msgs/CMakeLists.txt | 1 + packages/wbb_msgs/msg/Eraser.msg | 2 + 4 files changed, 29 insertions(+), 13 deletions(-) create mode 100644 packages/wbb_msgs/msg/Eraser.msg diff --git a/packages/joystick_proxy/joystick_proxy/joystick.py b/packages/joystick_proxy/joystick_proxy/joystick.py index 9589cac..a126089 100644 --- a/packages/joystick_proxy/joystick_proxy/joystick.py +++ b/packages/joystick_proxy/joystick_proxy/joystick.py @@ -10,7 +10,7 @@ class Joystick(Node): def __init__(self): super(Joystick, self).__init__('joystick') self.sub = self.create_subscription(Joy, "/joy", self.handleJoystick, 10) - self.pub = self.create_publisher(Control, "/control", 10) + self.pub = self.create_publisher(Control, "/movement", 10) # Path to the joy control map yaml file self.declare_parameter('control_map', None) diff --git a/packages/wbb_control/wbb_control/control.py b/packages/wbb_control/wbb_control/control.py index a447b83..ae80437 100644 --- a/packages/wbb_control/wbb_control/control.py +++ b/packages/wbb_control/wbb_control/control.py @@ -7,34 +7,47 @@ import websockets from rclpy.node import Node -from wbb_msgs.msg import Control +from wbb_msgs.msg import Control, Eraser -class Direction(IntEnum): - FWD = 0 - BACK = 1 - STOP = 2 +class Command(IntEnum): + MOVE = 0 + ERASER_UP = 1 + ERASER_DOWN = 2 class ControlNode(Node): def __init__(self): super(ControlNode, self).__init__("control") self.declare_parameter("esp32_ip", "192.168.1.121") - self.sub = self.create_subscription(Control, "/control", self.send, 10) + self.move_sub = self.create_subscription(Control, "/movement", self.send, 10) + self.eraser_sub = self.create_subscription(Eraser, "/eraser", self.send, 10) + self.eraser_up = True + self.lock = asyncio.Lock() async def init_conn(self): ip = self.get_parameter("esp32_ip").value self.ws = await websockets.connect("wss://" + ip + "/ws", port=80, ssl=False) async def send(self, msg): - d = Direction.STOP - if msg.velocity > 0: - d = Direction.FWD - elif msg.velocity < 0: - d = Direction.BACK + await self.ws.send( + str(int(Command.MOVE)) + ";" + str(round(float(msg.curvature), 6)) + ";" + str(round(float(msg.velocity), 6)) + ) + + async def move_eraser(self, msg): + async with self.lock: + if msg.toggle: + self.eraser_up = not self.eraser_up + else: + self.eraser_up = msg.up + + cmd = Command.ERASER_DOWN + if self.eraser_up: + cmd = Command.ERASER_UP await self.ws.send( - str(int(d)) + ";" + str(round(float(msg.curvature), 6)) + ";" + str(int(Command.MOVE)) + ";" + str(round(float(msg.curvature), 6)) + ";" + str( + round(float(msg.velocity), 6)) ) diff --git a/packages/wbb_msgs/CMakeLists.txt b/packages/wbb_msgs/CMakeLists.txt index e66b9cc..fdefb47 100644 --- a/packages/wbb_msgs/CMakeLists.txt +++ b/packages/wbb_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/ImageMarkerPos.msg msg/ImageMarkerPosArray.msg msg/Control.msg + msg/Eraser.msg DEPENDENCIES builtin_interfaces geometry_msgs diff --git a/packages/wbb_msgs/msg/Eraser.msg b/packages/wbb_msgs/msg/Eraser.msg new file mode 100644 index 0000000..797b3fc --- /dev/null +++ b/packages/wbb_msgs/msg/Eraser.msg @@ -0,0 +1,2 @@ +bool toggle +bool up \ No newline at end of file From f1fc238da3f06e160ca37b974884183e5ec06dc5 Mon Sep 17 00:00:00 2001 From: enaix Date: Mon, 29 May 2023 19:59:05 +0300 Subject: [PATCH 08/10] Bind eraser to the controller button --- packages/joystick_proxy/joystick_proxy/joystick.py | 11 ++++++++++- packages/wbb_control/wbb_control/control.py | 5 +---- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/packages/joystick_proxy/joystick_proxy/joystick.py b/packages/joystick_proxy/joystick_proxy/joystick.py index a126089..845bb3d 100644 --- a/packages/joystick_proxy/joystick_proxy/joystick.py +++ b/packages/joystick_proxy/joystick_proxy/joystick.py @@ -2,7 +2,7 @@ from rclpy.node import Node from yaml import load, Loader -from wbb_msgs.msg import Control +from wbb_msgs.msg import Control, Eraser from sensor_msgs.msg import Joy @@ -11,11 +11,13 @@ def __init__(self): super(Joystick, self).__init__('joystick') self.sub = self.create_subscription(Joy, "/joy", self.handleJoystick, 10) self.pub = self.create_publisher(Control, "/movement", 10) + self.eraser_pub = self.create_publisher(Eraser, "/eraser", 10) # Path to the joy control map yaml file self.declare_parameter('control_map', None) self.declare_parameter('max_curvature', 2.0) self.declare_parameter('min_velocity', 0.1) + self.btn_state = 0 self.max_curv = self.get_parameter('max_curvature').value self.min_velocity = self.get_parameter('min_velocity').value @@ -38,6 +40,13 @@ def handleJoystick(self, msg): control.velocity = vel self.pub.publish(control) + btn = msg.axes[self.joy_map["off_button"]] + if not btn == self.btn_state: + er = Eraser() + er.toggle = True + self.eraser_pub.publish(er) + self.btn_state = btn + def main(): rclpy.init() joy = Joystick() diff --git a/packages/wbb_control/wbb_control/control.py b/packages/wbb_control/wbb_control/control.py index ae80437..4b5cc96 100644 --- a/packages/wbb_control/wbb_control/control.py +++ b/packages/wbb_control/wbb_control/control.py @@ -45,10 +45,7 @@ async def move_eraser(self, msg): if self.eraser_up: cmd = Command.ERASER_UP - await self.ws.send( - str(int(Command.MOVE)) + ";" + str(round(float(msg.curvature), 6)) + ";" + str( - round(float(msg.velocity), 6)) - ) + await self.ws.send(str(int(cmd)) + ";") async def loop(node): From 15b1d179c584c35c236e40c63330a4de65375d5c Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 7 Jun 2023 15:11:11 +0300 Subject: [PATCH 09/10] Add launch file to share folder --- packages/wbb_control/setup.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/packages/wbb_control/setup.py b/packages/wbb_control/setup.py index c4dcb9a..d94b7d6 100644 --- a/packages/wbb_control/setup.py +++ b/packages/wbb_control/setup.py @@ -1,4 +1,5 @@ from setuptools import setup +from glob import glob package_name = 'wbb_control' @@ -10,6 +11,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + ('share/' + package_name, glob('launch/*.yaml')), ], install_requires=['setuptools', 'websockets'], zip_safe=True, From 67858bfe272178df701c4e4bce6bfac139bee20b Mon Sep 17 00:00:00 2001 From: enaix Date: Wed, 7 Jun 2023 15:12:10 +0300 Subject: [PATCH 10/10] Add joystick launch file to setup --- packages/joystick_proxy/setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/packages/joystick_proxy/setup.py b/packages/joystick_proxy/setup.py index 163b390..1415c4b 100644 --- a/packages/joystick_proxy/setup.py +++ b/packages/joystick_proxy/setup.py @@ -12,7 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ('share/' + package_name, glob('launch/*.yaml')), ], install_requires=['setuptools'], zip_safe=True,