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/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/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/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..845bb3d --- /dev/null +++ b/packages/joystick_proxy/joystick_proxy/joystick.py @@ -0,0 +1,57 @@ +import rclpy +from rclpy.node import Node +from yaml import load, Loader + +from wbb_msgs.msg import Control, Eraser +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, "/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 + 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 + + #self.get_logger().info('curv: %f; vel: %f' % (curv, vel)) + + control = Control() + control.curvature = curv + 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() + 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/resource/joystick_proxy b/packages/joystick_proxy/resource/joystick_proxy new file mode 100644 index 0000000..e69de29 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..1415c4b --- /dev/null +++ b/packages/joystick_proxy/setup.py @@ -0,0 +1,30 @@ +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')), + ('share/' + package_name, glob('launch/*.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 new file mode 100644 index 0000000..f39ce4e --- /dev/null +++ b/packages/wbb_control/package.xml @@ -0,0 +1,21 @@ + + + + wbb_control + 0.0.0 + Whiteboard bot controller node + enaix + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + wbb_msgs + python3-websockets + rclpy + + + ament_python + + diff --git a/packages/wbb_control/resource/wbb_control b/packages/wbb_control/resource/wbb_control new file mode 100644 index 0000000..e69de29 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..d94b7d6 --- /dev/null +++ b/packages/wbb_control/setup.py @@ -0,0 +1,28 @@ +from setuptools import setup +from glob import glob + +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']), + ('share/' + package_name, glob('launch/*.yaml')), + ], + install_requires=['setuptools', 'websockets'], + zip_safe=True, + maintainer='enaix', + maintainer_email='enaix@protonmail.com', + description='Whiteboard bot control node', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'control = wbb_control.control:main' + ], + }, +) 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/__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..4b5cc96 --- /dev/null +++ b/packages/wbb_control/wbb_control/control.py @@ -0,0 +1,65 @@ +#!/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, Eraser + + +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.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): + 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(cmd)) + ";") + + +async def loop(node): + await node.init_conn() + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0) + await asyncio.sleep(0.0001) + + +def main(): + rclpy.init() + node = ControlNode() + asyncio.run(loop(node)) + + +if __name__ == "__main__": + main() diff --git a/packages/wbb_msgs/CMakeLists.txt b/packages/wbb_msgs/CMakeLists.txt index 4dcd60f..fdefb47 100644 --- a/packages/wbb_msgs/CMakeLists.txt +++ b/packages/wbb_msgs/CMakeLists.txt @@ -7,14 +7,19 @@ 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 + msg/Eraser.msg DEPENDENCIES builtin_interfaces + geometry_msgs ADD_LINTER_TESTS ) @@ -25,4 +30,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/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 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 +