From 65ef4f630d2520094c426c89b3f50481855676e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bj=C3=B6rn=20Ellensohn?= Date: Wed, 15 Jan 2025 18:41:33 +0100 Subject: [PATCH] update: noetic version --- CMakeLists.txt | 43 ++++--- package.xml | 28 +++-- rmp220_middleware/__init__.py | 0 rmp220_middleware/rmp220_middleware.py | 167 +++++++++++-------------- setup.cfg | 4 - setup.py | 26 ---- test/test_copyright.py | 25 ---- test/test_flake8.py | 25 ---- test/test_pep257.py | 23 ---- 9 files changed, 116 insertions(+), 225 deletions(-) delete mode 100644 rmp220_middleware/__init__.py delete mode 100644 setup.cfg delete mode 100644 setup.py delete mode 100644 test/test_copyright.py delete mode 100644 test/test_flake8.py delete mode 100644 test/test_pep257.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 628c209..8e9a3d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,26 +1,33 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.0.2) project(rmp220_middleware) +# Set compiler options for warnings if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +# Find catkin and required components +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + sensor_msgs + segway_msgs +) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() +# Declare catkin package +catkin_package( + CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs segway_msgs +) + +# Install Python scripts +catkin_install_python(PROGRAMS + rmp220_middleware/rmp220_middleware.py # Replace with the actual script path + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Testing (optional) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + # Add tests here if needed endif() - -ament_package() diff --git a/package.xml b/package.xml index cb21106..2f0d4b4 100644 --- a/package.xml +++ b/package.xml @@ -1,18 +1,28 @@ - - + rmp220_middleware 0.0.0 - TODO: Package description - bjorn + Middleware package for controlling the RMP220 chassis in ROS 1. + + Bjorn Ellensohn TODO: License declaration - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + + catkin + rospy + std_msgs + geometry_msgs + sensor_msgs + segway_msgs + rospy + std_msgs + geometry_msgs + sensor_msgs + segway_msgs + + - ament_python + catkin diff --git a/rmp220_middleware/__init__.py b/rmp220_middleware/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py index 5904c06..7dce0e4 100644 --- a/rmp220_middleware/rmp220_middleware.py +++ b/rmp220_middleware/rmp220_middleware.py @@ -1,7 +1,6 @@ -#!/usr/bin/env python3 +#!/usr/bin/env python -import rclpy -from rclpy.node import Node +import rospy from std_msgs.msg import Bool from geometry_msgs.msg import Twist from sensor_msgs.msg import Joy @@ -9,146 +8,124 @@ from enum import Enum from segway_msgs.srv import RosSetChassisEnableCmd from segway_msgs.msg import ChassisModeFb +class State(Enum): + DISABLED = 0 # solid yellow + ENABLED = 1 # solid green + PASSIVE = 2 # solid white (push) + STOPPED = 3 # solid red + PAUSED = 4 # no extra visual feedback, solid yellow -import atexit -import signal -import sys - -class State(Enum): # is this best-practice? - DISABLED = 0 # solid yellow - ENABLED = 1 # solid green - PASSIVE = 2 # solid white (push) - STOPPED = 3 # solid red - PAUSED = 4 # no extra visual feedback, solid yellow - -class StateMachineNode(Node): +class StateMachineNode: def __init__(self): - super().__init__('state_machine_node') + rospy.init_node('state_machine_node', anonymous=True) # Initialize state and other variables - self.state = State.DISABLED # is this necessary? + self.state = State.DISABLED self.timeout = 20.0 # Timeout in seconds - # Create twist class for publishing velocities self.twist = Twist() - self.latest_cmd_vel = Twist() self.abs_x = 0.0 self.abs_z = 0.0 - #self.limit = 0.5 # Limit for linear and angular velocity + # Publishers and subscribers + self.cmd_vel_pub = rospy.Publisher('/cmd_vel_out', Twist, queue_size=10) + self.cmd_vel_sub = rospy.Subscriber('/cmd_vel_mux', Twist, self.cmd_vel_callback) + self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback) + self.chassis_mode_sub = rospy.Subscriber('/chassis_mode_fb', ChassisModeFb, self.chassis_mode_callback) - # 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) # changed to 100 Hz for now + # Service clients + rospy.wait_for_service('set_chassis_enable') + self.chassis_enable_client = rospy.ServiceProxy('set_chassis_enable', RosSetChassisEnableCmd) + rospy.loginfo('Chassis enable service available.') - # 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.') - - # Create a subscriber for the chassis status topic - self.chassis_mode_sub = self.create_subscription( - ChassisModeFb, - '/chassis_mode_fb', - self.chassis_mode_callback, - 10 - ) + # Timer for periodic updates + self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback) # 100 Hz def chassis_mode_callback(self, msg): - # Need to evaluate that for possible errors - # Handle the incoming chassis status message, will update every second - if self.state == State.PAUSED: # trying to save processing time by skipping directly if PAUSED + if self.state == State.PAUSED: return - else: - if msg.chassis_mode == 0: - self.state = State.DISABLED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 1: # Assuming 1 represents enabled and 0 represents disabled - self.state = State.ENABLED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 2: - self.state = State.PASSIVE - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) - if msg.chassis_mode == 3: - self.state = State.STOPPED - self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) + + if msg.chassis_mode == 0: + self.state = State.DISABLED + rospy.loginfo('Set chassis_mode to DISABLED') + elif msg.chassis_mode == 1: + self.state = State.ENABLED + rospy.loginfo('Set chassis_mode to ENABLED') + elif msg.chassis_mode == 2: + self.state = State.PASSIVE + rospy.loginfo('Set chassis_mode to PASSIVE') + elif msg.chassis_mode == 3: + self.state = State.STOPPED + rospy.loginfo('Set chassis_mode to STOPPED') def enable_chassis(self): - req = RosSetChassisEnableCmd.Request() - req.ros_set_chassis_enable_cmd = True - self.chassis_enable_client.call_async(req) - self.state = State.ENABLED - self.get_logger().info('Enabling chassis...') + try: + self.chassis_enable_client(True) + self.state = State.ENABLED + rospy.loginfo('Enabling chassis...') + except rospy.ServiceException as e: + rospy.logerr(f'Failed to enable chassis: {e}') def pause_chassis(self): - req = RosSetChassisEnableCmd.Request() - req.ros_set_chassis_enable_cmd = False - self.chassis_enable_client.call_async(req) - self.state = State.PAUSED - self.get_logger().info('Pausing chassis...') + try: + self.chassis_enable_client(False) + self.state = State.PAUSED + rospy.loginfo('Pausing chassis...') + except rospy.ServiceException as e: + rospy.logerr(f'Failed to pause chassis: {e}') def disable_chassis(self): - req = RosSetChassisEnableCmd.Request() - req.ros_set_chassis_enable_cmd = False - self.chassis_enable_client.call_async(req) - self.state = State.DISABLED - self.get_logger().info('Disabling chassis...') + try: + self.chassis_enable_client(False) + self.state = State.DISABLED + rospy.loginfo('Disabling chassis...') + except rospy.ServiceException as e: + rospy.logerr(f'Failed to disable chassis: {e}') def joy_callback(self, msg): - start_button = msg.buttons[7] # Joystick button 'start' - select_button = msg.buttons[6] # Joystick button 'select' + start_button = msg.buttons[7] # Joystick button 'start' + select_button = msg.buttons[6] # Joystick button 'select' if start_button == 1: - self.get_logger().info("State: ENABLED (Button 'start')") + rospy.loginfo("State: ENABLED (Button 'start')") self.enable_chassis() self.timeout = 20 + if select_button == 1: - self.get_logger().info("State: DISABLED (Button 'select')") + rospy.loginfo("State: DISABLED (Button 'select')") self.pause_chassis() def cmd_vel_callback(self, msg): - # Should be called everytime a new cmd_vel is received on /cmd_vel_mux - # 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 - - # Will also derive the absolute values of the linear and angular velocity self.abs_x = abs(msg.linear.x) self.abs_z = abs(msg.angular.z) - - # Reset timeout when receiving commands self.timeout = 20.0 - def timer_callback(self): - if self.state == State.PAUSED or self.state == State.STOPPED or self.state == State.PASSIVE: - return # Do nothing if chassis is disabled, stopped or passive --> should save processing power + def timer_callback(self, event): + if self.state in [State.PAUSED, State.STOPPED, State.PASSIVE]: + return + if self.state == State.ENABLED: if self.timeout <= 0: self.state = State.DISABLED - self.get_logger().info("State: DISABLED (Timeout)") + rospy.loginfo("State: DISABLED (Timeout)") self.disable_chassis() else: - self.timeout -= 0.01 # at a rate of 100 Hz this equals to -1 per second. + self.timeout -= 0.01 self.cmd_vel_pub.publish(self.latest_cmd_vel) - if self.state == State.DISABLED and (self.abs_x > 0.002 or self.abs_z > 0.002): # This is a hack to enable the chassis when receiving commands e.g. from Nav2 + + if self.state == State.DISABLED and (self.abs_x > 0.002 or self.abs_z > 0.002): self.state = State.ENABLED - self.get_logger().info("State: ENABLED (cmd_vel)") + rospy.loginfo("State: ENABLED (cmd_vel)") self.enable_chassis() -def main(args=None): - rclpy.init(args=args) +if __name__ == '__main__': node = StateMachineNode() try: - rclpy.spin(node) - except KeyboardInterrupt: + rospy.spin() + except rospy.ROSInterruptException: pass finally: node.disable_chassis() - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() + rospy.loginfo('Shutting down node...') 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/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'