ellensohn/rmp220_middleware
1
0
Fork 0
mirror of https://github.com/bjoernellens1/rmp220_middleware.git synced 2025-01-18 16:16:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

update: noetic version

Browse Source
This commit is contained in:
Björn Ellensohn 2025-01-15 18:41:33 +01:00
parent f265f8a0dd
commit 65ef4f630d
9 changed files with 116 additions and 225 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
CMakeLists.txt
View File

@ -1,26 +1,33 @@
cmake_minimum_required(VERSION 3.8) cmake_minimum_required(VERSION 3.0.2)
project(rmp220_middleware) project(rmp220_middleware)
# Set compiler options for warnings
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wall -Wextra -Wpedantic)
endif() endif()
# find dependencies # Find catkin and required components
find_package(ament_cmake REQUIRED) find_package(catkin REQUIRED COMPONENTS
# uncomment the following section in order to fill in rospy
# further dependencies manually. std_msgs
# find_package(<dependency> REQUIRED) geometry_msgs
sensor_msgs
segway_msgs
)
if(BUILD_TESTING) # Declare catkin package
find_package(ament_lint_auto REQUIRED) catkin_package(
# the following line skips the linter which checks for copyrights CATKIN_DEPENDS rospy std_msgs geometry_msgs sensor_msgs segway_msgs
# 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) # Install Python scripts
# comment the line when this package is in a git repo and when catkin_install_python(PROGRAMS
# a copyright and license is added to all source files rmp220_middleware/rmp220_middleware.py # Replace with the actual script path
set(ament_cmake_cpplint_FOUND TRUE) DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
ament_lint_auto_find_test_dependencies() )
# Testing (optional)
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
# Add tests here if needed
endif() endif()
ament_package()

28
package.xml
View File

@ -1,18 +1,28 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <package format="2">
<package format="3">
<name>rmp220_middleware</name> <name>rmp220_middleware</name>
<version>0.0.0</version> <version>0.0.0</version>
<description>TODO: Package description</description> <description>Middleware package for controlling the RMP220 chassis in ROS 1.</description>
<maintainer email="bjoern.ellensohn@gmail.com">bjorn</maintainer>
<maintainer email="bjoern.ellensohn@gmail.com">Bjorn Ellensohn</maintainer>
<license>TODO: License declaration</license> <license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend> <!-- Dependencies -->
<test_depend>ament_flake8</test_depend> <build_depend>catkin</build_depend>
<test_depend>ament_pep257</test_depend> <build_depend>rospy</build_depend>
<test_depend>python3-pytest</test_depend> <build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>segway_msgs</build_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>segway_msgs</exec_depend>
<!-- Export build type -->
<export> <export>
<build_type>ament_python</build_type> <build_type>catkin</build_type>
</export> </export>
</package> </package>

0
rmp220_middleware/__init__.py
View File

167
rmp220_middleware/rmp220_middleware.py
View File

