update

.vscode/c_cpp_properties.json

@@ -1,20 +0,0 @@
-{
-  "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

@@ -1,10 +0,0 @@
-{
-    "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"
-    ]
-}

CMakeLists.txt

@@ -10,6 +10,11 @@ find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(segway_msgs REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -23,4 +28,9 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+add_executable(rmp220_middleware rmp220_middleware.cpp)
+ament_target_dependencies(state_machine_node rclcpp std_msgs geometry_msgs sensor_msgs segway_msgs)
+
+install(TARGETS rmp220_middleware DESTINATION lib/${PROJECT_NAME})
+
 ament_package()

package.xml

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

rmp220_middleware/rmp220_middleware copy.py

@@ -1,96 +0,0 @@
-#!/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

@@ -1,104 +0,0 @@
-#!/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.01, self.timer_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.')
-
-    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 msg.buttons[7] == 1:  # Joystick button 'start'
-        if 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'
-        if 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):
-        # This method shall only update the latest_cmd_vel attribute so it can be republished by the timer_callback with 100 HZ. Should have a look at performance though.
-        self.latest_cmd_vel = msg
-        self.timeout = 20.0  # Reset timeout when receiving commands
-
-    def timer_callback(self):
-        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.01
-                self.cmd_vel_pub.publish(self.latest_cmd_vel)
-        if self.state == State.DISABLED and (abs(self.latest_cmd_vel.linear) > 0.1 or abs(self.latest_cmd_vel.angular > 0.1)): # This is a hack to enable the chassis when receiving commands e.g. from Nav2
-            self.state = State.ENABLED
-            self.get_logger().info("State: ENABLED (cmd_vel)")
-            self.enable_chassis()
-        else:
-            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()
-
-if __name__ == '__main__':
-    main()

setup.cfg

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

setup.py

@@ -1,26 +0,0 @@
-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'
-        ],
-    },
-)

src/rmp220_middleware.cpp

@@ -0,0 +1,114 @@
+#include <memory>
+#include <chrono>
+#include <cmath>
+#include <signal.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+#include <segway_msgs/srv/ros_set_chassis_enable_cmd.hpp>
+
+using namespace std::chrono_literals;
+
+enum class State {
+    DISABLED,
+    ENABLED
+};
+
+class StateMachineNode : public rclcpp::Node {
+public:
+    StateMachineNode() : Node("state_machine_node") {
+        state = State::DISABLED;
+        timeout = 20.0;
+
+        cmd_vel_pub = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_out", 10);
+        cmd_vel_sub = create_subscription<geometry_msgs::msg::Twist>(
+            "/cmd_vel_mux", 10,
+            std::bind(&StateMachineNode::cmd_vel_callback, this, std::placeholders::_1)
+        );
+        joy_sub = create_subscription<sensor_msgs::msg::Joy>(
+            "/joy", 10,
+            std::bind(&StateMachineNode::joy_callback, this, std::placeholders::_1)
+        );
+
+        timer = create_wall_timer(10ms, std::bind(&StateMachineNode::timer_callback, this));
+
+        chassis_enable_client = create_client<segway_msgs::srv::RosSetChassisEnableCmd>("set_chassis_enable");
+        while (!chassis_enable_client->wait_for_service(1s)) {
+            RCLCPP_INFO(get_logger(), "Service not available, waiting for chassis enable service...");
+        }
+        RCLCPP_INFO(get_logger(), "Chassis enable service available.");
+    }
+
+private:
+    State state;
+    double timeout;
+    geometry_msgs::msg::Twist twist;
+    geometry_msgs::msg::Twist latest_cmd_vel;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub;
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
+    rclcpp::TimerBase::SharedPtr timer;
+    rclcpp::Client<segway_msgs::srv::RosSetChassisEnableCmd>::SharedPtr chassis_enable_client;
+
+    void enable_chassis() {
+        auto request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
+        request->ros_set_chassis_enable_cmd = true;
+        auto future = chassis_enable_client->async_send_request(request);
+        RCLCPP_INFO(get_logger(), "Enabling chassis...");
+    }
+
+    void disable_chassis() {
+        auto request = std::make_shared<segway_msgs::srv::RosSetChassisEnableCmd::Request>();
+        request->ros_set_chassis_enable_cmd = false;
+        auto future = chassis_enable_client->async_send_request(request);
+        RCLCPP_INFO(get_logger(), "Disabling chassis...");
+    }
+
+    void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) {
+        if (msg->buttons[7] == 1) {  // Joystick button 'start'
+            state = State::ENABLED;
+            RCLCPP_INFO(get_logger(), "State: ENABLED (Button 'start')");
+            enable_chassis();
+        }
+        if (msg->buttons[6] == 1) {  // Joystick button 'select'
+            state = State::DISABLED;
+            RCLCPP_INFO(get_logger(), "State: DISABLED (Button 'select')");
+            disable_chassis();
+        }
+    }
+
+    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+        latest_cmd_vel = *msg;
+        timeout = 20.0;
+    }
+
+    void timer_callback() {
+        if (state == State::ENABLED) {
+            if (timeout <= 0) {
+                state = State::DISABLED;
+                RCLCPP_INFO(get_logger(), "State: DISABLED (Timeout)");
+                disable_chassis();
+            } else {
+                timeout -= 0.01;
+                cmd_vel_pub->publish(latest_cmd_vel);
+            }
+        }
+        if (state == State::DISABLED &&
+            (std::abs(latest_cmd_vel.linear.x) > 0.1 || std::abs(latest_cmd_vel.angular.z) > 0.1)) {
+            state = State::ENABLED;
+            RCLCPP_INFO(get_logger(), "State: ENABLED (cmd_vel)");
+            enable_chassis();
+        } else {
+            cmd_vel_pub->publish(twist);
+        }
+    }
+};
+
+int main(int argc, char * argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<StateMachineNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

test/test_copyright.py

@@ -1,25 +0,0 @@
-# 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

@@ -1,25 +0,0 @@
-# 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

@@ -1,23 +0,0 @@
-# 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'