works but needs improvement

.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
+}

.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"
+    ]
+}

package.xml

@@ -7,12 +7,12 @@
   <maintainer email="bjoern.ellensohn@gmail.com">bjorn</maintainer>
   <license>TODO: License declaration</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <test_depend>ament_copyright</test_depend>
-
+  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_pep257</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <test_depend>python3-pytest</test_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

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()

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()

setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rmp220_middleware
+[install]
+install_scripts=$base/lib/rmp220_middleware

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'
+        ],
+    },
+)

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'

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)

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'