diff --git a/flexbe_input/flexbe_input/__init__.py b/flexbe_input/flexbe_input/__init__.py
index e69de29..d3b30f2 100644
--- a/flexbe_input/flexbe_input/__init__.py
+++ b/flexbe_input/flexbe_input/__init__.py
@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+
+# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of the Philipp Schillinger, Team ViGIR, Christopher Newport University nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Initialize module for flexbe_input."""
diff --git a/flexbe_input/flexbe_input/complex_action_server.py b/flexbe_input/flexbe_input/complex_action_server.py
index a156c17..c2173aa 100644
--- a/flexbe_input/flexbe_input/complex_action_server.py
+++ b/flexbe_input/flexbe_input/complex_action_server.py
@@ -1,23 +1,26 @@
#! /usr/bin/env python
+
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in the
-# documentation and/or other materials provided with the distribution.
-# * Neither the name of the Willow Garage, Inc. nor the names of its
-# contributors may be used to endorse or promote products derived from
-# this software without specific prior written permission.
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+#
+# * Neither the name of the Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
@@ -30,18 +33,20 @@
# Author: Brian Wright.
# Based on C++ simple_action_server.h by Eitan Marder-Eppstein
+
import queue
+import threading
+import traceback
+
+from flexbe_core import Logger
import rclpy
from rclpy.action import ActionServer
from rclpy.duration import Duration
-import threading
-import traceback
-from flexbe_core import Logger
-
def nop_cb(goal_handle):
+ """Execute no operation (pass through) callback."""
pass
@@ -51,16 +56,21 @@ def nop_cb(goal_handle):
class ComplexActionServer:
- # @brief Constructor for a ComplexActionServer
- # @param name A name for the action server
- # @param execute_cb Optional callback that gets called in a separate thread whenever
- # a new goal is received, allowing users to have blocking callbacks.
- # Adding an execute callback also deactivates the goalCallback.
- # @param auto_start A boolean value that tells the ActionServer wheteher or not
- # to start publishing as soon as it comes up.
- # THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and
- # start() should be called after construction of the server.
+ """
+ Constructor for a ComplexActionServer.
+
+ @param name A name for the action server
+ @param execute_cb Optional callback that gets called in a separate thread whenever
+ a new goal is received, allowing users to have blocking callbacks.
+ Adding an execute callback also deactivates the goalCallback.
+ @param auto_start A boolean value that tells the ActionServer wheteher or not
+ to start publishing as soon as it comes up.
+ THIS SHOULD ALWAYS BE SET TO FALSE TO AVOID RACE CONDITIONS and
+ start() should be called after construction of the server.
+ """
+
def __init__(self, node, name, ActionSpec, execute_cb=None, auto_start=False):
+ """Initialize instance of the ComplexActionServer."""
self.node = node
self.goals_received_ = 0
self.goal_queue_ = queue.Queue()
@@ -109,9 +119,10 @@ def __del__(self):
# @brief Accepts a new goal when one is available The status of this
# goal is set to active upon acceptance,
def accept_new_goal(self):
+ """Accept a new goal."""
# with self.action_server.lock, self.lock:
- Logger.logdebug("Accepting a new goal")
+ Logger.logdebug('Accepting a new goal')
self.goals_received_ -= 1
@@ -119,7 +130,7 @@ def accept_new_goal(self):
current_goal = self.goal_queue_.get()
# set the status of the current goal to be active
- # current_goal.set_accepted("This goal has been accepted by the simple action server");
+ # current_goal.set_accepted('This goal has been accepted by the simple action server');
current_goal.succeed()
return current_goal
@@ -127,11 +138,13 @@ def accept_new_goal(self):
# @brief Allows polling implementations to query about the availability of a new goal
# @return True if a new goal is available, false otherwise
def is_new_goal_available(self):
+ """Check if new goal is available."""
return self.goals_received_ > 0
# @brief Allows polling implementations to query about the status of the current goal
# @return True if a goal is active, false otherwise
def is_active(self):
+ """Check if current goal is active."""
if self.current_goal and not self.current_goal.get_goal():
return False
@@ -139,7 +152,8 @@ def is_active(self):
# @brief Sets the status of the active goal to succeeded
# @param result An optional result to send back to any clients of the goal
- def set_succeeded(self, result=None, text="", goal_handle=None):
+ def set_succeeded(self, result=None, text='', goal_handle=None):
+ """Set status to success."""
goal_handle.succeed()
if not result:
result = self.get_default_result()
@@ -148,7 +162,8 @@ def set_succeeded(self, result=None, text="", goal_handle=None):
# @brief Sets the status of the active goal to aborted
# @param result An optional result to send back to any clients of the goal
- def set_aborted(self, result=None, text="", goal_handle=None):
+ def set_aborted(self, result=None, text='', goal_handle=None):
+ """Set goal aborted."""
goal_handle.abort()
if not result:
result = self.get_default_result()
@@ -158,26 +173,30 @@ def set_aborted(self, result=None, text="", goal_handle=None):
# @brief Publishes feedback for a given goal
# @param feedback Shared pointer to the feedback to publish
def publish_feedback(self, feedback):
+ """Publish feedback."""
self.current_goal.publish_feedback(feedback)
def get_default_result(self):
+ """Get result."""
return self.action_server.action_type
# @brief Allows users to register a callback to be invoked when a new goal is available
# @param cb The callback to be invoked
def register_goal_callback(self, cb):
+ """Register callback."""
if self.execute_callback:
- Logger.logwarn("Cannot call ComplexActionServer.register_goal_callback() "
- "because an executeCallback exists. Not going to register it.")
+ Logger.logwarn('Cannot call ComplexActionServer.register_goal_callback() '
+ 'because an executeCallback exists. Not going to register it.')
else:
self.goal_callback = cb
# @brief Callback for when the ActionServer receives a new goal and passes it on
def internal_goal_callback(self, goal):
+ """Call when the ActionServer receives a new goal and passes it on."""
self.execute_condition.acquire()
try:
- Logger.localinfo(f"A new goal {goal.goal_id} has been recieved by the single goal action server")
+ Logger.localinfo(f'A new goal {goal.goal_id} has been recieved by the single goal action server')
self.next_goal = goal
self.new_goal = True
@@ -191,19 +210,21 @@ def internal_goal_callback(self, goal):
self.execute_condition.release()
except Exception as e:
- Logger.logerr("ComplexActionServer.internal_goal_callback - exception %s", str(e))
+ Logger.logerr('ComplexActionServer.internal_goal_callback - exception %s', str(e))
self.execute_condition.release()
# @brief Callback for when the ActionServer receives a new preempt and passes it on
def internal_preempt_callback(self, preempt):
+ """Call when the ActionServer receives a new preempt and passes it on."""
return
# @brief Called from a separate thread to call blocking execute calls
def executeLoop(self):
+ """Excute the server loop."""
loop_duration = Duration(seconds=0.1)
while (rclpy.ok()):
- Logger.logdebug("ComplexActionServer: execute")
+ Logger.logdebug('ComplexActionServer: execute')
with self.terminate_mutex:
if (self.need_to_terminate):
@@ -212,22 +233,23 @@ def executeLoop(self):
if (self.is_new_goal_available()):
goal_handle = self.accept_new_goal()
if not self.execute_callback:
- Logger.logerr("execute_callback_ must exist. This is a bug in ComplexActionServer")
+ Logger.logerr('execute_callback_ must exist. This is a bug in ComplexActionServer')
return
try:
- print("run new executecb")
+ print('run new executecb')
thread = threading.Thread(target=self.run_goal, args=(goal_handle.get_goal(), goal_handle))
thread.start()
except Exception as ex:
- Logger.logerr(f"Exception in your execute callback: {str(ex)}\n",
+ Logger.logerr(f'Exception in your execute callback: {str(ex)}\n',
f"{traceback.format_exc().replace('%', '%%')}")
- self.set_aborted(None, f"Exception in execute callback: {str(ex)}")
+ self.set_aborted(None, f'Exception in execute callback: {str(ex)}')
with self.execute_condition:
self.execute_condition.wait(loop_duration.nanoseconds * 1.e-9)
def run_goal(self, goal, goal_handle):
+ """Run goal callback."""
self.execute_callback(goal, goal_handle)
diff --git a/flexbe_input/flexbe_input/flexbe_input.py b/flexbe_input/flexbe_input/flexbe_input.py
index 9c715f5..1fdae84 100644
--- a/flexbe_input/flexbe_input/flexbe_input.py
+++ b/flexbe_input/flexbe_input/flexbe_input.py
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-# Copyright 2023 Philipp Schillinger, Team ViGIR, Christopher Newport University
+# Copyright 2024 Philipp Schillinger, Team ViGIR, Christopher Newport University
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
@@ -37,10 +37,12 @@
@author: Philipp Schillinger, Brian Wright
"""
-from rclpy.action import ActionClient
+from flexbe_core import Logger
from flexbe_msgs.action import BehaviorInput
-from flexbe_core import Logger
+
+from rclpy.action import ActionClient
+
from .complex_action_server import ComplexActionServer
@@ -57,10 +59,11 @@ def __init__(self, node):
execute_cb=self.execute_cb,
auto_start=False)
- Logger.loginfo("Ready for data requests...")
+ Logger.loginfo('Ready for data requests...')
def execute_cb(self, goal, goal_handle):
- Logger.loginfo("--> Got a request!")
+ """Execute callback."""
+ Logger.loginfo('--> Got a request!')
Logger.loginfo(f"'{goal.msg}'")
relay_ocs_client_ = ActionClient(self._node,
@@ -68,28 +71,28 @@ def execute_cb(self, goal, goal_handle):
'flexbe/operator_input')
# wait for data msg
- Logger.localinfo("FlexBEInput waiting to relay from OCS ...")
+ Logger.localinfo('FlexBEInput waiting to relay from OCS ...')
relay_ocs_client_.wait_for_server()
- Logger.localinfo("FlexBEInput is ready!")
+ Logger.localinfo('FlexBEInput is ready!')
# Fill in the goal here
result = relay_ocs_client_.send_goal(goal) # This is a blocking call!
# result.data now serialized
data_str = result.data
- Logger.localinfo(f"FlexBEInput result={data_str}")
+ Logger.localinfo(f'FlexBEInput result={data_str}')
if result.result_code == BehaviorInput.Result.RESULT_OK:
self._as.set_succeeded(BehaviorInput.Result(result_code=BehaviorInput.Result.RESULT_OK,
- data=data_str), "ok", goal_handle)
+ data=data_str), 'ok', goal_handle)
elif result.result_code == BehaviorInput.Result.RESULT_FAILED:
# remove
self._as.set_succeeded(BehaviorInput.Result(result_code=BehaviorInput.Result.RESULT_FAILED,
- data=data_str), "failed", goal_handle)
- Logger.loginfo("<-- Replied with FAILED")
+ data=data_str), 'failed', goal_handle)
+ Logger.loginfo('<-- Replied with FAILED')
elif result.result_code == BehaviorInput.Result.RESULT_ABORTED:
self._as.set_succeeded(BehaviorInput.Result(result_code=BehaviorInput.Result.RESULT_ABORTED,
- data=data_str), "Aborted", goal_handle)
- Logger.loginfo("<-- Replied with ABORT")
+ data=data_str), 'Aborted', goal_handle)
+ Logger.loginfo('<-- Replied with ABORT')
diff --git a/flexbe_input/flexbe_input/input_action_server.py b/flexbe_input/flexbe_input/input_action_server.py
index 831e009..67dbfe3 100644
--- a/flexbe_input/flexbe_input/input_action_server.py
+++ b/flexbe_input/flexbe_input/input_action_server.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Christopher Newport University
+# Copyright 2024 Christopher Newport University
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
@@ -31,16 +31,18 @@
import sys
import time
-from PyQt5 import QtWidgets
-
-import rclpy
-from rclpy.action import ActionServer
-from rclpy.node import Node
+from PySide6.QtWidgets import QApplication
from flexbe_core import Logger
+
from flexbe_input.input_gui import InputGUI
+
from flexbe_msgs.action import BehaviorInput
+import rclpy
+from rclpy.action import ActionServer
+from rclpy.node import Node
+
class InputActionServer(Node):
"""
@@ -51,6 +53,7 @@ class InputActionServer(Node):
"""
def __init__(self):
+ """Initialize the InputActionServer instance."""
super().__init__('input_action_server')
self._server = ActionServer(
self,
@@ -58,7 +61,8 @@ def __init__(self):
'flexbe/behavior_input',
self.execute_callback
)
- self._app = None
+ self._app = QApplication(sys.argv)
+
self._input = None
Logger.initialize(self)
@@ -71,11 +75,11 @@ def get_input_type(self, request_type):
@return prompt, instance type, number of elements
"""
# Thse are the only types handled by this simple UI
- types = {BehaviorInput.Goal.REQUEST_INT: ("int", int, 1),
- BehaviorInput.Goal.REQUEST_FLOAT: ("float", (float, int), 1), # int acceptable for desired float
- BehaviorInput.Goal.REQUEST_2D: ("list of 2 numbers", (list, tuple), 2), # allow either list or tuple
- BehaviorInput.Goal.REQUEST_3D: ("list of 3 numbers", (list, tuple), 3), # e.g., "[1, 2]", "(1, 2)", or "1, 2"
- BehaviorInput.Goal.REQUEST_4D: ("list of 4 numbers", (list, tuple), 4),
+ types = {BehaviorInput.Goal.REQUEST_INT: ('int', int, 1),
+ BehaviorInput.Goal.REQUEST_FLOAT: ('float', (float, int), 1), # int acceptable for desired float
+ BehaviorInput.Goal.REQUEST_2D: ('list of 2 numbers', (list, tuple), 2), # allow either list or tuple
+ BehaviorInput.Goal.REQUEST_3D: ('list of 3 numbers', (list, tuple), 3), # e.g., '[1, 2]', '(1, 2)', or '1, 2'
+ BehaviorInput.Goal.REQUEST_4D: ('list of 4 numbers', (list, tuple), 4),
}
if request_type in types:
@@ -84,34 +88,32 @@ def get_input_type(self, request_type):
return None
def execute_callback(self, goal_handle):
-
+ """On receipt of goal, open GUI and request input from user."""
result = BehaviorInput.Result()
- Logger.localinfo("Requesting: %s", goal_handle.request.msg)
+ Logger.localinfo('Requesting: %s', goal_handle.request.msg)
try:
type_text, type_class, expected_elements = self.get_input_type(goal_handle.request.request_type)
- prompt_text = f"{goal_handle.request.msg}\n{type_text}"
+ prompt_text = f'{goal_handle.request.msg}\n{type_text}'
except Exception as exc: # pylint: disable=W0703
- result.data = f"Input action server UI does not handle requests for type {goal_handle.request.request_type}"
+ result.data = f'Input action server UI does not handle requests for type {goal_handle.request.request_type}'
result.result_code = BehaviorInput.Result.RESULT_ABORTED
- Logger.localwarn(f"{result.data}\n Exception: {exc}")
+ Logger.localwarn(f'{result.data}\n Exception: {exc}')
goal_handle.abort()
return result
# Get data from user
- app = QtWidgets.QApplication(sys.argv)
mainWin = InputGUI(prompt_text)
mainWin.show()
while mainWin.is_none() and mainWin.isVisible():
- QtWidgets.qApp.processEvents()
- time.sleep(.05)
+ self._app.processEvents()
+ time.sleep(.01)
self._input = mainWin.get_input()
mainWin.close()
- app.quit()
if self._input is None:
- Logger.logwarn("No data entered while input window was visible!")
+ Logger.logwarn('No data entered while input window was visible!')
result.result_code = BehaviorInput.Result.RESULT_ABORTED
- result.data = "No data entered while input window was visible!"
+ result.data = 'No data entered while input window was visible!'
goal_handle.abort()
else:
try:
@@ -125,7 +127,7 @@ def execute_callback(self, goal_handle):
data_len = 1 if isinstance(input_data, (int, float)) else len(input_data)
if data_len != expected_elements:
- result.data = (f"Invalid number of elements {data_len} not {expected_elements} "
+ result.data = (f'Invalid number of elements {data_len} not {expected_elements} '
f"of {type_class} - expected '{type_text}'")
result.result_code = BehaviorInput.Result.RESULT_FAILED
Logger.localwarn(result.data)
@@ -133,11 +135,11 @@ def execute_callback(self, goal_handle):
return result
result.data = str(pickle.dumps(input_data))
- Logger.localinfo(" Data returned %s", self._input)
+ Logger.localinfo(' Data returned %s', self._input)
result.result_code = BehaviorInput.Result.RESULT_OK
goal_handle.succeed()
except Exception as exc: # pylint: disable=W0703
- Logger.logwarn("Failure to set data: %s", str(exc))
+ Logger.logwarn('Failure to set data: %s', str(exc))
result.result_code = BehaviorInput.Result.RESULT_FAILED
result.data = str(exc)
goal_handle.abort()
@@ -146,14 +148,15 @@ def execute_callback(self, goal_handle):
def main(args=None):
+ """Run action server and GUI on request."""
rclpy.init(args=args)
action_server = InputActionServer()
try:
- Logger.localinfo("Waiting for requests from FlexBE input state ...")
+ Logger.localinfo('Waiting for requests from FlexBE input state ...')
rclpy.spin(action_server)
except KeyboardInterrupt:
pass
- print("\nShutting down the input action server!", flush=True)
+ print('\nShutting down the input action server!', flush=True)
if __name__ == '__main__':
diff --git a/flexbe_input/flexbe_input/input_gui.py b/flexbe_input/flexbe_input/input_gui.py
index 61dcf5e..61530f4 100644
--- a/flexbe_input/flexbe_input/input_gui.py
+++ b/flexbe_input/flexbe_input/input_gui.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Christopher Newport University
+# Copyright 2024 Christopher Newport University
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
@@ -26,8 +26,9 @@
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-from PyQt5.QtWidgets import QMainWindow, QLabel, QLineEdit, QPushButton
-from PyQt5.QtCore import QSize
+"""FlexBE InputGUI."""
+from PySide6.QtCore import QSize
+from PySide6.QtWidgets import QLabel, QLineEdit, QMainWindow, QPushButton
class InputGUI(QMainWindow):
@@ -38,12 +39,13 @@ class InputGUI(QMainWindow):
"""
def __init__(self, prompt):
+ """Initialize the InputGUI instance."""
QMainWindow.__init__(self)
self.input = None
self.setMinimumSize(QSize(320, 140))
- self.setWindowTitle("Input State")
+ self.setWindowTitle('FlexBE Input State')
self.prompt = QLabel(self)
self.prompt.move(60, 20)
@@ -62,12 +64,15 @@ def __init__(self, prompt):
self.adjustSize()
def set_input(self):
+ """Set input text from GUI."""
self.input = self.line.text()
def is_none(self):
+ """Return true while input is none."""
if self.input is None:
return True
return False
def get_input(self):
+ """Get the stored input."""
return self.input
diff --git a/flexbe_input/package.xml b/flexbe_input/package.xml
index 121048c..8b3ec03 100644
--- a/flexbe_input/package.xml
+++ b/flexbe_input/package.xml
@@ -19,6 +19,7 @@
rclpy
flexbe_core
flexbe_msgs
+ python3-qtpy
ament_copyright
ament_flake8
diff --git a/flexbe_input/setup.py b/flexbe_input/setup.py
index 1c1a222..ed13f04 100644
--- a/flexbe_input/setup.py
+++ b/flexbe_input/setup.py
@@ -1,6 +1,6 @@
"""Setup for flexbe_input package."""
-from setuptools import setup
from setuptools import find_packages
+from setuptools import setup
PACKAGE_NAME = 'flexbe_input'
@@ -11,9 +11,10 @@
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + PACKAGE_NAME]),
('share/' + PACKAGE_NAME, ['package.xml']),
- # No tests yet ('share/' + PACKAGE_NAME + "/tests", glob('tests/*.test')),
+ # No tests yet ('share/' + PACKAGE_NAME + '/tests', glob('tests/*.test')),
],
- install_requires=['setuptools'],
+ install_requires=['setuptools',
+ 'pyside6'],
zip_safe=True,
maintainer='phil',
maintainer_email='philsplus@gmail.com',