From 3e3ace63d8dc9eeade447d4a75211903ca1bff0e Mon Sep 17 00:00:00 2001 From: berliernj Date: Wed, 12 Apr 2023 15:51:28 -0400 Subject: [PATCH 01/32] Initial Commit --- src/amp_msgs/CMakeLists.txt | 41 ++++++++++++++++ src/amp_msgs/package.xml | 20 ++++++++ src/amp_msgs/srv/TrackState.srv | 3 ++ src/amp_rcs_client/amp_rcs/__init__.py | 0 src/amp_rcs_client/amp_rcs/dummy_server.py | 28 +++++++++++ src/amp_rcs_client/amp_rcs/service_client.py | 51 ++++++++++++++++++++ src/amp_rcs_client/package.xml | 20 ++++++++ src/amp_rcs_client/resource/amp_rcs | 0 src/amp_rcs_client/setup.cfg | 4 ++ src/amp_rcs_client/setup.py | 27 +++++++++++ src/amp_rcs_client/test/test_copyright.py | 23 +++++++++ src/amp_rcs_client/test/test_flake8.py | 25 ++++++++++ src/amp_rcs_client/test/test_pep257.py | 23 +++++++++ 13 files changed, 265 insertions(+) create mode 100644 src/amp_msgs/CMakeLists.txt create mode 100644 src/amp_msgs/package.xml create mode 100644 src/amp_msgs/srv/TrackState.srv create mode 100644 src/amp_rcs_client/amp_rcs/__init__.py create mode 100644 src/amp_rcs_client/amp_rcs/dummy_server.py create mode 100644 src/amp_rcs_client/amp_rcs/service_client.py create mode 100644 src/amp_rcs_client/package.xml create mode 100644 src/amp_rcs_client/resource/amp_rcs create mode 100644 src/amp_rcs_client/setup.cfg create mode 100644 src/amp_rcs_client/setup.py create mode 100644 src/amp_rcs_client/test/test_copyright.py create mode 100644 src/amp_rcs_client/test/test_flake8.py create mode 100644 src/amp_rcs_client/test/test_pep257.py diff --git a/src/amp_msgs/CMakeLists.txt b/src/amp_msgs/CMakeLists.txt new file mode 100644 index 0000000..57f7c08 --- /dev/null +++ b/src/amp_msgs/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.5) +project(amp_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/TrackState.srv" + ) + +ament_package() diff --git a/src/amp_msgs/package.xml b/src/amp_msgs/package.xml new file mode 100644 index 0000000..f49a683 --- /dev/null +++ b/src/amp_msgs/package.xml @@ -0,0 +1,20 @@ + + + + amp_msgs + 0.0.1 + Package for holding custom definitions + Nick Berlier + GNU General Public License 3.0 + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/amp_msgs/srv/TrackState.srv b/src/amp_msgs/srv/TrackState.srv new file mode 100644 index 0000000..eb25ad0 --- /dev/null +++ b/src/amp_msgs/srv/TrackState.srv @@ -0,0 +1,3 @@ +string state +--- +string state diff --git a/src/amp_rcs_client/amp_rcs/__init__.py b/src/amp_rcs_client/amp_rcs/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py new file mode 100644 index 0000000..6b41ab1 --- /dev/null +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -0,0 +1,28 @@ +'''ROS2 service server for behavioral determination based on client call''' + +#usr/bin/env python3 +from rcs_service.srv import TrackState +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +class ServiceServer(Node): + def __init__(self): + super().__init__('service_server') + self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) + + def track_state_callback(self, request, response): + response.state = request.state + self.get_logger().info('Recieved: %s' % (request.state)) + self.get_logger().info('Response: %s\n' % (response.state)) + + return response + +def main(args=None): + rclpy.init(args=args) + node = ServiceServer() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == 'main': + main() diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py new file mode 100644 index 0000000..89ee0e4 --- /dev/null +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -0,0 +1,51 @@ +'''ROS2 service client for state based service calling''' + +#usr/bin/env python3 +from rcs_service.srv import TrackState +import rclpy +import sys +from rclpy.node import Node +from std_msgs.msg import String + +class ServiceClient(Node): + def __init__(self): + super().__init__('service_client') + self.client = self.create_client(TrackState, 'track_state') + while not self.client.wait_for_service(timeout_sec = 5.0): + self.get_logger().info('service not found and/or available, waiting again...') + self.request = TrackState.Request() + + def send_request(self, state): + self.request.state = state + self.future = self.client.call_async(self.request) + rclpy.spin_until_future_complete(self, self.future) + + return self.future.result() + + +class KartStateDetermination(Node): + def __init__(self): + super().__init__('kart_state_determination') + self.state = "Red" # Change to different deafault string? + + self.track_subscription = self.create_subscription(String, "track_state", self.callback, 10) + self.kart_publisher = self.create_publisher(String, "kart_state", 10) + + def callback(self, msg): + if msg.data != self.state: + self.state = msg.data + service_client = ServiceClient() + response = service_client.send_request(self.state) + response_msg = String() + response_msg.data = response.state + self.kart_publisher.publish(response_msg) + +def main (args=None): + rclpy.init(args=args) + node = KartStateDetermination() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() + diff --git a/src/amp_rcs_client/package.xml b/src/amp_rcs_client/package.xml new file mode 100644 index 0000000..80f8ca9 --- /dev/null +++ b/src/amp_rcs_client/package.xml @@ -0,0 +1,20 @@ + + + + amp_rcs + 0.0.1 + Service client for sent track state change requests to the service server + Nick Berlier + GNU General Public License 3.0 + + rclpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/amp_rcs_client/resource/amp_rcs b/src/amp_rcs_client/resource/amp_rcs new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_rcs_client/setup.cfg b/src/amp_rcs_client/setup.cfg new file mode 100644 index 0000000..d920b74 --- /dev/null +++ b/src/amp_rcs_client/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/amp_rcs +[install] +install-scripts=$base/lib/amp_rcs diff --git a/src/amp_rcs_client/setup.py b/src/amp_rcs_client/setup.py new file mode 100644 index 0000000..3f6344c --- /dev/null +++ b/src/amp_rcs_client/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = 'amp_rcs' + +setup( + name=package_name, + version='0.0.1', + 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='berliernj', + maintainer_email='berlier3@gmail.com', + description='Service client for sent track state change requests to the service server', + license='GNU General Public License 3.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + [ "amp_rcs_client = amp_rcs_client.service_client:main", + "amp_rcs_server = amp_rcs_server.dummy_server:main" + ], + }, +) diff --git a/src/amp_rcs_client/test/test_copyright.py b/src/amp_rcs_client/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/amp_rcs_client/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/amp_rcs_client/test/test_flake8.py b/src/amp_rcs_client/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/amp_rcs_client/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/src/amp_rcs_client/test/test_pep257.py b/src/amp_rcs_client/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/amp_rcs_client/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' From 2c75908ada8b4057d1372015bca813cde6002eba Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 12 Apr 2023 20:01:47 +0000 Subject: [PATCH 02/32] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/amp_msgs/package.xml | 2 +- src/amp_rcs_client/amp_rcs/dummy_server.py | 37 +++++---- src/amp_rcs_client/amp_rcs/service_client.py | 84 +++++++++++--------- src/amp_rcs_client/package.xml | 7 +- src/amp_rcs_client/setup.py | 11 +-- 5 files changed, 77 insertions(+), 64 deletions(-) diff --git a/src/amp_msgs/package.xml b/src/amp_msgs/package.xml index f49a683..dd85e4d 100644 --- a/src/amp_msgs/package.xml +++ b/src/amp_msgs/package.xml @@ -1,4 +1,4 @@ - + amp_msgs diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index 6b41ab1..ea92162 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -6,23 +6,28 @@ from rclpy.node import Node from std_msgs.msg import String + class ServiceServer(Node): - def __init__(self): - super().__init__('service_server') - self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) - - def track_state_callback(self, request, response): - response.state = request.state - self.get_logger().info('Recieved: %s' % (request.state)) - self.get_logger().info('Response: %s\n' % (response.state)) - - return response + + def __init__(self): + super().__init__('service_server') + self.server = self.create_service(TrackState, 'track_state', + self.track_state_callback) + + def track_state_callback(self, request, response): + response.state = request.state + self.get_logger().info('Recieved: %s' % (request.state)) + self.get_logger().info('Response: %s\n' % (response.state)) + + return response + def main(args=None): - rclpy.init(args=args) - node = ServiceServer() - rclpy.spin(node) - rclpy.shutdown() - + rclpy.init(args=args) + node = ServiceServer() + rclpy.spin(node) + rclpy.shutdown() + + if __name__ == 'main': - main() + main() diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py index 89ee0e4..05e9dc3 100644 --- a/src/amp_rcs_client/amp_rcs/service_client.py +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -3,49 +3,55 @@ #usr/bin/env python3 from rcs_service.srv import TrackState import rclpy -import sys +import sys from rclpy.node import Node from std_msgs.msg import String + class ServiceClient(Node): - def __init__(self): - super().__init__('service_client') - self.client = self.create_client(TrackState, 'track_state') - while not self.client.wait_for_service(timeout_sec = 5.0): - self.get_logger().info('service not found and/or available, waiting again...') - self.request = TrackState.Request() - - def send_request(self, state): - self.request.state = state - self.future = self.client.call_async(self.request) - rclpy.spin_until_future_complete(self, self.future) - - return self.future.result() - - + + def __init__(self): + super().__init__('service_client') + self.client = self.create_client(TrackState, 'track_state') + while not self.client.wait_for_service(timeout_sec=5.0): + self.get_logger().info( + 'service not found and/or available, waiting again...') + self.request = TrackState.Request() + + def send_request(self, state): + self.request.state = state + self.future = self.client.call_async(self.request) + rclpy.spin_until_future_complete(self, self.future) + + return self.future.result() + + class KartStateDetermination(Node): - def __init__(self): - super().__init__('kart_state_determination') - self.state = "Red" # Change to different deafault string? - - self.track_subscription = self.create_subscription(String, "track_state", self.callback, 10) - self.kart_publisher = self.create_publisher(String, "kart_state", 10) - - def callback(self, msg): - if msg.data != self.state: - self.state = msg.data - service_client = ServiceClient() - response = service_client.send_request(self.state) - response_msg = String() - response_msg.data = response.state - self.kart_publisher.publish(response_msg) - -def main (args=None): - rclpy.init(args=args) - node = KartStateDetermination() - rclpy.spin(node) - rclpy.shutdown() - + + def __init__(self): + super().__init__('kart_state_determination') + self.state = "Red" # Change to different default string? + + self.track_subscription = self.create_subscription( + String, "track_state", self.callback, 10) + self.kart_publisher = self.create_publisher(String, "kart_state", 10) + + def callback(self, msg): + if msg.data != self.state: + self.state = msg.data + service_client = ServiceClient() + response = service_client.send_request(self.state) + response_msg = String() + response_msg.data = response.state + self.kart_publisher.publish(response_msg) + + +def main(args=None): + rclpy.init(args=args) + node = KartStateDetermination() + rclpy.spin(node) + rclpy.shutdown() + + if __name__ == '__main__': main() - diff --git a/src/amp_rcs_client/package.xml b/src/amp_rcs_client/package.xml index 80f8ca9..70c53a8 100644 --- a/src/amp_rcs_client/package.xml +++ b/src/amp_rcs_client/package.xml @@ -1,12 +1,13 @@ - + amp_rcs 0.0.1 - Service client for sent track state change requests to the service server + Service client for sent track state change requests to the service server Nick Berlier GNU General Public License 3.0 - + rclpy ament_copyright diff --git a/src/amp_rcs_client/setup.py b/src/amp_rcs_client/setup.py index 3f6344c..092f262 100644 --- a/src/amp_rcs_client/setup.py +++ b/src/amp_rcs_client/setup.py @@ -8,20 +8,21 @@ packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], zip_safe=True, maintainer='berliernj', maintainer_email='berlier3@gmail.com', - description='Service client for sent track state change requests to the service server', + description= + 'Service client for sent track state change requests to the service server', license='GNU General Public License 3.0', tests_require=['pytest'], entry_points={ - 'console_scripts': - [ "amp_rcs_client = amp_rcs_client.service_client:main", - "amp_rcs_server = amp_rcs_server.dummy_server:main" + 'console_scripts': [ + "amp_rcs_client = amp_rcs_client.service_client:main", + "amp_rcs_server = amp_rcs_server.dummy_server:main" ], }, ) From 36bf7ad2cf2793049de076272c8eee61e4043771 Mon Sep 17 00:00:00 2001 From: berliernj Date: Fri, 14 Apr 2023 13:21:21 -0400 Subject: [PATCH 03/32] Fixed bug due to name change --- src/amp_rcs_client/setup.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/amp_rcs_client/setup.py b/src/amp_rcs_client/setup.py index 092f262..9e5f3d3 100644 --- a/src/amp_rcs_client/setup.py +++ b/src/amp_rcs_client/setup.py @@ -21,8 +21,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - "amp_rcs_client = amp_rcs_client.service_client:main", - "amp_rcs_server = amp_rcs_server.dummy_server:main" + "amp_rcs_client = amp_rcs.service_client:main", + "amp_rcs_server = amp_rcs.dummy_server:main" ], }, ) From c6505dbc952201670a4312bd990d95cd655b5348 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:40:06 -0400 Subject: [PATCH 04/32] kart_commander node initial Added QoS profile and state transition rules --- .../amp_kart_commander/__init__.py | 0 .../amp_kart_commander/kart_commander.py | 71 +++++++++++++++++++ src/amp_kart_commander/package.xml | 18 +++++ src/amp_kart_commander/setup.cfg | 4 ++ src/amp_kart_commander/setup.py | 25 +++++++ src/amp_kart_commander/test/test_copyright.py | 23 ++++++ src/amp_kart_commander/test/test_flake8.py | 25 +++++++ src/amp_kart_commander/test/test_pep257.py | 23 ++++++ 8 files changed, 189 insertions(+) create mode 100644 src/amp_kart_commander/amp_kart_commander/__init__.py create mode 100644 src/amp_kart_commander/amp_kart_commander/kart_commander.py create mode 100644 src/amp_kart_commander/package.xml create mode 100644 src/amp_kart_commander/setup.cfg create mode 100644 src/amp_kart_commander/setup.py create mode 100644 src/amp_kart_commander/test/test_copyright.py create mode 100644 src/amp_kart_commander/test/test_flake8.py create mode 100644 src/amp_kart_commander/test/test_pep257.py diff --git a/src/amp_kart_commander/amp_kart_commander/__init__.py b/src/amp_kart_commander/amp_kart_commander/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py new file mode 100644 index 0000000..5fee07d --- /dev/null +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -0,0 +1,71 @@ +'''Server that receives updates to TrackState from RCS and sets the kart cmd_vel mux accordingly''' + +#usr/bin/env python3 +from amp_msgs.srv import TrackState +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool + +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy + +# Create high-reliability QoS for mux toggling +qos_profile = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, # Last always has priority anyways + reliability=ReliabilityPolicy.RELIABLE, # Guaranteed delivery + durability=DurabilityPolicy. + TRANSIENT_LOCAL # Re-send msg to late-joining subscribers +) + + +class ServiceServer(Node): + + def __init__(self): + super().__init__('service_server') + self.server = self.create_service(TrackState, 'track_state', + self.track_state_callback) + self.stop_publisher = self.create_publisher(Bool, + 'stop', + qos_profile=qos_profile) + self.joy_only_publisher = self.create_publisher( + Bool, 'joy_only', qos_profile=qos_profile) + + def track_state_callback(self, request, response): + response.state = request.state + + self.get_logger().info('Recieved: %s' % (request.state)) + self.get_logger().info('Response: %s\n' % (response.state)) + + # TODO More complex states in TrackState.srv have transition rules not yet implemented + + # When "teleop_only" is false and "stop" is false, then both joy and nav2 messages + # can coexist. + # Note that while you can reach Stop from any other state, returning to + # Autonomus from Stop is not allowed; switch to RC first. + if response.state == response.ESTOP or response.state == response.SAFESTOP: + self.stop_publisher.publish(True) + self.get_logger().info('STOP: nav2 and teleop disabled') + if response.state == response.RC or response.state == response.RACEDONE: + self.joy_only_publisher.publish(True) + self.get_logger().info('RC: nav2 disabled') + self.stop_publisher.publish(False) + self.get_logger().info('RC: STOP mode restrictions lifted') + if response.state == response.AUTONOMOUS_SLOW or response.state == response.AUTONOMOUS_OVERTAKE: + self.joy_only_publisher.publish(False) + self.get_logger().info( + 'AUTO: allow both nav2 and teleop controls. Switching directly from' + 'ESTOP/SAFESTOP to this state is not allowed. Switch to RC first' + ) + + return response + + +def main(args=None): + rclpy.init(args=args) + node = ServiceServer() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == 'main': + main() diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml new file mode 100644 index 0000000..d6a5c7a --- /dev/null +++ b/src/amp_kart_commander/package.xml @@ -0,0 +1,18 @@ + + + + amp_kart_commander + 0.0.0 + Updates cmd_vel max based on TrackState changes + lucy + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg new file mode 100644 index 0000000..539f781 --- /dev/null +++ b/src/amp_kart_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/amp_kart_commander +[install] +install-scripts=$base/lib/amp_kart_commander diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py new file mode 100644 index 0000000..daf9862 --- /dev/null +++ b/src/amp_kart_commander/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'amp_kart_commander' + +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='lucy', + maintainer_email='xchenbox@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['kart_commander = amp_kart_commander.kart_commander:main'], + }, +) diff --git a/src/amp_kart_commander/test/test_copyright.py b/src/amp_kart_commander/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/amp_kart_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/amp_kart_commander/test/test_flake8.py b/src/amp_kart_commander/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/amp_kart_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/src/amp_kart_commander/test/test_pep257.py b/src/amp_kart_commander/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/amp_kart_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' From da0bac3dfb333455f1ab647dd2d76e9404de6332 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:41:33 -0400 Subject: [PATCH 05/32] kChange twist setup to single mux in hindsight it was simpler. If one mux fails both are likely screwed --- src/amp_kart_bringup/README.md | 55 ++++++++++--------- .../launch/twist_mux.launch.py | 31 ++++------- ...ux_2.params.yaml => twist_mux.params.yaml} | 19 ++++--- .../params/twist_mux_1.params.yaml | 12 ---- 4 files changed, 50 insertions(+), 67 deletions(-) rename src/amp_kart_bringup/params/{twist_mux_2.params.yaml => twist_mux.params.yaml} (50%) delete mode 100644 src/amp_kart_bringup/params/twist_mux_1.params.yaml diff --git a/src/amp_kart_bringup/README.md b/src/amp_kart_bringup/README.md index cea38c0..be7e6b9 100644 --- a/src/amp_kart_bringup/README.md +++ b/src/amp_kart_bringup/README.md @@ -4,35 +4,37 @@ Contains files needed to start the kart. ## Mux structure -The muxes used are [`twist_mux`](http://wiki.ros.org/twist_mux). -They are laid out as can be seen below. +There are two control sources that all produce `twist_mux` messages to "cmd_vel": joy_vel (teleop controller) and nav_vel (autnomous navigation). +The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. It's important that the states can override each other in this priority: + +- ESTOP/SAFE STOP +- TELEOP +- AUTO + +The priorities are implemented with a [`twist_mux`](http://wiki.ros.org/twist_mux). +and laid out as can be seen below. ``` - /joy_disable /joy_enable - | | - | | /stop - ____ | | - | | /mux1_vel ____ - /joy_vel ____| |_____________| | - | | | | - |____| | |____ /cmd_vel - mux1 | | - /nav_vel ___| | - |____| - mux2 + + + /joy_only /stop (255) + \ / + | | + _|_|_ + /joy_vel____| | + | |____ /cmd_vel + | | + /nav_vel ___| | + |_____| + ``` -Priorities for the muxes are listed below, as well as the corresponding topic -and (expected) topic publishers: +Priorities for each topic and lock are listed, as well as the expected topic publishers: -- `mux1` → `/mux1_vel` - - `joy_disable` ~ `kart_commander`: 150 - - `joy_vel` ~ `teleop_twist_joy_node`: 100 -- `mux2` → `/cmd_vel` - - `stop` ~ `kart_commander`: 255 - - `joy_enable` ~ `kart_commander`: 50 - - `nav_vel` ~ `nav2`: 10 - - `mux1_vel` ~ `mux1`: 100 +- `stop` ~ `kart_commander`: 255 +- `joy_vel` ~ `teleop_twist_joy_node`: 50 +- `joy_only_publisher` ~ `kart_commander`: 50 +- `nav_vel` ~ `nav2`: 10 ## Directory Tree @@ -48,7 +50,7 @@ and (expected) topic publishers: - `rviz.launch.py` _Initialize RViz with Nav2 configuration_ - `slam.launch.py` _Initialize map_server and slam_toolbox_ - `teleop.launch.py` _Launch joy teleop nodes_ - - `twist_mux.launch.py` _Launch both twist muxex_ + - `twist_mux.launch.py` _Launch both twist muxex and kart_commander_ - `VLP16.launch.py` _Initialize Velodyne LIDAR VLP16 nodes_ - `map/` - `empty_map.yaml` _Default empty map_ @@ -56,7 +58,6 @@ and (expected) topic publishers: - `VLP16.params.yaml` _VLP16 parameters_ - `VLP16db.params.yaml` _VLP16 calibration parameters_ - `nav2.params.yaml` _Nav2 parameters_ - - `twist_mux_1.params.yaml` - - `twist_mux_2.params.yaml` + - `twist_mux.params.yaml` - `rviz/` - `nav2_default_view.rviz.yaml` _RViz config for Nav2_ diff --git a/src/amp_kart_bringup/launch/twist_mux.launch.py b/src/amp_kart_bringup/launch/twist_mux.launch.py index 82dbc6d..4cc4228 100644 --- a/src/amp_kart_bringup/launch/twist_mux.launch.py +++ b/src/amp_kart_bringup/launch/twist_mux.launch.py @@ -5,6 +5,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node def generate_launch_description(): @@ -12,33 +13,23 @@ def generate_launch_description(): share_dir = get_package_share_directory('amp_kart_bringup') params_dir = os.path.join(share_dir, 'params') - mux1_cmd = IncludeLaunchDescription( + mux_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('twist_mux'), 'launch', 'twist_mux_launch.py')), launch_arguments={ - 'config_locks': os.path.join(params_dir, - 'twist_mux_1.params.yaml'), - 'config_topics': os.path.join(params_dir, - 'twist_mux_1.params.yaml'), - 'cmd_vel_out': 'mux1_vel' - }.items()) - - mux2_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('twist_mux'), 'launch', - 'twist_mux_launch.py')), - launch_arguments={ - 'config_locks': os.path.join(params_dir, - 'twist_mux_2.params.yaml'), - 'config_topics': os.path.join(params_dir, - 'twist_mux_2.params.yaml'), + 'config_locks': os.path.join(params_dir, 'twist_mux.params.yaml'), + 'config_topics': os.path.join(params_dir, 'twist_mux.params.yaml'), 'cmd_vel_out': 'cmd_vel' }.items()) - ld = LaunchDescription() + kart_commander_action = Node(package='amp_kart_commander', + namespace='', + executable='kart_commander', + name='sikart_commander') - ld.add_action(mux1_cmd) - ld.add_action(mux2_cmd) + ld = LaunchDescription() + ld.add_action(mux_cmd) + ld.add_action(kart_commander_action) return ld diff --git a/src/amp_kart_bringup/params/twist_mux_2.params.yaml b/src/amp_kart_bringup/params/twist_mux.params.yaml similarity index 50% rename from src/amp_kart_bringup/params/twist_mux_2.params.yaml rename to src/amp_kart_bringup/params/twist_mux.params.yaml index fd15712..48c0808 100644 --- a/src/amp_kart_bringup/params/twist_mux_2.params.yaml +++ b/src/amp_kart_bringup/params/twist_mux.params.yaml @@ -1,20 +1,23 @@ twist_mux: ros__parameters: + # Make sure the locks and corresponding locked topics have the same priority + # When the kart_commander has not explicitly disallowed auto, + # this config allows for teleop and auto to happen simultaneously (in alternating periods of 0.5s) locks: - stop: - topic: stop - timeout: 0.0 - priority: 255 joy_enable: topic: joy_enable timeout: 0.0 priority: 50 + stop: + topic: stop + timeout: 0.0 + priority: 255 topics: + joy_vel: + topic: joy_vel + timeout: 0.5 + priority: 50 navigation: topic: nav_vel timeout: 0.5 priority: 10 - mux1: - topic: mux1_vel - timeout: 0.5 - priority: 100 diff --git a/src/amp_kart_bringup/params/twist_mux_1.params.yaml b/src/amp_kart_bringup/params/twist_mux_1.params.yaml deleted file mode 100644 index 9e8b747..0000000 --- a/src/amp_kart_bringup/params/twist_mux_1.params.yaml +++ /dev/null @@ -1,12 +0,0 @@ -twist_mux: - ros__parameters: - locks: - joy_disable: - topic: joy_disable - timeout: 0.0 - priority: 150 - topics: - joy_vel: - topic: joy_vel - timeout: 0.5 - priority: 100 From 56f99e970a6ebed25bf7c5c2c7bded08b6e23715 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:43:07 -0400 Subject: [PATCH 06/32] Add constants for TrackState and KartState response See the AKS server for the state graph --- src/amp_msgs/srv/TrackState.srv | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/amp_msgs/srv/TrackState.srv b/src/amp_msgs/srv/TrackState.srv index eb25ad0..f698fd7 100644 --- a/src/amp_msgs/srv/TrackState.srv +++ b/src/amp_msgs/srv/TrackState.srv @@ -1,3 +1,19 @@ string state + +# Track State constants (Received from server) +# TODO set the values to the actual RCS strings +string EMERGENCY="black" +string SAFESTOP="red" +string RACEDONE="checkered" +string RC="orange" +string AUTONOMOUS_SLOW="yellow" +string AUTONOMOUS_OVERTAKE="green" --- string state + +# Kart State constants (Sent back to server) +string EMERGENCY="todo1" +string STOPPED="todo2" +string RC="todo3" +string READY="todo4" +string AUTONOMOUS="todo5" From 6e6184a905ebb74b05b7726ded2e1d4ab2ac4ccc Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:44:01 -0400 Subject: [PATCH 07/32] Changed trackstate srv package to match the new name It was using the rcs_service package before --- src/amp_rcs_client/amp_rcs/service_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py index 05e9dc3..b224e5a 100644 --- a/src/amp_rcs_client/amp_rcs/service_client.py +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -1,7 +1,7 @@ '''ROS2 service client for state based service calling''' #usr/bin/env python3 -from rcs_service.srv import TrackState +from amp_msgs.srv import TrackState import rclpy import sys from rclpy.node import Node From 7f74120380e8b354d4423c40b51a45c603236956 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 07:19:46 -0400 Subject: [PATCH 08/32] bug fixes during testing No documentation for constants needed some trial n error --- .../amp_kart_commander/kart_commander.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index 5fee07d..d748bfe 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -6,7 +6,7 @@ from rclpy.node import Node from std_msgs.msg import Bool -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy +from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy # Create high-reliability QoS for mux toggling qos_profile = QoSProfile( @@ -33,8 +33,10 @@ def __init__(self): def track_state_callback(self, request, response): response.state = request.state - self.get_logger().info('Recieved: %s' % (request.state)) - self.get_logger().info('Response: %s\n' % (response.state)) + self.get_logger().info('Recieved: %s Response: %s' % + (request.state, response.state)) + + # self.get_logger().info('eeee' + str(dir(TrackState.Request))) # TODO More complex states in TrackState.srv have transition rules not yet implemented @@ -42,19 +44,19 @@ def track_state_callback(self, request, response): # can coexist. # Note that while you can reach Stop from any other state, returning to # Autonomus from Stop is not allowed; switch to RC first. - if response.state == response.ESTOP or response.state == response.SAFESTOP: - self.stop_publisher.publish(True) + if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP: + self.stop_publisher.publish(Bool(data=True)) self.get_logger().info('STOP: nav2 and teleop disabled') - if response.state == response.RC or response.state == response.RACEDONE: - self.joy_only_publisher.publish(True) + if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE: + self.joy_only_publisher.publish(Bool(data=True)) self.get_logger().info('RC: nav2 disabled') - self.stop_publisher.publish(False) + self.stop_publisher.publish(Bool(data=False)) self.get_logger().info('RC: STOP mode restrictions lifted') - if response.state == response.AUTONOMOUS_SLOW or response.state == response.AUTONOMOUS_OVERTAKE: - self.joy_only_publisher.publish(False) + if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE: + self.joy_only_publisher.publish(Bool(data=False)) self.get_logger().info( - 'AUTO: allow both nav2 and teleop controls. Switching directly from' - 'ESTOP/SAFESTOP to this state is not allowed. Switch to RC first' + 'AUTO: allow both nav2 and teleop controls. Switching directly from ' + 'ESTOP/SAFESTOP to this state will not disable STOP. Switch to RC first' ) return response From afd528639d487e2ebbcf36db3b5e91a0018f80da Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 07:25:11 -0400 Subject: [PATCH 09/32] Add test cases tests to be made automated later, see readme in tests --- src/amp_kart_commander/README.md | 12 +++++++ .../resource/amp_kart_commander | 0 src/amp_kart_commander/test/README.md | 35 +++++++++++++++++++ 3 files changed, 47 insertions(+) create mode 100644 src/amp_kart_commander/README.md create mode 100644 src/amp_kart_commander/resource/amp_kart_commander create mode 100644 src/amp_kart_commander/test/README.md diff --git a/src/amp_kart_commander/README.md b/src/amp_kart_commander/README.md new file mode 100644 index 0000000..c873b74 --- /dev/null +++ b/src/amp_kart_commander/README.md @@ -0,0 +1,12 @@ +# Kart Commander + +Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the cmd_vel mux accordingly. + +Might consider moving the mux diagram from `bringup/launch/twist_mux` here. There's more elaboration on this node in particular there. Somebody fix the rest, + +When "teleop_only" is false and "stop" is false, then both joy and nav2 messages can coexist. +Note that while you can reach `Stop` from any other state, returning to `Autonomus` from `Stop` is not allowed; switch to RC first. + +## Tests + +See the READMe in the test folder diff --git a/src/amp_kart_commander/resource/amp_kart_commander b/src/amp_kart_commander/resource/amp_kart_commander new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md new file mode 100644 index 0000000..4161ebb --- /dev/null +++ b/src/amp_kart_commander/test/README.md @@ -0,0 +1,35 @@ +# Kart Commander Testing + +I didn't get around to implementing [launch testing](https://github.com/ros2/launch/tree/rolling/launch_testing) for kart_commander, but here are some manual tests that should be integrated into a future implementation. + +## Auto Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Verify that the default state allows for nav2 commands to show in `cmd_vel` + +## RC Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Send msgs over `joy_teleop` +- Verify that the default state allows teleop to show in `cmd_vel` +- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"` (try `checkered` too) +- Verify that the info output shows the diagnostic from kart_commander that we are in RC state. +- Verify thet nav2 commands no longer show in cmd_vel +- Send msgs over `joy_teleop` +- Verify that the RC statesallows teleop to show in `cmd_vel` + +## Emergency Stop Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: red}"` (try `black` too) +- Verify that the info output shows the diagnostic from kart_commander that we are in STOP state. +- Verify thet nav2 commands don't show in cmd_vel +- Send msgs over `joy_teleop` +- Verify thet joy commands don't show in cmd_vel + +# Not allowed to go STOP -> Auto directly + +- Following the previous test, send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` (try `yellow` too) +- Verify that no nav2 comes through 'cmd_vel' +- Switch to Teleop and then back to Auto `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"`, `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` +- - Verify that nav comes through 'cmd_vel' From 4511e3d35806032dd5af5ae63f52c4b1508e572b Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 06:46:22 -0400 Subject: [PATCH 10/32] amp_commander: Add new mux node and docs the new `switching_mux` let through one topic at a time --- src/amp_kart_bringup/README.md | 45 ++++------ .../launch/kart_commander.launch.py | 31 +++++++ .../launch/twist_mux.launch.py | 35 -------- .../params/switch_mux.params.yaml | 6 ++ .../params/twist_mux.params.yaml | 23 ----- src/amp_kart_commander/README.md | 20 ++++- .../amp_kart_commander/kart_commander.py | 29 ++----- .../amp_kart_commander/switch_mux.py | 84 +++++++++++++++++++ src/amp_kart_commander/setup.py | 6 +- src/amp_kart_commander/test/README.md | 2 +- 10 files changed, 167 insertions(+), 114 deletions(-) create mode 100644 src/amp_kart_bringup/launch/kart_commander.launch.py delete mode 100644 src/amp_kart_bringup/launch/twist_mux.launch.py create mode 100644 src/amp_kart_bringup/params/switch_mux.params.yaml delete mode 100644 src/amp_kart_bringup/params/twist_mux.params.yaml create mode 100644 src/amp_kart_commander/amp_kart_commander/switch_mux.py diff --git a/src/amp_kart_bringup/README.md b/src/amp_kart_bringup/README.md index be7e6b9..25844b2 100644 --- a/src/amp_kart_bringup/README.md +++ b/src/amp_kart_bringup/README.md @@ -4,38 +4,25 @@ Contains files needed to start the kart. ## Mux structure -There are two control sources that all produce `twist_mux` messages to "cmd_vel": joy_vel (teleop controller) and nav_vel (autnomous navigation). -The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. It's important that the states can override each other in this priority: +There are two control sources that both produce `twist_mux` messages to `cmd_vel`: joy_vel (teleop controller) and nav_vel (autnomous navigation). +The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. The kart_commander uses these states to determine which control source to let through, or neither. -- ESTOP/SAFE STOP -- TELEOP -- AUTO - -The priorities are implemented with a [`twist_mux`](http://wiki.ros.org/twist_mux). -and laid out as can be seen below. +The priorities are implemented with `switch_mux`, a node defined in `kart_commander` ``` - - - /joy_only /stop (255) - \ / - | | - _|_|_ - /joy_vel____| | - | |____ /cmd_vel - | | - /nav_vel ___| | - |_____| - + /topic_select + | + | + _|___ + /joy_vel____| | + | |____ /cmd_vel + | | + switch_mux + | | + /nav_vel ___| | + |_____| ``` -Priorities for each topic and lock are listed, as well as the expected topic publishers: - -- `stop` ~ `kart_commander`: 255 -- `joy_vel` ~ `teleop_twist_joy_node`: 50 -- `joy_only_publisher` ~ `kart_commander`: 50 -- `nav_vel` ~ `nav2`: 10 - ## Directory Tree - `config/` @@ -50,7 +37,7 @@ Priorities for each topic and lock are listed, as well as the expected topic pub - `rviz.launch.py` _Initialize RViz with Nav2 configuration_ - `slam.launch.py` _Initialize map_server and slam_toolbox_ - `teleop.launch.py` _Launch joy teleop nodes_ - - `twist_mux.launch.py` _Launch both twist muxex and kart_commander_ + - `kart_commander.launch.py` _Launch kart_commander and the twist muxes_ - `VLP16.launch.py` _Initialize Velodyne LIDAR VLP16 nodes_ - `map/` - `empty_map.yaml` _Default empty map_ @@ -58,6 +45,6 @@ Priorities for each topic and lock are listed, as well as the expected topic pub - `VLP16.params.yaml` _VLP16 parameters_ - `VLP16db.params.yaml` _VLP16 calibration parameters_ - `nav2.params.yaml` _Nav2 parameters_ - - `twist_mux.params.yaml` + - `kart_commander.params.yaml` - `rviz/` - `nav2_default_view.rviz.yaml` _RViz config for Nav2_ diff --git a/src/amp_kart_bringup/launch/kart_commander.launch.py b/src/amp_kart_bringup/launch/kart_commander.launch.py new file mode 100644 index 0000000..a63def9 --- /dev/null +++ b/src/amp_kart_bringup/launch/kart_commander.launch.py @@ -0,0 +1,31 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + params_file = os.path.join(get_package_share_directory('amp_kart_bringup'), + 'params', 'switch_mux.params.yaml') + + switch_mux_action = Node(package='amp_kart_commander', + namespace='', + executable='switch_mux', + name='switch_mux', + parameters=[params_file]) + + kart_commander_action = Node(package='amp_kart_commander', + namespace='', + executable='kart_commander', + name='kart_commander') + + ld = LaunchDescription() + ld.add_action(switch_mux_action) + ld.add_action(kart_commander_action) + + return ld diff --git a/src/amp_kart_bringup/launch/twist_mux.launch.py b/src/amp_kart_bringup/launch/twist_mux.launch.py deleted file mode 100644 index 4cc4228..0000000 --- a/src/amp_kart_bringup/launch/twist_mux.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node - - -def generate_launch_description(): - - share_dir = get_package_share_directory('amp_kart_bringup') - params_dir = os.path.join(share_dir, 'params') - - mux_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('twist_mux'), 'launch', - 'twist_mux_launch.py')), - launch_arguments={ - 'config_locks': os.path.join(params_dir, 'twist_mux.params.yaml'), - 'config_topics': os.path.join(params_dir, 'twist_mux.params.yaml'), - 'cmd_vel_out': 'cmd_vel' - }.items()) - - kart_commander_action = Node(package='amp_kart_commander', - namespace='', - executable='kart_commander', - name='sikart_commander') - - ld = LaunchDescription() - ld.add_action(mux_cmd) - ld.add_action(kart_commander_action) - - return ld diff --git a/src/amp_kart_bringup/params/switch_mux.params.yaml b/src/amp_kart_bringup/params/switch_mux.params.yaml new file mode 100644 index 0000000..661b865 --- /dev/null +++ b/src/amp_kart_bringup/params/switch_mux.params.yaml @@ -0,0 +1,6 @@ +switch_mux: + ros__parameters: + input_topics: + - joy_vel + - nav_vel + initial_topic: __none diff --git a/src/amp_kart_bringup/params/twist_mux.params.yaml b/src/amp_kart_bringup/params/twist_mux.params.yaml deleted file mode 100644 index 48c0808..0000000 --- a/src/amp_kart_bringup/params/twist_mux.params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -twist_mux: - ros__parameters: - # Make sure the locks and corresponding locked topics have the same priority - # When the kart_commander has not explicitly disallowed auto, - # this config allows for teleop and auto to happen simultaneously (in alternating periods of 0.5s) - locks: - joy_enable: - topic: joy_enable - timeout: 0.0 - priority: 50 - stop: - topic: stop - timeout: 0.0 - priority: 255 - topics: - joy_vel: - topic: joy_vel - timeout: 0.5 - priority: 50 - navigation: - topic: nav_vel - timeout: 0.5 - priority: 10 diff --git a/src/amp_kart_commander/README.md b/src/amp_kart_commander/README.md index c873b74..0aa76ca 100644 --- a/src/amp_kart_commander/README.md +++ b/src/amp_kart_commander/README.md @@ -1,12 +1,24 @@ # Kart Commander -Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the cmd_vel mux accordingly. +Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the switch_mux accordingly. Might consider moving the mux diagram from `bringup/launch/twist_mux` here. There's more elaboration on this node in particular there. Somebody fix the rest, -When "teleop_only" is false and "stop" is false, then both joy and nav2 messages can coexist. -Note that while you can reach `Stop` from any other state, returning to `Autonomus` from `Stop` is not allowed; switch to RC first. +Note that while you can reach `Stop` from any other state, returning to `Autonomous` from `Stop` is not allowed; switch to RC first. + +## `switch_mux` + +An one-in, one-out mux for `Twist` messages, since ROS2 does not support generic message types in Foxy. +Publishes to a `Twist` topic named "cmd_vel" and takes in a configurable list of `Twist` topics + +- Parameters + - `initial_topic`: (optional) name of the initial `Twist` topic to pass through + - `input_topics`: list of `Twist` topics to listen for +- Subscribed Topics + - `select_topic`: `String` name of topic from `input_topics` to pass through, or `__none`, ehich explicitly turns off the mux +- Published Topics + - `cmd_vel`: Twist topic output chosen from the inputs, or nothing at all ## Tests -See the READMe in the test folder +See the README in the test folder diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index d748bfe..2248af5 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -4,7 +4,7 @@ from amp_msgs.srv import TrackState import rclpy from rclpy.node import Node -from std_msgs.msg import Bool +from std_msgs.msg import String from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy @@ -24,11 +24,9 @@ def __init__(self): super().__init__('service_server') self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) - self.stop_publisher = self.create_publisher(Bool, - 'stop', - qos_profile=qos_profile) - self.joy_only_publisher = self.create_publisher( - Bool, 'joy_only', qos_profile=qos_profile) + self.mux_publisher = self.create_publisher(String, + 'select_topic', + qos_profile=qos_profile) def track_state_callback(self, request, response): response.state = request.state @@ -40,24 +38,15 @@ def track_state_callback(self, request, response): # TODO More complex states in TrackState.srv have transition rules not yet implemented - # When "teleop_only" is false and "stop" is false, then both joy and nav2 messages - # can coexist. - # Note that while you can reach Stop from any other state, returning to - # Autonomus from Stop is not allowed; switch to RC first. if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP: - self.stop_publisher.publish(Bool(data=True)) + self.mux_publisher.publish(String(data='__none')) self.get_logger().info('STOP: nav2 and teleop disabled') if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE: - self.joy_only_publisher.publish(Bool(data=True)) - self.get_logger().info('RC: nav2 disabled') - self.stop_publisher.publish(Bool(data=False)) - self.get_logger().info('RC: STOP mode restrictions lifted') + self.mux_publisher.publish(String(data='joy_vel')) + self.get_logger().info('RC: joy only') if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE: - self.joy_only_publisher.publish(Bool(data=False)) - self.get_logger().info( - 'AUTO: allow both nav2 and teleop controls. Switching directly from ' - 'ESTOP/SAFESTOP to this state will not disable STOP. Switch to RC first' - ) + self.mux_publisher.publish(String(data='nav_vel')) + self.get_logger().info('AUTO: nav2 only') return response diff --git a/src/amp_kart_commander/amp_kart_commander/switch_mux.py b/src/amp_kart_commander/amp_kart_commander/switch_mux.py new file mode 100644 index 0000000..f9b4a88 --- /dev/null +++ b/src/amp_kart_commander/amp_kart_commander/switch_mux.py @@ -0,0 +1,84 @@ +''' Mux the allows choosing a single topic to let through ''' +#usr/bin/env python3 +import rclpy +import functools + +from rclpy.node import Node +from std_msgs.msg import String +from geometry_msgs.msg import Twist +from rclpy.exceptions import ParameterNotDeclaredException + +from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy +# Create lossy QoS that is compatible with any publisher +subscriber_any = QoSProfile(history=HistoryPolicy.KEEP_LAST, + depth=5, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE) + +# Create high-reliability QoS for topic selection +reliable = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, # Last always has priority anyways + reliability=ReliabilityPolicy.RELIABLE, # Guaranteed delivery + durability=DurabilityPolicy.TRANSIENT_LOCAL # Re-send msg to late-joiners +) + +EMPTY_TOPIC = "__none" + + +class SwitchMux(Node): + + def __init__(self): + super().__init__('switch_mux', allow_undeclared_parameters=True) + + self.declare_parameter('initial_topic', String) + self.declare_parameter('input_topics') + + # `initial_topic` is optionally defined at initialization + try: + self.current_topic = self.get_parameter('initial_topic').value + except ParameterNotDeclaredException: + self.current_topic = EMPTY_TOPIC + + # `input_topics` must be defined at initialization + self.input_topics = self._parameters['input_topics'].value + if self.input_topics is None: + raise ParameterNotDeclaredException('input_topics') + + for topic_name in self.input_topics: + # This specific name is disallowed to guarantee that it disables the mux + if topic_name == EMPTY_TOPIC: + continue + # All input topics share the same base callback (input_topic_callback), but we + # bind a third `topic_name` parameter that changes with the topic + named_callback = functools.partial(self.input_topic_callback, + topic_name) + self.create_subscription(Twist, topic_name, named_callback, + subscriber_any) + + # Topic name + self.subscription = self.create_subscription(String, 'select_topic', + self.set_topic_callback, + reliable) + self.publisher = self.create_publisher(Twist, 'cmd_vel', reliable) + + def set_topic_callback(self, topic_name_msg): + self.current_topic = str(topic_name_msg.data) + + def input_topic_callback(self, topic_name, twist_msg): + ''' Upon receiving a message to any registered input of the mux''' + self.get_logger().info( + f"got from {topic_name} curr is {self.current_topic}") + if topic_name == self.current_topic: + self.publisher.publish(twist_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SwitchMux() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == 'main': + main() diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py index daf9862..bd9e012 100644 --- a/src/amp_kart_commander/setup.py +++ b/src/amp_kart_commander/setup.py @@ -19,7 +19,9 @@ license='TODO: License declaration', tests_require=['pytest'], entry_points={ - 'console_scripts': - ['kart_commander = amp_kart_commander.kart_commander:main'], + 'console_scripts': [ + 'kart_commander = amp_kart_commander.kart_commander:main', + 'switch_mux = amp_kart_commander.switch_mux:main' + ], }, ) diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md index 4161ebb..90afc87 100644 --- a/src/amp_kart_commander/test/README.md +++ b/src/amp_kart_commander/test/README.md @@ -5,7 +5,7 @@ I didn't get around to implementing [launch testing](https://github.com/ros2/lau ## Auto Works - Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Verify that the default state allows for nav2 commands to show in `cmd_vel` +- Verify that the default state does not allow nav2 commands to show in `cmd_vel` ## RC Works From 59f684be1d65be595cc760df8ba6e0d6e2bb4957 Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 06:51:04 -0400 Subject: [PATCH 11/32] amp_commander: remove debug log remove debug logger line --- src/amp_kart_commander/amp_kart_commander/switch_mux.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/switch_mux.py b/src/amp_kart_commander/amp_kart_commander/switch_mux.py index f9b4a88..cfc1230 100644 --- a/src/amp_kart_commander/amp_kart_commander/switch_mux.py +++ b/src/amp_kart_commander/amp_kart_commander/switch_mux.py @@ -67,8 +67,6 @@ def set_topic_callback(self, topic_name_msg): def input_topic_callback(self, topic_name, twist_msg): ''' Upon receiving a message to any registered input of the mux''' - self.get_logger().info( - f"got from {topic_name} curr is {self.current_topic}") if topic_name == self.current_topic: self.publisher.publish(twist_msg) From 7438da356a31401c505112a92ab09a4297fd4d5b Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 07:04:16 -0400 Subject: [PATCH 12/32] amp_lint install scripts breaking ci install-scripts to install_scripts --- src/amp_kart_commander/setup.cfg | 2 +- src/amp_rcs_client/setup.cfg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg index 539f781..e42791e 100644 --- a/src/amp_kart_commander/setup.cfg +++ b/src/amp_kart_commander/setup.cfg @@ -1,4 +1,4 @@ [develop] script-dir=$base/lib/amp_kart_commander [install] -install-scripts=$base/lib/amp_kart_commander +install_scripts=$base/lib/amp_kart_commander diff --git a/src/amp_rcs_client/setup.cfg b/src/amp_rcs_client/setup.cfg index d920b74..26851ca 100644 --- a/src/amp_rcs_client/setup.cfg +++ b/src/amp_rcs_client/setup.cfg @@ -1,4 +1,4 @@ [develop] script-dir=$base/lib/amp_rcs [install] -install-scripts=$base/lib/amp_rcs +install_scripts=$base/lib/amp_rcs From abddec27a2b0d1bc3cd500e3715839b981d98e0f Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 20:14:49 -0400 Subject: [PATCH 13/32] amp_lint: appease ci script_dir with an underscore --- src/amp_kart_commander/setup.cfg | 2 +- src/amp_rcs_client/setup.cfg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg index e42791e..ffddaf8 100644 --- a/src/amp_kart_commander/setup.cfg +++ b/src/amp_kart_commander/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/amp_kart_commander +script_dir=$base/lib/amp_kart_commander [install] install_scripts=$base/lib/amp_kart_commander diff --git a/src/amp_rcs_client/setup.cfg b/src/amp_rcs_client/setup.cfg index 26851ca..ffd6879 100644 --- a/src/amp_rcs_client/setup.cfg +++ b/src/amp_rcs_client/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/amp_rcs +script_dir=$base/lib/amp_rcs [install] install_scripts=$base/lib/amp_rcs From 341b0fa27a0d1d9d7fba1c4eb5aa91916b2ba33a Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 20:51:53 -0400 Subject: [PATCH 14/32] amp_ci: remove tests they were causing ci to fail --- src/amp_kart_commander/package.xml | 5 --- src/amp_kart_commander/setup.py | 1 - src/amp_kart_commander/test/README.md | 35 ------------------- src/amp_kart_commander/test/test_copyright.py | 23 ------------ src/amp_kart_commander/test/test_flake8.py | 25 ------------- src/amp_kart_commander/test/test_pep257.py | 23 ------------ src/amp_rcs_client/amp_rcs/dummy_server.py | 4 +-- src/amp_rcs_client/amp_rcs/service_client.py | 5 +-- src/amp_rcs_client/setup.py | 1 - src/amp_rcs_client/test/test_copyright.py | 23 ------------ src/amp_rcs_client/test/test_flake8.py | 25 ------------- src/amp_rcs_client/test/test_pep257.py | 23 ------------ 12 files changed, 2 insertions(+), 191 deletions(-) delete mode 100644 src/amp_kart_commander/test/README.md delete mode 100644 src/amp_kart_commander/test/test_copyright.py delete mode 100644 src/amp_kart_commander/test/test_flake8.py delete mode 100644 src/amp_kart_commander/test/test_pep257.py delete mode 100644 src/amp_rcs_client/test/test_copyright.py delete mode 100644 src/amp_rcs_client/test/test_flake8.py delete mode 100644 src/amp_rcs_client/test/test_pep257.py diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml index d6a5c7a..b5b9d4f 100644 --- a/src/amp_kart_commander/package.xml +++ b/src/amp_kart_commander/package.xml @@ -7,11 +7,6 @@ lucy TODO: License declaration - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - ament_python diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py index bd9e012..f49b6fb 100644 --- a/src/amp_kart_commander/setup.py +++ b/src/amp_kart_commander/setup.py @@ -17,7 +17,6 @@ maintainer_email='xchenbox@gmail.com', description='TODO: Package description', license='TODO: License declaration', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'kart_commander = amp_kart_commander.kart_commander:main', diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md deleted file mode 100644 index 90afc87..0000000 --- a/src/amp_kart_commander/test/README.md +++ /dev/null @@ -1,35 +0,0 @@ -# Kart Commander Testing - -I didn't get around to implementing [launch testing](https://github.com/ros2/launch/tree/rolling/launch_testing) for kart_commander, but here are some manual tests that should be integrated into a future implementation. - -## Auto Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Verify that the default state does not allow nav2 commands to show in `cmd_vel` - -## RC Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Send msgs over `joy_teleop` -- Verify that the default state allows teleop to show in `cmd_vel` -- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"` (try `checkered` too) -- Verify that the info output shows the diagnostic from kart_commander that we are in RC state. -- Verify thet nav2 commands no longer show in cmd_vel -- Send msgs over `joy_teleop` -- Verify that the RC statesallows teleop to show in `cmd_vel` - -## Emergency Stop Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: red}"` (try `black` too) -- Verify that the info output shows the diagnostic from kart_commander that we are in STOP state. -- Verify thet nav2 commands don't show in cmd_vel -- Send msgs over `joy_teleop` -- Verify thet joy commands don't show in cmd_vel - -# Not allowed to go STOP -> Auto directly - -- Following the previous test, send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` (try `yellow` too) -- Verify that no nav2 comes through 'cmd_vel' -- Switch to Teleop and then back to Auto `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"`, `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` -- - Verify that nav comes through 'cmd_vel' diff --git a/src/amp_kart_commander/test/test_copyright.py b/src/amp_kart_commander/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/src/amp_kart_commander/test/test_copyright.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_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/amp_kart_commander/test/test_flake8.py b/src/amp_kart_commander/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/amp_kart_commander/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/src/amp_kart_commander/test/test_pep257.py b/src/amp_kart_commander/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/amp_kart_commander/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' diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index ea92162..0a0abe5 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -1,10 +1,8 @@ -'''ROS2 service server for behavioral determination based on client call''' +"""ROS2 service server for behavioral determination based on client call""" -#usr/bin/env python3 from rcs_service.srv import TrackState import rclpy from rclpy.node import Node -from std_msgs.msg import String class ServiceServer(Node): diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py index e5fc494..d1762fc 100644 --- a/src/amp_rcs_client/amp_rcs/service_client.py +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -1,10 +1,7 @@ -'''ROS2 service client for state based service calling''' +"""ROS2 service client for state based service calling""" -#usr/bin/env python3 from amp_msgs.srv import TrackState import rclpy -import sys - from rclpy.node import Node from std_msgs.msg import String diff --git a/src/amp_rcs_client/setup.py b/src/amp_rcs_client/setup.py index 9e5f3d3..62c4b0f 100644 --- a/src/amp_rcs_client/setup.py +++ b/src/amp_rcs_client/setup.py @@ -18,7 +18,6 @@ description= 'Service client for sent track state change requests to the service server', license='GNU General Public License 3.0', - tests_require=['pytest'], entry_points={ 'console_scripts': [ "amp_rcs_client = amp_rcs.service_client:main", diff --git a/src/amp_rcs_client/test/test_copyright.py b/src/amp_rcs_client/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/src/amp_rcs_client/test/test_copyright.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_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/amp_rcs_client/test/test_flake8.py b/src/amp_rcs_client/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/amp_rcs_client/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/src/amp_rcs_client/test/test_pep257.py b/src/amp_rcs_client/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/amp_rcs_client/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' From fbca70a93dcb7e59cbf24a367acfe5712fb87291 Mon Sep 17 00:00:00 2001 From: berliernj Date: Wed, 12 Apr 2023 15:51:28 -0400 Subject: [PATCH 15/32] Initial Commit --- src/amp_msgs/package.xml | 1 - src/amp_rcs_client/test/test_copyright.py | 23 +++++++++++++++++++++ src/amp_rcs_client/test/test_flake8.py | 25 +++++++++++++++++++++++ src/amp_rcs_client/test/test_pep257.py | 23 +++++++++++++++++++++ 4 files changed, 71 insertions(+), 1 deletion(-) create mode 100644 src/amp_rcs_client/test/test_copyright.py create mode 100644 src/amp_rcs_client/test/test_flake8.py create mode 100644 src/amp_rcs_client/test/test_pep257.py diff --git a/src/amp_msgs/package.xml b/src/amp_msgs/package.xml index 09a7d10..8548ac2 100644 --- a/src/amp_msgs/package.xml +++ b/src/amp_msgs/package.xml @@ -12,7 +12,6 @@ rosidl_default_runtime rosidl_interface_packages - ament_cmake diff --git a/src/amp_rcs_client/test/test_copyright.py b/src/amp_rcs_client/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/amp_rcs_client/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/amp_rcs_client/test/test_flake8.py b/src/amp_rcs_client/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/amp_rcs_client/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/src/amp_rcs_client/test/test_pep257.py b/src/amp_rcs_client/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/amp_rcs_client/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' From eb4ca8488743f2510b5058a997655538c245e22b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 12 Apr 2023 20:01:47 +0000 Subject: [PATCH 16/32] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/amp_rcs_client/amp_rcs/dummy_server.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index b031bcf..292ce0e 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -5,6 +5,7 @@ from rclpy.node import Node + class ServiceServer(Node): def __init__(self): From 1512010d3f0b5436d6d8b694512385f1da8b0f0f Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:40:06 -0400 Subject: [PATCH 17/32] kart_commander node initial Added QoS profile and state transition rules --- .../amp_kart_commander/__init__.py | 0 .../amp_kart_commander/kart_commander.py | 71 +++++++++++++++++++ src/amp_kart_commander/package.xml | 18 +++++ src/amp_kart_commander/setup.cfg | 4 ++ src/amp_kart_commander/setup.py | 25 +++++++ src/amp_kart_commander/test/test_copyright.py | 23 ++++++ src/amp_kart_commander/test/test_flake8.py | 25 +++++++ src/amp_kart_commander/test/test_pep257.py | 23 ++++++ 8 files changed, 189 insertions(+) create mode 100644 src/amp_kart_commander/amp_kart_commander/__init__.py create mode 100644 src/amp_kart_commander/amp_kart_commander/kart_commander.py create mode 100644 src/amp_kart_commander/package.xml create mode 100644 src/amp_kart_commander/setup.cfg create mode 100644 src/amp_kart_commander/setup.py create mode 100644 src/amp_kart_commander/test/test_copyright.py create mode 100644 src/amp_kart_commander/test/test_flake8.py create mode 100644 src/amp_kart_commander/test/test_pep257.py diff --git a/src/amp_kart_commander/amp_kart_commander/__init__.py b/src/amp_kart_commander/amp_kart_commander/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py new file mode 100644 index 0000000..5fee07d --- /dev/null +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -0,0 +1,71 @@ +'''Server that receives updates to TrackState from RCS and sets the kart cmd_vel mux accordingly''' + +#usr/bin/env python3 +from amp_msgs.srv import TrackState +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool + +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy + +# Create high-reliability QoS for mux toggling +qos_profile = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, # Last always has priority anyways + reliability=ReliabilityPolicy.RELIABLE, # Guaranteed delivery + durability=DurabilityPolicy. + TRANSIENT_LOCAL # Re-send msg to late-joining subscribers +) + + +class ServiceServer(Node): + + def __init__(self): + super().__init__('service_server') + self.server = self.create_service(TrackState, 'track_state', + self.track_state_callback) + self.stop_publisher = self.create_publisher(Bool, + 'stop', + qos_profile=qos_profile) + self.joy_only_publisher = self.create_publisher( + Bool, 'joy_only', qos_profile=qos_profile) + + def track_state_callback(self, request, response): + response.state = request.state + + self.get_logger().info('Recieved: %s' % (request.state)) + self.get_logger().info('Response: %s\n' % (response.state)) + + # TODO More complex states in TrackState.srv have transition rules not yet implemented + + # When "teleop_only" is false and "stop" is false, then both joy and nav2 messages + # can coexist. + # Note that while you can reach Stop from any other state, returning to + # Autonomus from Stop is not allowed; switch to RC first. + if response.state == response.ESTOP or response.state == response.SAFESTOP: + self.stop_publisher.publish(True) + self.get_logger().info('STOP: nav2 and teleop disabled') + if response.state == response.RC or response.state == response.RACEDONE: + self.joy_only_publisher.publish(True) + self.get_logger().info('RC: nav2 disabled') + self.stop_publisher.publish(False) + self.get_logger().info('RC: STOP mode restrictions lifted') + if response.state == response.AUTONOMOUS_SLOW or response.state == response.AUTONOMOUS_OVERTAKE: + self.joy_only_publisher.publish(False) + self.get_logger().info( + 'AUTO: allow both nav2 and teleop controls. Switching directly from' + 'ESTOP/SAFESTOP to this state is not allowed. Switch to RC first' + ) + + return response + + +def main(args=None): + rclpy.init(args=args) + node = ServiceServer() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == 'main': + main() diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml new file mode 100644 index 0000000..d6a5c7a --- /dev/null +++ b/src/amp_kart_commander/package.xml @@ -0,0 +1,18 @@ + + + + amp_kart_commander + 0.0.0 + Updates cmd_vel max based on TrackState changes + lucy + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg new file mode 100644 index 0000000..539f781 --- /dev/null +++ b/src/amp_kart_commander/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/amp_kart_commander +[install] +install-scripts=$base/lib/amp_kart_commander diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py new file mode 100644 index 0000000..daf9862 --- /dev/null +++ b/src/amp_kart_commander/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'amp_kart_commander' + +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='lucy', + maintainer_email='xchenbox@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': + ['kart_commander = amp_kart_commander.kart_commander:main'], + }, +) diff --git a/src/amp_kart_commander/test/test_copyright.py b/src/amp_kart_commander/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/src/amp_kart_commander/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/amp_kart_commander/test/test_flake8.py b/src/amp_kart_commander/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/amp_kart_commander/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/src/amp_kart_commander/test/test_pep257.py b/src/amp_kart_commander/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/amp_kart_commander/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' From b0134cbbeaa801eb9090c113faf4e7e53fc5c00e Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:41:33 -0400 Subject: [PATCH 18/32] kChange twist setup to single mux in hindsight it was simpler. If one mux fails both are likely screwed --- src/amp_kart_bringup/README.md | 55 ++++++++++--------- .../launch/twist_mux.launch.py | 31 ++++------- ...ux_2.params.yaml => twist_mux.params.yaml} | 19 ++++--- .../params/twist_mux_1.params.yaml | 12 ---- 4 files changed, 50 insertions(+), 67 deletions(-) rename src/amp_kart_bringup/params/{twist_mux_2.params.yaml => twist_mux.params.yaml} (50%) delete mode 100644 src/amp_kart_bringup/params/twist_mux_1.params.yaml diff --git a/src/amp_kart_bringup/README.md b/src/amp_kart_bringup/README.md index 1fbf121..8edda29 100644 --- a/src/amp_kart_bringup/README.md +++ b/src/amp_kart_bringup/README.md @@ -4,35 +4,37 @@ Contains files needed to start the kart. ## Mux structure -The muxes used are [`twist_mux`](http://wiki.ros.org/twist_mux). -They are laid out as can be seen below. +There are two control sources that all produce `twist_mux` messages to "cmd_vel": joy_vel (teleop controller) and nav_vel (autnomous navigation). +The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. It's important that the states can override each other in this priority: + +- ESTOP/SAFE STOP +- TELEOP +- AUTO + +The priorities are implemented with a [`twist_mux`](http://wiki.ros.org/twist_mux). +and laid out as can be seen below. ``` - /joy_disable /joy_enable - | | - | | /stop - ____ | | - | | /mux1_vel ____ - /joy_vel ____| |_____________| | - | | | | - |____| | |____ /cmd_vel - mux1 | | - /nav_vel ___| | - |____| - mux2 + + + /joy_only /stop (255) + \ / + | | + _|_|_ + /joy_vel____| | + | |____ /cmd_vel + | | + /nav_vel ___| | + |_____| + ``` -Priorities for the muxes are listed below, as well as the corresponding topic -and (expected) topic publishers: +Priorities for each topic and lock are listed, as well as the expected topic publishers: -- `mux1` → `/mux1_vel` - - `joy_disable` ~ `kart_commander`: 150 - - `joy_vel` ~ `teleop_twist_joy_node`: 100 -- `mux2` → `/cmd_vel` - - `stop` ~ `kart_commander`: 255 - - `joy_enable` ~ `kart_commander`: 50 - - `nav_vel` ~ `nav2`: 10 - - `mux1_vel` ~ `mux1`: 100 +- `stop` ~ `kart_commander`: 255 +- `joy_vel` ~ `teleop_twist_joy_node`: 50 +- `joy_only_publisher` ~ `kart_commander`: 50 +- `nav_vel` ~ `nav2`: 10 ## Directory Tree @@ -48,7 +50,7 @@ and (expected) topic publishers: - `rviz.launch.py` _Initialize RViz with Nav2 configuration_ - `slam.launch.py` _Initialize map_server and slam_toolbox_ - `teleop.launch.py` _Launch joy teleop nodes_ - - `twist_mux.launch.py` _Launch both twist muxex_ + - `twist_mux.launch.py` _Launch both twist muxex and kart_commander_ - `VLP16.launch.py` _Initialize Velodyne LIDAR VLP16 nodes_ - `zed.launch.py` _Zed camera launch file_ - `map/` @@ -58,7 +60,6 @@ and (expected) topic publishers: - `VLP16db.params.yaml` _VLP16 calibration parameters_ - `nav2.params.yaml` _Nav2 parameters_ - `patchworkpp.params.yaml` _Patchwork++ parameters_ - - `twist_mux_1.params.yaml` - - `twist_mux_2.params.yaml` + - `twist_mux.params.yaml` - `rviz/` - `nav2_default_view.rviz.yaml` _RViz config for Nav2_ diff --git a/src/amp_kart_bringup/launch/twist_mux.launch.py b/src/amp_kart_bringup/launch/twist_mux.launch.py index 82dbc6d..4cc4228 100644 --- a/src/amp_kart_bringup/launch/twist_mux.launch.py +++ b/src/amp_kart_bringup/launch/twist_mux.launch.py @@ -5,6 +5,7 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node def generate_launch_description(): @@ -12,33 +13,23 @@ def generate_launch_description(): share_dir = get_package_share_directory('amp_kart_bringup') params_dir = os.path.join(share_dir, 'params') - mux1_cmd = IncludeLaunchDescription( + mux_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('twist_mux'), 'launch', 'twist_mux_launch.py')), launch_arguments={ - 'config_locks': os.path.join(params_dir, - 'twist_mux_1.params.yaml'), - 'config_topics': os.path.join(params_dir, - 'twist_mux_1.params.yaml'), - 'cmd_vel_out': 'mux1_vel' - }.items()) - - mux2_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('twist_mux'), 'launch', - 'twist_mux_launch.py')), - launch_arguments={ - 'config_locks': os.path.join(params_dir, - 'twist_mux_2.params.yaml'), - 'config_topics': os.path.join(params_dir, - 'twist_mux_2.params.yaml'), + 'config_locks': os.path.join(params_dir, 'twist_mux.params.yaml'), + 'config_topics': os.path.join(params_dir, 'twist_mux.params.yaml'), 'cmd_vel_out': 'cmd_vel' }.items()) - ld = LaunchDescription() + kart_commander_action = Node(package='amp_kart_commander', + namespace='', + executable='kart_commander', + name='sikart_commander') - ld.add_action(mux1_cmd) - ld.add_action(mux2_cmd) + ld = LaunchDescription() + ld.add_action(mux_cmd) + ld.add_action(kart_commander_action) return ld diff --git a/src/amp_kart_bringup/params/twist_mux_2.params.yaml b/src/amp_kart_bringup/params/twist_mux.params.yaml similarity index 50% rename from src/amp_kart_bringup/params/twist_mux_2.params.yaml rename to src/amp_kart_bringup/params/twist_mux.params.yaml index fd15712..48c0808 100644 --- a/src/amp_kart_bringup/params/twist_mux_2.params.yaml +++ b/src/amp_kart_bringup/params/twist_mux.params.yaml @@ -1,20 +1,23 @@ twist_mux: ros__parameters: + # Make sure the locks and corresponding locked topics have the same priority + # When the kart_commander has not explicitly disallowed auto, + # this config allows for teleop and auto to happen simultaneously (in alternating periods of 0.5s) locks: - stop: - topic: stop - timeout: 0.0 - priority: 255 joy_enable: topic: joy_enable timeout: 0.0 priority: 50 + stop: + topic: stop + timeout: 0.0 + priority: 255 topics: + joy_vel: + topic: joy_vel + timeout: 0.5 + priority: 50 navigation: topic: nav_vel timeout: 0.5 priority: 10 - mux1: - topic: mux1_vel - timeout: 0.5 - priority: 100 diff --git a/src/amp_kart_bringup/params/twist_mux_1.params.yaml b/src/amp_kart_bringup/params/twist_mux_1.params.yaml deleted file mode 100644 index 9e8b747..0000000 --- a/src/amp_kart_bringup/params/twist_mux_1.params.yaml +++ /dev/null @@ -1,12 +0,0 @@ -twist_mux: - ros__parameters: - locks: - joy_disable: - topic: joy_disable - timeout: 0.0 - priority: 150 - topics: - joy_vel: - topic: joy_vel - timeout: 0.5 - priority: 100 From a6a320bff7fef196baaf293217967c8920db3b5e Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:43:07 -0400 Subject: [PATCH 19/32] Add constants for TrackState and KartState response See the AKS server for the state graph --- src/amp_msgs/srv/TrackState.srv | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/amp_msgs/srv/TrackState.srv b/src/amp_msgs/srv/TrackState.srv index eb25ad0..f698fd7 100644 --- a/src/amp_msgs/srv/TrackState.srv +++ b/src/amp_msgs/srv/TrackState.srv @@ -1,3 +1,19 @@ string state + +# Track State constants (Received from server) +# TODO set the values to the actual RCS strings +string EMERGENCY="black" +string SAFESTOP="red" +string RACEDONE="checkered" +string RC="orange" +string AUTONOMOUS_SLOW="yellow" +string AUTONOMOUS_OVERTAKE="green" --- string state + +# Kart State constants (Sent back to server) +string EMERGENCY="todo1" +string STOPPED="todo2" +string RC="todo3" +string READY="todo4" +string AUTONOMOUS="todo5" From 88283cc43d3f0abb082ee3e304254db1226d69d2 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 06:44:01 -0400 Subject: [PATCH 20/32] Changed trackstate srv package to match the new name It was using the rcs_service package before --- src/amp_rcs_client/amp_rcs/service_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py index 8d289d7..7ea9662 100644 --- a/src/amp_rcs_client/amp_rcs/service_client.py +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -1,6 +1,7 @@ """ROS2 service client for state based service calling.""" -from rcs_service.srv import TrackState +#usr/bin/env python3 +from amp_msgs.srv import TrackState import rclpy from rclpy.node import Node from std_msgs.msg import String From 0114b40eea5fc3f161f2fa6039a485531acdaf46 Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 07:19:46 -0400 Subject: [PATCH 21/32] bug fixes during testing No documentation for constants needed some trial n error --- .../amp_kart_commander/kart_commander.py | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index 5fee07d..d748bfe 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -6,7 +6,7 @@ from rclpy.node import Node from std_msgs.msg import Bool -from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy +from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy # Create high-reliability QoS for mux toggling qos_profile = QoSProfile( @@ -33,8 +33,10 @@ def __init__(self): def track_state_callback(self, request, response): response.state = request.state - self.get_logger().info('Recieved: %s' % (request.state)) - self.get_logger().info('Response: %s\n' % (response.state)) + self.get_logger().info('Recieved: %s Response: %s' % + (request.state, response.state)) + + # self.get_logger().info('eeee' + str(dir(TrackState.Request))) # TODO More complex states in TrackState.srv have transition rules not yet implemented @@ -42,19 +44,19 @@ def track_state_callback(self, request, response): # can coexist. # Note that while you can reach Stop from any other state, returning to # Autonomus from Stop is not allowed; switch to RC first. - if response.state == response.ESTOP or response.state == response.SAFESTOP: - self.stop_publisher.publish(True) + if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP: + self.stop_publisher.publish(Bool(data=True)) self.get_logger().info('STOP: nav2 and teleop disabled') - if response.state == response.RC or response.state == response.RACEDONE: - self.joy_only_publisher.publish(True) + if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE: + self.joy_only_publisher.publish(Bool(data=True)) self.get_logger().info('RC: nav2 disabled') - self.stop_publisher.publish(False) + self.stop_publisher.publish(Bool(data=False)) self.get_logger().info('RC: STOP mode restrictions lifted') - if response.state == response.AUTONOMOUS_SLOW or response.state == response.AUTONOMOUS_OVERTAKE: - self.joy_only_publisher.publish(False) + if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE: + self.joy_only_publisher.publish(Bool(data=False)) self.get_logger().info( - 'AUTO: allow both nav2 and teleop controls. Switching directly from' - 'ESTOP/SAFESTOP to this state is not allowed. Switch to RC first' + 'AUTO: allow both nav2 and teleop controls. Switching directly from ' + 'ESTOP/SAFESTOP to this state will not disable STOP. Switch to RC first' ) return response From a01278685e226890f72858f95b9976a34611aadd Mon Sep 17 00:00:00 2001 From: reschivon Date: Sun, 16 Apr 2023 07:25:11 -0400 Subject: [PATCH 22/32] Add test cases tests to be made automated later, see readme in tests --- src/amp_kart_commander/README.md | 12 +++++++ .../resource/amp_kart_commander | 0 src/amp_kart_commander/test/README.md | 35 +++++++++++++++++++ 3 files changed, 47 insertions(+) create mode 100644 src/amp_kart_commander/README.md create mode 100644 src/amp_kart_commander/resource/amp_kart_commander create mode 100644 src/amp_kart_commander/test/README.md diff --git a/src/amp_kart_commander/README.md b/src/amp_kart_commander/README.md new file mode 100644 index 0000000..c873b74 --- /dev/null +++ b/src/amp_kart_commander/README.md @@ -0,0 +1,12 @@ +# Kart Commander + +Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the cmd_vel mux accordingly. + +Might consider moving the mux diagram from `bringup/launch/twist_mux` here. There's more elaboration on this node in particular there. Somebody fix the rest, + +When "teleop_only" is false and "stop" is false, then both joy and nav2 messages can coexist. +Note that while you can reach `Stop` from any other state, returning to `Autonomus` from `Stop` is not allowed; switch to RC first. + +## Tests + +See the READMe in the test folder diff --git a/src/amp_kart_commander/resource/amp_kart_commander b/src/amp_kart_commander/resource/amp_kart_commander new file mode 100644 index 0000000..e69de29 diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md new file mode 100644 index 0000000..4161ebb --- /dev/null +++ b/src/amp_kart_commander/test/README.md @@ -0,0 +1,35 @@ +# Kart Commander Testing + +I didn't get around to implementing [launch testing](https://github.com/ros2/launch/tree/rolling/launch_testing) for kart_commander, but here are some manual tests that should be integrated into a future implementation. + +## Auto Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Verify that the default state allows for nav2 commands to show in `cmd_vel` + +## RC Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Send msgs over `joy_teleop` +- Verify that the default state allows teleop to show in `cmd_vel` +- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"` (try `checkered` too) +- Verify that the info output shows the diagnostic from kart_commander that we are in RC state. +- Verify thet nav2 commands no longer show in cmd_vel +- Send msgs over `joy_teleop` +- Verify that the RC statesallows teleop to show in `cmd_vel` + +## Emergency Stop Works + +- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` +- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: red}"` (try `black` too) +- Verify that the info output shows the diagnostic from kart_commander that we are in STOP state. +- Verify thet nav2 commands don't show in cmd_vel +- Send msgs over `joy_teleop` +- Verify thet joy commands don't show in cmd_vel + +# Not allowed to go STOP -> Auto directly + +- Following the previous test, send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` (try `yellow` too) +- Verify that no nav2 comes through 'cmd_vel' +- Switch to Teleop and then back to Auto `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"`, `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` +- - Verify that nav comes through 'cmd_vel' From 2ef90077f5a71cc5c55a27a09b8275fc8fd0efde Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 06:46:22 -0400 Subject: [PATCH 23/32] amp_commander: Add new mux node and docs the new `switching_mux` let through one topic at a time --- src/amp_kart_bringup/README.md | 45 ++++------ .../launch/kart_commander.launch.py | 31 +++++++ .../launch/twist_mux.launch.py | 35 -------- .../params/switch_mux.params.yaml | 6 ++ .../params/twist_mux.params.yaml | 23 ----- src/amp_kart_commander/README.md | 20 ++++- .../amp_kart_commander/kart_commander.py | 29 ++----- .../amp_kart_commander/switch_mux.py | 84 +++++++++++++++++++ src/amp_kart_commander/setup.py | 6 +- src/amp_kart_commander/test/README.md | 2 +- 10 files changed, 167 insertions(+), 114 deletions(-) create mode 100644 src/amp_kart_bringup/launch/kart_commander.launch.py delete mode 100644 src/amp_kart_bringup/launch/twist_mux.launch.py create mode 100644 src/amp_kart_bringup/params/switch_mux.params.yaml delete mode 100644 src/amp_kart_bringup/params/twist_mux.params.yaml create mode 100644 src/amp_kart_commander/amp_kart_commander/switch_mux.py diff --git a/src/amp_kart_bringup/README.md b/src/amp_kart_bringup/README.md index 8edda29..673c7ea 100644 --- a/src/amp_kart_bringup/README.md +++ b/src/amp_kart_bringup/README.md @@ -4,38 +4,25 @@ Contains files needed to start the kart. ## Mux structure -There are two control sources that all produce `twist_mux` messages to "cmd_vel": joy_vel (teleop controller) and nav_vel (autnomous navigation). -The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. It's important that the states can override each other in this priority: +There are two control sources that both produce `twist_mux` messages to `cmd_vel`: joy_vel (teleop controller) and nav_vel (autnomous navigation). +The RCS provides some track states (safe stop, estop, teleop, auto) that control which input we select. The kart_commander uses these states to determine which control source to let through, or neither. -- ESTOP/SAFE STOP -- TELEOP -- AUTO - -The priorities are implemented with a [`twist_mux`](http://wiki.ros.org/twist_mux). -and laid out as can be seen below. +The priorities are implemented with `switch_mux`, a node defined in `kart_commander` ``` - - - /joy_only /stop (255) - \ / - | | - _|_|_ - /joy_vel____| | - | |____ /cmd_vel - | | - /nav_vel ___| | - |_____| - + /topic_select + | + | + _|___ + /joy_vel____| | + | |____ /cmd_vel + | | + switch_mux + | | + /nav_vel ___| | + |_____| ``` -Priorities for each topic and lock are listed, as well as the expected topic publishers: - -- `stop` ~ `kart_commander`: 255 -- `joy_vel` ~ `teleop_twist_joy_node`: 50 -- `joy_only_publisher` ~ `kart_commander`: 50 -- `nav_vel` ~ `nav2`: 10 - ## Directory Tree - `config/` @@ -50,7 +37,7 @@ Priorities for each topic and lock are listed, as well as the expected topic pub - `rviz.launch.py` _Initialize RViz with Nav2 configuration_ - `slam.launch.py` _Initialize map_server and slam_toolbox_ - `teleop.launch.py` _Launch joy teleop nodes_ - - `twist_mux.launch.py` _Launch both twist muxex and kart_commander_ + - `kart_commander.launch.py` _Launch kart_commander and the twist muxes_ - `VLP16.launch.py` _Initialize Velodyne LIDAR VLP16 nodes_ - `zed.launch.py` _Zed camera launch file_ - `map/` @@ -60,6 +47,6 @@ Priorities for each topic and lock are listed, as well as the expected topic pub - `VLP16db.params.yaml` _VLP16 calibration parameters_ - `nav2.params.yaml` _Nav2 parameters_ - `patchworkpp.params.yaml` _Patchwork++ parameters_ - - `twist_mux.params.yaml` + - `switch_mux.params.yaml` - `rviz/` - `nav2_default_view.rviz.yaml` _RViz config for Nav2_ diff --git a/src/amp_kart_bringup/launch/kart_commander.launch.py b/src/amp_kart_bringup/launch/kart_commander.launch.py new file mode 100644 index 0000000..a63def9 --- /dev/null +++ b/src/amp_kart_bringup/launch/kart_commander.launch.py @@ -0,0 +1,31 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + + params_file = os.path.join(get_package_share_directory('amp_kart_bringup'), + 'params', 'switch_mux.params.yaml') + + switch_mux_action = Node(package='amp_kart_commander', + namespace='', + executable='switch_mux', + name='switch_mux', + parameters=[params_file]) + + kart_commander_action = Node(package='amp_kart_commander', + namespace='', + executable='kart_commander', + name='kart_commander') + + ld = LaunchDescription() + ld.add_action(switch_mux_action) + ld.add_action(kart_commander_action) + + return ld diff --git a/src/amp_kart_bringup/launch/twist_mux.launch.py b/src/amp_kart_bringup/launch/twist_mux.launch.py deleted file mode 100644 index 4cc4228..0000000 --- a/src/amp_kart_bringup/launch/twist_mux.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node - - -def generate_launch_description(): - - share_dir = get_package_share_directory('amp_kart_bringup') - params_dir = os.path.join(share_dir, 'params') - - mux_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('twist_mux'), 'launch', - 'twist_mux_launch.py')), - launch_arguments={ - 'config_locks': os.path.join(params_dir, 'twist_mux.params.yaml'), - 'config_topics': os.path.join(params_dir, 'twist_mux.params.yaml'), - 'cmd_vel_out': 'cmd_vel' - }.items()) - - kart_commander_action = Node(package='amp_kart_commander', - namespace='', - executable='kart_commander', - name='sikart_commander') - - ld = LaunchDescription() - ld.add_action(mux_cmd) - ld.add_action(kart_commander_action) - - return ld diff --git a/src/amp_kart_bringup/params/switch_mux.params.yaml b/src/amp_kart_bringup/params/switch_mux.params.yaml new file mode 100644 index 0000000..661b865 --- /dev/null +++ b/src/amp_kart_bringup/params/switch_mux.params.yaml @@ -0,0 +1,6 @@ +switch_mux: + ros__parameters: + input_topics: + - joy_vel + - nav_vel + initial_topic: __none diff --git a/src/amp_kart_bringup/params/twist_mux.params.yaml b/src/amp_kart_bringup/params/twist_mux.params.yaml deleted file mode 100644 index 48c0808..0000000 --- a/src/amp_kart_bringup/params/twist_mux.params.yaml +++ /dev/null @@ -1,23 +0,0 @@ -twist_mux: - ros__parameters: - # Make sure the locks and corresponding locked topics have the same priority - # When the kart_commander has not explicitly disallowed auto, - # this config allows for teleop and auto to happen simultaneously (in alternating periods of 0.5s) - locks: - joy_enable: - topic: joy_enable - timeout: 0.0 - priority: 50 - stop: - topic: stop - timeout: 0.0 - priority: 255 - topics: - joy_vel: - topic: joy_vel - timeout: 0.5 - priority: 50 - navigation: - topic: nav_vel - timeout: 0.5 - priority: 10 diff --git a/src/amp_kart_commander/README.md b/src/amp_kart_commander/README.md index c873b74..0aa76ca 100644 --- a/src/amp_kart_commander/README.md +++ b/src/amp_kart_commander/README.md @@ -1,12 +1,24 @@ # Kart Commander -Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the cmd_vel mux accordingly. +Kart Commander hosts a Service Server that listens for TrackState changes from the RCS node and manipulates the switch_mux accordingly. Might consider moving the mux diagram from `bringup/launch/twist_mux` here. There's more elaboration on this node in particular there. Somebody fix the rest, -When "teleop_only" is false and "stop" is false, then both joy and nav2 messages can coexist. -Note that while you can reach `Stop` from any other state, returning to `Autonomus` from `Stop` is not allowed; switch to RC first. +Note that while you can reach `Stop` from any other state, returning to `Autonomous` from `Stop` is not allowed; switch to RC first. + +## `switch_mux` + +An one-in, one-out mux for `Twist` messages, since ROS2 does not support generic message types in Foxy. +Publishes to a `Twist` topic named "cmd_vel" and takes in a configurable list of `Twist` topics + +- Parameters + - `initial_topic`: (optional) name of the initial `Twist` topic to pass through + - `input_topics`: list of `Twist` topics to listen for +- Subscribed Topics + - `select_topic`: `String` name of topic from `input_topics` to pass through, or `__none`, ehich explicitly turns off the mux +- Published Topics + - `cmd_vel`: Twist topic output chosen from the inputs, or nothing at all ## Tests -See the READMe in the test folder +See the README in the test folder diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index d748bfe..2248af5 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -4,7 +4,7 @@ from amp_msgs.srv import TrackState import rclpy from rclpy.node import Node -from std_msgs.msg import Bool +from std_msgs.msg import String from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy @@ -24,11 +24,9 @@ def __init__(self): super().__init__('service_server') self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) - self.stop_publisher = self.create_publisher(Bool, - 'stop', - qos_profile=qos_profile) - self.joy_only_publisher = self.create_publisher( - Bool, 'joy_only', qos_profile=qos_profile) + self.mux_publisher = self.create_publisher(String, + 'select_topic', + qos_profile=qos_profile) def track_state_callback(self, request, response): response.state = request.state @@ -40,24 +38,15 @@ def track_state_callback(self, request, response): # TODO More complex states in TrackState.srv have transition rules not yet implemented - # When "teleop_only" is false and "stop" is false, then both joy and nav2 messages - # can coexist. - # Note that while you can reach Stop from any other state, returning to - # Autonomus from Stop is not allowed; switch to RC first. if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP: - self.stop_publisher.publish(Bool(data=True)) + self.mux_publisher.publish(String(data='__none')) self.get_logger().info('STOP: nav2 and teleop disabled') if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE: - self.joy_only_publisher.publish(Bool(data=True)) - self.get_logger().info('RC: nav2 disabled') - self.stop_publisher.publish(Bool(data=False)) - self.get_logger().info('RC: STOP mode restrictions lifted') + self.mux_publisher.publish(String(data='joy_vel')) + self.get_logger().info('RC: joy only') if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE: - self.joy_only_publisher.publish(Bool(data=False)) - self.get_logger().info( - 'AUTO: allow both nav2 and teleop controls. Switching directly from ' - 'ESTOP/SAFESTOP to this state will not disable STOP. Switch to RC first' - ) + self.mux_publisher.publish(String(data='nav_vel')) + self.get_logger().info('AUTO: nav2 only') return response diff --git a/src/amp_kart_commander/amp_kart_commander/switch_mux.py b/src/amp_kart_commander/amp_kart_commander/switch_mux.py new file mode 100644 index 0000000..f9b4a88 --- /dev/null +++ b/src/amp_kart_commander/amp_kart_commander/switch_mux.py @@ -0,0 +1,84 @@ +''' Mux the allows choosing a single topic to let through ''' +#usr/bin/env python3 +import rclpy +import functools + +from rclpy.node import Node +from std_msgs.msg import String +from geometry_msgs.msg import Twist +from rclpy.exceptions import ParameterNotDeclaredException + +from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy +# Create lossy QoS that is compatible with any publisher +subscriber_any = QoSProfile(history=HistoryPolicy.KEEP_LAST, + depth=5, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE) + +# Create high-reliability QoS for topic selection +reliable = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, # Last always has priority anyways + reliability=ReliabilityPolicy.RELIABLE, # Guaranteed delivery + durability=DurabilityPolicy.TRANSIENT_LOCAL # Re-send msg to late-joiners +) + +EMPTY_TOPIC = "__none" + + +class SwitchMux(Node): + + def __init__(self): + super().__init__('switch_mux', allow_undeclared_parameters=True) + + self.declare_parameter('initial_topic', String) + self.declare_parameter('input_topics') + + # `initial_topic` is optionally defined at initialization + try: + self.current_topic = self.get_parameter('initial_topic').value + except ParameterNotDeclaredException: + self.current_topic = EMPTY_TOPIC + + # `input_topics` must be defined at initialization + self.input_topics = self._parameters['input_topics'].value + if self.input_topics is None: + raise ParameterNotDeclaredException('input_topics') + + for topic_name in self.input_topics: + # This specific name is disallowed to guarantee that it disables the mux + if topic_name == EMPTY_TOPIC: + continue + # All input topics share the same base callback (input_topic_callback), but we + # bind a third `topic_name` parameter that changes with the topic + named_callback = functools.partial(self.input_topic_callback, + topic_name) + self.create_subscription(Twist, topic_name, named_callback, + subscriber_any) + + # Topic name + self.subscription = self.create_subscription(String, 'select_topic', + self.set_topic_callback, + reliable) + self.publisher = self.create_publisher(Twist, 'cmd_vel', reliable) + + def set_topic_callback(self, topic_name_msg): + self.current_topic = str(topic_name_msg.data) + + def input_topic_callback(self, topic_name, twist_msg): + ''' Upon receiving a message to any registered input of the mux''' + self.get_logger().info( + f"got from {topic_name} curr is {self.current_topic}") + if topic_name == self.current_topic: + self.publisher.publish(twist_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SwitchMux() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == 'main': + main() diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py index daf9862..bd9e012 100644 --- a/src/amp_kart_commander/setup.py +++ b/src/amp_kart_commander/setup.py @@ -19,7 +19,9 @@ license='TODO: License declaration', tests_require=['pytest'], entry_points={ - 'console_scripts': - ['kart_commander = amp_kart_commander.kart_commander:main'], + 'console_scripts': [ + 'kart_commander = amp_kart_commander.kart_commander:main', + 'switch_mux = amp_kart_commander.switch_mux:main' + ], }, ) diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md index 4161ebb..90afc87 100644 --- a/src/amp_kart_commander/test/README.md +++ b/src/amp_kart_commander/test/README.md @@ -5,7 +5,7 @@ I didn't get around to implementing [launch testing](https://github.com/ros2/lau ## Auto Works - Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Verify that the default state allows for nav2 commands to show in `cmd_vel` +- Verify that the default state does not allow nav2 commands to show in `cmd_vel` ## RC Works From b9171040e293f96c218fd8eeb49b3a9f719e4682 Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 06:51:04 -0400 Subject: [PATCH 24/32] amp_commander: remove debug log remove debug logger line --- src/amp_kart_commander/amp_kart_commander/switch_mux.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/switch_mux.py b/src/amp_kart_commander/amp_kart_commander/switch_mux.py index f9b4a88..cfc1230 100644 --- a/src/amp_kart_commander/amp_kart_commander/switch_mux.py +++ b/src/amp_kart_commander/amp_kart_commander/switch_mux.py @@ -67,8 +67,6 @@ def set_topic_callback(self, topic_name_msg): def input_topic_callback(self, topic_name, twist_msg): ''' Upon receiving a message to any registered input of the mux''' - self.get_logger().info( - f"got from {topic_name} curr is {self.current_topic}") if topic_name == self.current_topic: self.publisher.publish(twist_msg) From e1796b8ead0097d53a758f858096eda60cc30c68 Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 07:04:16 -0400 Subject: [PATCH 25/32] amp_lint install scripts breaking ci install-scripts to install_scripts --- src/amp_kart_commander/setup.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg index 539f781..e42791e 100644 --- a/src/amp_kart_commander/setup.cfg +++ b/src/amp_kart_commander/setup.cfg @@ -1,4 +1,4 @@ [develop] script-dir=$base/lib/amp_kart_commander [install] -install-scripts=$base/lib/amp_kart_commander +install_scripts=$base/lib/amp_kart_commander From bc110ad56f13a8e52e4aa67b06bfc2ac5f4156ab Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 20:14:49 -0400 Subject: [PATCH 26/32] amp_lint: appease ci script_dir with an underscore --- src/amp_kart_commander/setup.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_kart_commander/setup.cfg b/src/amp_kart_commander/setup.cfg index e42791e..ffddaf8 100644 --- a/src/amp_kart_commander/setup.cfg +++ b/src/amp_kart_commander/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/amp_kart_commander +script_dir=$base/lib/amp_kart_commander [install] install_scripts=$base/lib/amp_kart_commander From 2db155a0fe4557d24cc653fa683653d74e208ef9 Mon Sep 17 00:00:00 2001 From: reschivon Date: Wed, 10 May 2023 20:51:53 -0400 Subject: [PATCH 27/32] amp_ci: remove tests they were causing ci to fail --- src/amp_kart_commander/package.xml | 5 --- src/amp_kart_commander/setup.py | 1 - src/amp_kart_commander/test/README.md | 35 ------------------- src/amp_kart_commander/test/test_copyright.py | 23 ------------ src/amp_kart_commander/test/test_flake8.py | 25 ------------- src/amp_kart_commander/test/test_pep257.py | 23 ------------ src/amp_rcs_client/amp_rcs/dummy_server.py | 3 +- src/amp_rcs_client/amp_rcs/service_client.py | 3 +- src/amp_rcs_client/setup.py | 1 - src/amp_rcs_client/test/test_copyright.py | 23 ------------ src/amp_rcs_client/test/test_flake8.py | 25 ------------- src/amp_rcs_client/test/test_pep257.py | 23 ------------ 12 files changed, 2 insertions(+), 188 deletions(-) delete mode 100644 src/amp_kart_commander/test/README.md delete mode 100644 src/amp_kart_commander/test/test_copyright.py delete mode 100644 src/amp_kart_commander/test/test_flake8.py delete mode 100644 src/amp_kart_commander/test/test_pep257.py delete mode 100644 src/amp_rcs_client/test/test_copyright.py delete mode 100644 src/amp_rcs_client/test/test_flake8.py delete mode 100644 src/amp_rcs_client/test/test_pep257.py diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml index d6a5c7a..b5b9d4f 100644 --- a/src/amp_kart_commander/package.xml +++ b/src/amp_kart_commander/package.xml @@ -7,11 +7,6 @@ lucy TODO: License declaration - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - ament_python diff --git a/src/amp_kart_commander/setup.py b/src/amp_kart_commander/setup.py index bd9e012..f49b6fb 100644 --- a/src/amp_kart_commander/setup.py +++ b/src/amp_kart_commander/setup.py @@ -17,7 +17,6 @@ maintainer_email='xchenbox@gmail.com', description='TODO: Package description', license='TODO: License declaration', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'kart_commander = amp_kart_commander.kart_commander:main', diff --git a/src/amp_kart_commander/test/README.md b/src/amp_kart_commander/test/README.md deleted file mode 100644 index 90afc87..0000000 --- a/src/amp_kart_commander/test/README.md +++ /dev/null @@ -1,35 +0,0 @@ -# Kart Commander Testing - -I didn't get around to implementing [launch testing](https://github.com/ros2/launch/tree/rolling/launch_testing) for kart_commander, but here are some manual tests that should be integrated into a future implementation. - -## Auto Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Verify that the default state does not allow nav2 commands to show in `cmd_vel` - -## RC Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Send msgs over `joy_teleop` -- Verify that the default state allows teleop to show in `cmd_vel` -- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"` (try `checkered` too) -- Verify that the info output shows the diagnostic from kart_commander that we are in RC state. -- Verify thet nav2 commands no longer show in cmd_vel -- Send msgs over `joy_teleop` -- Verify that the RC statesallows teleop to show in `cmd_vel` - -## Emergency Stop Works - -- Launch `Nav2` and `amp_bringup/launch/twist_mux.launch` -- Send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: red}"` (try `black` too) -- Verify that the info output shows the diagnostic from kart_commander that we are in STOP state. -- Verify thet nav2 commands don't show in cmd_vel -- Send msgs over `joy_teleop` -- Verify thet joy commands don't show in cmd_vel - -# Not allowed to go STOP -> Auto directly - -- Following the previous test, send `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` (try `yellow` too) -- Verify that no nav2 comes through 'cmd_vel' -- Switch to Teleop and then back to Auto `ros2 service call /track_state amp_msgs/srv/TrackState "{state: orange}"`, `ros2 service call /track_state amp_msgs/srv/TrackState "{state: green}"` -- - Verify that nav comes through 'cmd_vel' diff --git a/src/amp_kart_commander/test/test_copyright.py b/src/amp_kart_commander/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/src/amp_kart_commander/test/test_copyright.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_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/amp_kart_commander/test/test_flake8.py b/src/amp_kart_commander/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/amp_kart_commander/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/src/amp_kart_commander/test/test_pep257.py b/src/amp_kart_commander/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/amp_kart_commander/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' diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index 292ce0e..0a0abe5 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -1,11 +1,10 @@ -"""ROS2 service server for behavioral determination based on client call.""" +"""ROS2 service server for behavioral determination based on client call""" from rcs_service.srv import TrackState import rclpy from rclpy.node import Node - class ServiceServer(Node): def __init__(self): diff --git a/src/amp_rcs_client/amp_rcs/service_client.py b/src/amp_rcs_client/amp_rcs/service_client.py index 7ea9662..d1762fc 100644 --- a/src/amp_rcs_client/amp_rcs/service_client.py +++ b/src/amp_rcs_client/amp_rcs/service_client.py @@ -1,6 +1,5 @@ -"""ROS2 service client for state based service calling.""" +"""ROS2 service client for state based service calling""" -#usr/bin/env python3 from amp_msgs.srv import TrackState import rclpy from rclpy.node import Node diff --git a/src/amp_rcs_client/setup.py b/src/amp_rcs_client/setup.py index 9e5f3d3..62c4b0f 100644 --- a/src/amp_rcs_client/setup.py +++ b/src/amp_rcs_client/setup.py @@ -18,7 +18,6 @@ description= 'Service client for sent track state change requests to the service server', license='GNU General Public License 3.0', - tests_require=['pytest'], entry_points={ 'console_scripts': [ "amp_rcs_client = amp_rcs.service_client:main", diff --git a/src/amp_rcs_client/test/test_copyright.py b/src/amp_rcs_client/test/test_copyright.py deleted file mode 100644 index cc8ff03..0000000 --- a/src/amp_rcs_client/test/test_copyright.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_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/amp_rcs_client/test/test_flake8.py b/src/amp_rcs_client/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/src/amp_rcs_client/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/src/amp_rcs_client/test/test_pep257.py b/src/amp_rcs_client/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/src/amp_rcs_client/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' From 1f3757fb2aaad2b6f37f31ac834a7ba88b902c1e Mon Sep 17 00:00:00 2001 From: alanssitis Date: Thu, 11 May 2023 16:46:37 -0400 Subject: [PATCH 28/32] amp_rcs_client: fix changed msgs package lost in rebase --- src/amp_rcs_client/amp_rcs/dummy_server.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index 0a0abe5..8a45b64 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -1,6 +1,6 @@ """ROS2 service server for behavioral determination based on client call""" -from rcs_service.srv import TrackState +from amp_msgs.srv import TrackState import rclpy from rclpy.node import Node From 5e134cc3255ca55c2a3fe9cc353269e1f803dcb0 Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 02:59:29 -0400 Subject: [PATCH 29/32] amp_commander: Change trackstate server name to trackstatereciever renamed it in rcs too --- src/amp_kart_commander/amp_kart_commander/kart_commander.py | 6 +++--- src/amp_rcs_client/amp_rcs/dummy_server.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index 2248af5..a8030b7 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -18,10 +18,10 @@ ) -class ServiceServer(Node): +class TrackStateReciever(Node): def __init__(self): - super().__init__('service_server') + super().__init__('track_state_reciever') self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) self.mux_publisher = self.create_publisher(String, @@ -53,7 +53,7 @@ def track_state_callback(self, request, response): def main(args=None): rclpy.init(args=args) - node = ServiceServer() + node = TrackStateReciever() rclpy.spin(node) rclpy.shutdown() diff --git a/src/amp_rcs_client/amp_rcs/dummy_server.py b/src/amp_rcs_client/amp_rcs/dummy_server.py index 0a0abe5..5064cd7 100644 --- a/src/amp_rcs_client/amp_rcs/dummy_server.py +++ b/src/amp_rcs_client/amp_rcs/dummy_server.py @@ -8,7 +8,7 @@ class ServiceServer(Node): def __init__(self): - super().__init__('service_server') + super().__init__('track_state_reciever') self.server = self.create_service(TrackState, 'track_state', self.track_state_callback) From a2b47937b312998c479058d08f53c79ec6136bcb Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 03:00:13 -0400 Subject: [PATCH 30/32] amp_commander: added metadat to package.xml added metadat to package.xml --- src/amp_kart_commander/package.xml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml index b5b9d4f..d4c5d4b 100644 --- a/src/amp_kart_commander/package.xml +++ b/src/amp_kart_commander/package.xml @@ -2,11 +2,10 @@ amp_kart_commander - 0.0.0 - Updates cmd_vel max based on TrackState changes - lucy - TODO: License declaration - + 0.1.0 + Listens for TrackState changes to redirect the proper Twist source to cmd_vel + Xuyang Chen ament_python From 8c3d3133a59e0eae46070b67d1eb9268c556a99f Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 14:15:11 -0400 Subject: [PATCH 31/32] amp_ci: add license to package.xml add license to package.xml --- src/amp_kart_commander/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/amp_kart_commander/package.xml b/src/amp_kart_commander/package.xml index d4c5d4b..04f63a0 100644 --- a/src/amp_kart_commander/package.xml +++ b/src/amp_kart_commander/package.xml @@ -6,6 +6,8 @@ Listens for TrackState changes to redirect the proper Twist source to cmd_vel Xuyang Chen + Apache-2.0 + ament_python From d1b4b9691aa4b907a409056b240161e06ff713d9 Mon Sep 17 00:00:00 2001 From: reschivon Date: Fri, 12 May 2023 16:14:34 -0400 Subject: [PATCH 32/32] amp_commander: change from service to client since someone modified the mqtt node --- .../amp_kart_commander/kart_commander.py | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/amp_kart_commander/amp_kart_commander/kart_commander.py b/src/amp_kart_commander/amp_kart_commander/kart_commander.py index a8030b7..a357f5c 100644 --- a/src/amp_kart_commander/amp_kart_commander/kart_commander.py +++ b/src/amp_kart_commander/amp_kart_commander/kart_commander.py @@ -8,10 +8,11 @@ from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy, ReliabilityPolicy -# Create high-reliability QoS for mux toggling -qos_profile = QoSProfile( - history=HistoryPolicy.KEEP_LAST, - depth=1, # Last always has priority anyways +# Create high-reliability QoS with guaranteed delivery +qos_reliable = QoSProfile( + history=HistoryPolicy. + KEEP_LAST, # TRANSIENT_LOCAL set, not point in history + depth=1, # See above reliability=ReliabilityPolicy.RELIABLE, # Guaranteed delivery durability=DurabilityPolicy. TRANSIENT_LOCAL # Re-send msg to late-joining subscribers @@ -22,34 +23,32 @@ class TrackStateReciever(Node): def __init__(self): super().__init__('track_state_reciever') - self.server = self.create_service(TrackState, 'track_state', - self.track_state_callback) - self.mux_publisher = self.create_publisher(String, - 'select_topic', - qos_profile=qos_profile) + self.create_subscription(String, 'track_state', + self.track_state_callback, qos_reliable) + self.kart_state_pub = self.create_publisher(String, 'kart_state', + qos_reliable) + self.mux_publisher = self.create_publisher(String, 'select_topic', + qos_reliable) - def track_state_callback(self, request, response): - response.state = request.state + def track_state_callback(self, new_state): + new_state: str = new_state.data - self.get_logger().info('Recieved: %s Response: %s' % - (request.state, response.state)) + self.get_logger().info(f'Recieved Track State: {new_state}') # self.get_logger().info('eeee' + str(dir(TrackState.Request))) # TODO More complex states in TrackState.srv have transition rules not yet implemented - if response.state == TrackState.Request.EMERGENCY or response.state == TrackState.Request.SAFESTOP: + if new_state == TrackState.Request.EMERGENCY or new_state == TrackState.Request.SAFESTOP: self.mux_publisher.publish(String(data='__none')) self.get_logger().info('STOP: nav2 and teleop disabled') - if response.state == TrackState.Request.RC or response.state == TrackState.Request.RACEDONE: + if new_state == TrackState.Request.RC or new_state == TrackState.Request.RACEDONE: self.mux_publisher.publish(String(data='joy_vel')) self.get_logger().info('RC: joy only') - if response.state == TrackState.Request.AUTONOMOUS_SLOW or response.state == TrackState.Request.AUTONOMOUS_OVERTAKE: + if new_state == TrackState.Request.AUTONOMOUS_SLOW or new_state == TrackState.Request.AUTONOMOUS_OVERTAKE: self.mux_publisher.publish(String(data='nav_vel')) self.get_logger().info('AUTO: nav2 only') - return response - def main(args=None): rclpy.init(args=args)