From a136b12225e0f798012efb8bc0b650ca8d660464 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Mon, 14 Aug 2023 12:46:00 +0200 Subject: [PATCH] works but needs improvement --- .vscode/c_cpp_properties.json | 20 +++ .vscode/settings.json | 10 ++ package.xml | 10 +- resource/rmp220_middleware | 0 rmp220_middleware/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 199 bytes .../rmp220_middleware.cpython-310.pyc | Bin 0 -> 3842 bytes rmp220_middleware/rmp220_middleware copy.py | 96 ++++++++++++ rmp220_middleware/rmp220_middleware.py | 147 ++++++++++++++++++ setup.cfg | 4 + setup.py | 26 ++++ test/test_copyright.py | 25 +++ test/test_flake8.py | 25 +++ test/test_pep257.py | 23 +++ 14 files changed, 381 insertions(+), 5 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 resource/rmp220_middleware create mode 100644 rmp220_middleware/__init__.py create mode 100644 rmp220_middleware/__pycache__/__init__.cpython-310.pyc create mode 100644 rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc create mode 100644 rmp220_middleware/rmp220_middleware copy.py create mode 100644 rmp220_middleware/rmp220_middleware.py create mode 100644 setup.cfg create mode 100644 setup.py create mode 100644 test/test_copyright.py create mode 100644 test/test_flake8.py create mode 100644 test/test_pep257.py diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..af5a8df --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,20 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/humble/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..593784e --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,10 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} \ No newline at end of file diff --git a/package.xml b/package.xml index 25e798a..cb21106 100644 --- a/package.xml +++ b/package.xml @@ -7,12 +7,12 @@ bjorn TODO: License declaration - ament_cmake - - ament_lint_auto - ament_lint_common + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest - ament_cmake + ament_python diff --git a/resource/rmp220_middleware b/resource/rmp220_middleware new file mode 100644 index 0000000..e69de29 diff --git a/rmp220_middleware/__init__.py b/rmp220_middleware/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rmp220_middleware/__pycache__/__init__.cpython-310.pyc b/rmp220_middleware/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e09dd6603f25d8e020c26b8aaba47c5af4e18eda GIT binary patch literal 199 zcmd1j<>g`kf+Zr?Qb6=$5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;x_kenx(7s(w;d zeo>yjOMY@`Zfaghv3^m0aeP5hepYHSh?84jWMmMZo0*c5lUklw1XNy{nUjJ=98+a{ gd}dx|NqoFsLFFwDo80`A(wtN~kQ<7bfCLKz08$w=!vFvP literal 0 HcmV?d00001 diff --git a/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc b/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2ba5da2ca496dd6d1e476f017fdb56494fd0bb59 GIT binary patch literal 3842 zcmaJ^&2JmW6`$E%l1p+)Q6E-qrGaT6t+QxKwQk_LMi8sE95?;o!Ezgvhb}12np$eP zE6*$~i%?E7kW2FiwCF(sdZ=%`7U+M`+g@`BbSscs(Zv0|*(F7aPM4TBGw;pkd++z& zFs{`q4BuZ1sd#*au`j4I`&sC0p{T!s2qt*QI#e4W9~vElQ`-#9q1CZ;+X@TAVyCFv zg|IZVJGO2Y!}74wspxhotPY)yquV^Jb!vLudZ(`IrOwhLCTvlD$wb+&oS2;k`V~<{ zzv>(4FN-DNh}uiuS@BmzeczCe{go4j_4tl1ZsQ!MWo?(IRy!6x)%W8#L|u4t7^nmd z^Ox}v@gGK`A?o$VvD)#I2YX$m0`)NJ?uPz@q3F>%vmZxi3q`Gg_^iVP>llJ}Oks2^ zVeT;3N((zlH}NG6Jtxw33#sjSQFrKjURv?IVJt==wH?oUHtL4C#NPgR=jZo-{ct-i zJ#6d7D<+q)fF}P@n|txl-`pLcyVeQFzIqL|OvQ!cnLUfNKQDXfVhZ<8+B2XMFn+9RE2}u{)}0yQ-&gIX+taWH{IS|5cy=t9;tKkQJYRhJF9xOJzC-uB;{(TSrUQL3bkCi;-_r@RW`0{zs^P5qeG`r8cK}hcZUh}XUBtg_~ zK8k%Q)v=whLkyO9>u=)ji*gCGhL&(lsMYd8@^bnUd1pD1ng zq)&P{8106E+ViEPAe2@o_Xj^+&Bv(GuIkC)APM3K6LSsqx?#B6?d@O8!8lo*Tt?TMySRrE<5{eyZJ;do0l$-6A*7CW%FaRKBiMF{iuX!(%r47=k z2X#X2TH|S@4>N^vzmILLAbJ|VLQGPA_!M>7!#uHR z*7zE4aNDwtx>@J=y})q5ZbKy=1F1(1%p2e%)Xr`E9s z72LJ}mo5rp>b7S4$#_+VOLCO_*6@l~rA!f}$NsYsw3Tj9oLclr(xP;+Yg!?zcwKcA z_2du0kaU(;hHR10l>W1eP$)?gX+3kriIcw%-d7Z^Y+mFJU*Y5H3ksN@@vUl*KgKey z+5Tv}wjC&7Vg7FN7R2&4k!f_!j+>tZ;i_o|T2D)7g0zMn2K= z3i-U>(Fx}EE9Udcv>%1BNpVZ<-2d)2^IX<^7a3P2L2utZCydB4NN%HV5=YIO z2nsT}>7G-BAK%d0yW5;3$mYA#VSb3jchhw%a)TBn2hd?fHmOx7GW8~T2i>%^tG7^+ ztm94`Y^eH|tSN6mzJ;P_<#lAfI?Bde4FG;UetW^Tvq5IgumrRT*wkOpnK9B&$*>cs z0eqCV_@Fo_2{QZl$P|Ne)=nxX?9&M}H5enxTNkq_%Tac_%*ocX_L+K+uZOn$#WC$x zCF9Y`leSxE%T=5)&l}UmJ6rvq=vlhXaTg`Bh#t>fAvLq;l1$aL34e^KA0;mqML++2qpGK9QGgmf&wPvnP0=3|z8 z2RwnIU$l-{(f(cGby6&%sP*~{adbXE0(uCb(UNda8BF~VBw+(MwBVoXhajNB z$D894@|v0Du}@f3wpd~vZE(ab&VJ9L#v%KJ9dnAKh@AYiNHu0?>ynlJzx3N?9wNZ# zzad@OSFW8FWG_58qD&to`XWg72SJoB|H?nwjk{8O9O0jX933QDbEzYcJ0%_|^cYNS zwKqycd>A2WeS~|a7O^iGG_{7^Ad;jDGA+TxLgaEljn~F&JQC10{%adl+uzu{!X?3q z-exq@^IYC`*W@GORrGmVdK*%6>Zn8zI8}=fO1Mdv`#%1cB=YEjuODnn%8wMDI@8hm z0cQF5!zC%Irx?&b?B>&ye4f1+ig;bU2$Hm@f_~HuQ&SzOEUc5^^-(i-yYOS((r*I$ igY3W6hlFa?1hEYY6_cNB+VJm&(Xc9iu$vWl_x}LMdZ#P^ literal 0 HcmV?d00001 diff --git a/rmp220_middleware/rmp220_middleware copy.py b/rmp220_middleware/rmp220_middleware copy.py new file mode 100644 index 0000000..7f0d883 --- /dev/null +++ b/rmp220_middleware/rmp220_middleware copy.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 + +import rclpy +from enum import Enum +from rclpy.node import Node +from std_msgs.msg import Bool +from geometry_msgs.msg import Twist +#from sensor_msgs.msg import Joy +from segway_msgs.srv import RosSetChassisEnableCmd + +class State(Enum): + DISABLED = 0 + ENABLED = 1 + +class StateMachineNode(Node): + def __init__(self): + super().__init__('state_machine_node') + + # Initialize state and other variables + self.state = State.DISABLED + self.timeout = 2.0 # Timeout in seconds + + # Create publishers, subscribers, timers, and service clients here + self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10) + self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10) + #self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) + self.timer = self.create_timer(0.1, self.timer_callback) + + # Create service clients for chassis enable and disable + self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') + while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service not available, waiting for chassis enable service...') + self.chassis_disable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') + while not self.chassis_disable_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service not available, waiting for chassis disable service...') + + def joy_callback(self, msg): + # Implement logic to detect joystick button presses (start/select) and update state + # ... + if msg.buttons[7] == 1: # Joystick button 'start' + self.state = State.ENABLED + self.get_logger().info("State: ENABLED (Button 'start')") + self.enable_chassis() + if msg.buttons[6] == 1: # Joystick button 'select' + self.state = State.DISABLED + self.get_logger().info("State: DISABLED (Button 'select')") + self.disable_chassis() + + def enable_chassis(self): + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = True + self.chassis_enable_client.call_async(req) + + def disable_chassis(self): + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = False + self.chassis_disable_client.call_async(req) + + def cmd_vel_callback(self, msg): + # Update state to ENABLED upon receiving a command on /cmd_vel_mux + # ... + if self.state == State.ENABLED: + self.cmd_vel_pub.publish(msg) + self.timeout = 2.0 # Reset timeout when receiving commands + + def timer_callback(self): + # Republish the cmd_vel_mux command to cmd_vel_out topic + # ... + + # Reset the timeout counter + # ... + + # Check if the timeout has been exceeded, and if so, switch to DISABLED + # ... + + if self.state == State.ENABLED: + if self.timeout <= 0: + self.state = State.DISABLED + self.get_logger().info("State: DISABLED (Timeout)") + self.disable_chassis() + else: + self.timeout -= 0.1 + +def main(args=None): + rclpy.init(args=args) + node = StateMachineNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py new file mode 100644 index 0000000..30bb58b --- /dev/null +++ b/rmp220_middleware/rmp220_middleware.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Joy +from enum import Enum +from segway_msgs.srv import RosSetChassisEnableCmd + + +import atexit +import signal +import sys + +class State(Enum): + DISABLED = 0 + ENABLED = 1 + +class StateMachineNode(Node): + def __init__(self): + super().__init__('state_machine_node') + + # Initialize state and other variables + self.state = State.DISABLED + self.timeout = 20.0 # Timeout in seconds + #self.limit = 0.5 # Limit for linear and angular velocity + + # Create publishers, subscribers, timers, and service clients + self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10) + self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10) + self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) + self.timer = self.create_timer(0.1, self.timer_callback) + #self.timer = self.create_timer(0.1, self.cmd_vel_callback) + + # Create twist class for publishing velocities + self.twist = Twist() + + self.latest_cmd_vel = Twist() + + # Create service clients for chassis enable and disable + self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') + while not self.chassis_enable_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Service not available, waiting for chassis enable service...') + self.get_logger().info('Chassis enable service available.') + + # self.chassis_disable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_disable') + # while not self.chassis_disable_client.wait_for_service(timeout_sec=1.0): + # self.get_logger().info('Service not available, waiting for chassis disable service...') + # self.get_logger().info('Chassis disable service available.') + + def enable_chassis(self): + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = True + self.chassis_enable_client.call_async(req) + self.get_logger().info('Enabling chassis...') + + def disable_chassis(self): + req = RosSetChassisEnableCmd.Request() + req.ros_set_chassis_enable_cmd = False + self.chassis_enable_client.call_async(req) + self.get_logger().info('Disabling chassis...') + + def joy_callback(self, msg): + if self.state == State.DISABLED and self.cmd_vel_sub is not None: + self.state = State.ENABLED + self.get_logger().info("State: ENABLED (Joystick)") + self.enable_chassis() + if self.state == State.DISABLED and msg.buttons[7] == 1: # Joystick button 'start' + self.state = State.ENABLED + self.get_logger().info("State: ENABLED (Button 'start')") + self.enable_chassis() + if self.state == State.ENABLED and msg.buttons[6] == 1: # Joystick button 'select' + self.state = State.DISABLED + self.get_logger().info("State: DISABLED (Button 'select')") + self.disable_chassis() + + def cmd_vel_callback(self, msg): + if self.state == State.ENABLED: + self.cmd_vel_pub.publish(msg) + self.timeout = 20.0 # Reset timeout when receiving commands + if msg.linear.x == 0.0 and msg.angular.z == 0.0: + # No data received on cmd_vel_mux, publish zeros + twist_msg = Twist() + self.cmd_vel_pub.publish(twist_msg) + else: + # Forward received data to cmd_vel_out + self.cmd_vel_pub.publish(msg) + self.timeout = 10.0 # Reset timeout when receiving commands + + # Forward received data to cmd_vel_out + self.cmd_vel_pub.publish(msg) + self.timeout = 10.0 # Reset timeout when receiving commands + + if msg.linear.x < 0.3 or msg.angular.z < 0.3: + self.latest_cmd_vel = msg + else: + self.latest_cmd_vel = Twist() + + def timer_callback(self): + #self.cmd_vel_pub.publish(self.twist) + #self.cmd_vel_callback(self.cmd_vel_sub) + if self.state == State.ENABLED: + if self.timeout <= 0: + self.state = State.DISABLED + self.get_logger().info("State: DISABLED (Timeout)") + self.disable_chassis() + else: + self.timeout -= 0.1 + self.cmd_vel_pub.publish(self.latest_cmd_vel) + if self.state == State.DISABLED: + self.cmd_vel_pub.publish(self.twist) + +def main(args=None): + rclpy.init(args=args) + node = StateMachineNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.disable_chassis() + node.destroy_node() + rclpy.shutdown() + +# def main(args=None): +# rclpy.init(args=args) +# node = StateMachineNode() + +# # Register a signal handler for proper cleanup +# def cleanup_handler(signum, frame): +# node.disable_chassis() +# rclpy.shutdown() +# sys.exit(0) + +# signal.signal(signal.SIGINT, cleanup_handler) +# atexit.register(cleanup_handler) # Ensure cleanup on normal exit as well + +# try: +# rclpy.spin(node) +# except KeyboardInterrupt: +# pass +# finally: +# pass + +if __name__ == '__main__': + main() diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..c75dd66 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmp220_middleware +[install] +install_scripts=$base/lib/rmp220_middleware diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..d1e0099 --- /dev/null +++ b/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'rmp220_middleware' + +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': [ + 'rmp220_middleware = rmp220_middleware.rmp220_middleware:main' + ], + }, +) 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'