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 0000000..e09dd66 Binary files /dev/null and b/rmp220_middleware/__pycache__/__init__.cpython-310.pyc differ 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 0000000..2ba5da2 Binary files /dev/null and b/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc differ 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'