@ -1,7 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python
import rclpy import rospy
from rclpy.node import Node
from std_msgs.msg import Bool from std_msgs.msg import Bool
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy
@ -9,146 +8,124 @@ from enum import Enum
from segway_msgs.srv import RosSetChassisEnableCmd from segway_msgs.srv import RosSetChassisEnableCmd
from segway_msgs.msg import ChassisModeFb 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 class StateMachineNode:
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):
def __init__(self): def __init__(self):
super().__init__('state_machine_node') rospy.init_node('state_machine_node', anonymous=True)
# Initialize state and other variables # Initialize state and other variables
self.state = State.DISABLED # is this necessary? self.state = State.DISABLED
self.timeout = 20.0 # Timeout in seconds self.timeout = 20.0 # Timeout in seconds
# Create twist class for publishing velocities
self.twist = Twist() self.twist = Twist()
self.latest_cmd_vel = Twist() self.latest_cmd_vel = Twist()
self.abs_x = 0.0 self.abs_x = 0.0
self.abs_z = 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 # Service clients
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel_out', 10) rospy.wait_for_service('set_chassis_enable')
self.cmd_vel_sub = self.create_subscription(Twist, '/cmd_vel_mux', self.cmd_vel_callback, 10) self.chassis_enable_client = rospy.ServiceProxy('set_chassis_enable', RosSetChassisEnableCmd)
self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10) rospy.loginfo('Chassis enable service available.')
self.timer = self.create_timer(0.01, self.timer_callback) # changed to 100 Hz for now
# Create service clients for chassis enable and disable # Timer for periodic updates
self.chassis_enable_client = self.create_client(RosSetChassisEnableCmd, 'set_chassis_enable') self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback) # 100 Hz
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
)
def chassis_mode_callback(self, msg): def chassis_mode_callback(self, msg):
# Need to evaluate that for possible errors if self.state == State.PAUSED:
# 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
return return
else:
if msg.chassis_mode == 0: if msg.chassis_mode == 0:
self.state = State.DISABLED self.state = State.DISABLED
self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) rospy.loginfo('Set chassis_mode to DISABLED')
if msg.chassis_mode == 1: # Assuming 1 represents enabled and 0 represents disabled elif msg.chassis_mode == 1:
self.state = State.ENABLED self.state = State.ENABLED
self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) rospy.loginfo('Set chassis_mode to ENABLED')
if msg.chassis_mode == 2: elif msg.chassis_mode == 2:
self.state = State.PASSIVE self.state = State.PASSIVE
self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) rospy.loginfo('Set chassis_mode to PASSIVE')
if msg.chassis_mode == 3: elif msg.chassis_mode == 3:
self.state = State.STOPPED self.state = State.STOPPED
self.get_logger().info('Set chassis_mode to ' + str(self.state.value)) rospy.loginfo('Set chassis_mode to STOPPED')
def enable_chassis(self): def enable_chassis(self):
req = RosSetChassisEnableCmd.Request() try:
req.ros_set_chassis_enable_cmd = True self.chassis_enable_client(True)
self.chassis_enable_client.call_async(req) self.state = State.ENABLED
self.state = State.ENABLED rospy.loginfo('Enabling chassis...')
self.get_logger().info('Enabling chassis...') except rospy.ServiceException as e:
rospy.logerr(f'Failed to enable chassis: {e}')
def pause_chassis(self): def pause_chassis(self):
req = RosSetChassisEnableCmd.Request() try:
req.ros_set_chassis_enable_cmd = False self.chassis_enable_client(False)
self.chassis_enable_client.call_async(req) self.state = State.PAUSED
self.state = State.PAUSED rospy.loginfo('Pausing chassis...')
self.get_logger().info('Pausing chassis...') except rospy.ServiceException as e:
rospy.logerr(f'Failed to pause chassis: {e}')
def disable_chassis(self): def disable_chassis(self):
req = RosSetChassisEnableCmd.Request() try:
req.ros_set_chassis_enable_cmd = False self.chassis_enable_client(False)
self.chassis_enable_client.call_async(req) self.state = State.DISABLED
self.state = State.DISABLED rospy.loginfo('Disabling chassis...')
self.get_logger().info('Disabling chassis...') except rospy.ServiceException as e:
rospy.logerr(f'Failed to disable chassis: {e}')
def joy_callback(self, msg): def joy_callback(self, msg):
start_button = msg.buttons[7] # Joystick button 'start' start_button = msg.buttons[7] # Joystick button 'start'
select_button = msg.buttons[6] # Joystick button 'select' select_button = msg.buttons[6] # Joystick button 'select'
if start_button == 1: if start_button == 1:
self.get_logger().info("State: ENABLED (Button 'start')") rospy.loginfo("State: ENABLED (Button 'start')")
self.enable_chassis() self.enable_chassis()
self.timeout = 20 self.timeout = 20
if select_button == 1: if select_button == 1:
self.get_logger().info("State: DISABLED (Button 'select')") rospy.loginfo("State: DISABLED (Button 'select')")
self.pause_chassis() self.pause_chassis()
def cmd_vel_callback(self, msg): 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 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_x = abs(msg.linear.x)
self.abs_z = abs(msg.angular.z) self.abs_z = abs(msg.angular.z)
# Reset timeout when receiving commands
self.timeout = 20.0 self.timeout = 20.0
def timer_callback(self): def timer_callback(self, event):
if self.state == State.PAUSED or self.state == State.STOPPED or self.state == State.PASSIVE: if self.state in [State.PAUSED, State.STOPPED, State.PASSIVE]:
return # Do nothing if chassis is disabled, stopped or passive --> should save processing power return
if self.state == State.ENABLED: if self.state == State.ENABLED:
if self.timeout <= 0: if self.timeout <= 0:
self.state = State.DISABLED self.state = State.DISABLED
self.get_logger().info("State: DISABLED (Timeout)") rospy.loginfo("State: DISABLED (Timeout)")
self.disable_chassis() self.disable_chassis()
else: 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) 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.state = State.ENABLED
self.get_logger().info("State: ENABLED (cmd_vel)") rospy.loginfo("State: ENABLED (cmd_vel)")
self.enable_chassis() self.enable_chassis()
def main(args=None): if __name__ == '__main__':
rclpy.init(args=args)
node = StateMachineNode() node = StateMachineNode()
try: try:
rclpy.spin(node) rospy.spin()
except KeyboardInterrupt: except rospy.ROSInterruptException:
pass pass
finally: finally:
node.disable_chassis() node.disable_chassis()
node.destroy_node() rospy.loginfo('Shutting down node...')
rclpy.shutdown()
if __name__ == '__main__':
main()

4
setup.cfg
View File

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

26
setup.py
View File

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

25
test/test_copyright.py
View File

@ -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'

25
test/test_flake8.py
View File

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

23
test/test_pep257.py
View File

@ -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'