diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index af5a8df..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -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 -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 593784e..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -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" - ] -} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 628c209..0013d74 100644 --- a/CMakeLists.txt +++ b/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( 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() diff --git a/package.xml b/package.xml index cb21106..25e798a 100644 --- a/package.xml +++ b/package.xml @@ -7,12 +7,12 @@ bjorn TODO: License declaration - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_cmake + + ament_lint_auto + ament_lint_common - ament_python + ament_cmake diff --git a/resource/rmp220_middleware b/resource/rmp220_middleware deleted file mode 100644 index e69de29..0000000 diff --git a/rmp220_middleware/__init__.py b/rmp220_middleware/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rmp220_middleware/__pycache__/__init__.cpython-310.pyc b/rmp220_middleware/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index e09dd66..0000000 Binary files a/rmp220_middleware/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc b/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc deleted file mode 100644 index 2ba5da2..0000000 Binary files a/rmp220_middleware/__pycache__/rmp220_middleware.cpython-310.pyc and /dev/null differ diff --git a/rmp220_middleware/rmp220_middleware copy.py b/rmp220_middleware/rmp220_middleware copy.py deleted file mode 100644 index 7f0d883..0000000 --- a/rmp220_middleware/rmp220_middleware copy.py +++ /dev/null @@ -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() diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py deleted file mode 100644 index 595c6ce..0000000 --- a/rmp220_middleware/rmp220_middleware.py +++ /dev/null @@ -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() diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index c75dd66..0000000 --- a/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rmp220_middleware -[install] -install_scripts=$base/lib/rmp220_middleware diff --git a/setup.py b/setup.py deleted file mode 100644 index d1e0099..0000000 --- a/setup.py +++ /dev/null @@ -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' - ], - }, -) diff --git a/src/rmp220_middleware.cpp b/src/rmp220_middleware.cpp new file mode 100644 index 0000000..5d8a42b --- /dev/null +++ b/src/rmp220_middleware.cpp @@ -0,0 +1,114 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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("/cmd_vel_out", 10); + cmd_vel_sub = create_subscription( + "/cmd_vel_mux", 10, + std::bind(&StateMachineNode::cmd_vel_callback, this, std::placeholders::_1) + ); + joy_sub = create_subscription( + "/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("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::SharedPtr cmd_vel_pub; + rclcpp::Subscription::SharedPtr cmd_vel_sub; + rclcpp::Subscription::SharedPtr joy_sub; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Client::SharedPtr chassis_enable_client; + + void enable_chassis() { + auto request = std::make_shared(); + 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(); + 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(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/test/test_copyright.py b/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/test/test_copyright.py +++ /dev/null @@ -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' diff --git a/test/test_flake8.py b/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/test/test_flake8.py +++ /dev/null @@ -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) diff --git a/test/test_pep257.py b/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/test/test_pep257.py +++ /dev/null @@ -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'