mirror of
https://github.com/bjoernellens1/rmp220_middleware.git
synced 2025-01-18 16:16:58 +00:00
update: noetic version
This commit is contained in:
parent
f265f8a0dd
commit
65ef4f630d
@ -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(<dependency> 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()
|
||||
|
||||
28
package.xml
28
package.xml
@ -1,18 +1,28 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<package format="2">
|
||||
<name>rmp220_middleware</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="bjoern.ellensohn@gmail.com">bjorn</maintainer>
|
||||
<description>Middleware package for controlling the RMP220 chassis in ROS 1.</description>
|
||||
|
||||
<maintainer email="bjoern.ellensohn@gmail.com">Bjorn Ellensohn</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<!-- Dependencies -->
|
||||
<build_depend>catkin</build_depend>
|
||||
<build_depend>rospy</build_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>
|
||||
<build_type>ament_python</build_type>
|
||||
<build_type>catkin</build_type>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -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...')
|
||||
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/rmp220_middleware
|
||||
[install]
|
||||
install_scripts=$base/lib/rmp220_middleware
|
||||
26
setup.py
26
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'
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -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'
|
||||
@ -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)
|
||||
@ -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'
|
||||
Loading…
Reference in New Issue
Block a user