diff --git a/mocap4r2cli/mocap4r2cli/api/__init__.py b/mocap4r2cli/mocap4r2cli/api/__init__.py index 22daa9b..150ede9 100644 --- a/mocap4r2cli/mocap4r2cli/api/__init__.py +++ b/mocap4r2cli/mocap4r2cli/api/__init__.py @@ -12,8 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ros2cli.node.strategy import NodeStrategy from mocap4r2cli.verb import get_mocap_systems +from ros2cli.node.strategy import NodeStrategy + class MocapNameCompleter: """Callable returning a list of MOCAP4ROS2 system names.""" diff --git a/mocap4r2cli/mocap4r2cli/command/mocap4r2.py b/mocap4r2cli/mocap4r2cli/command/mocap4r2.py index 13b1ce8..d6b5936 100644 --- a/mocap4r2cli/mocap4r2cli/command/mocap4r2.py +++ b/mocap4r2cli/mocap4r2cli/command/mocap4r2.py @@ -15,6 +15,7 @@ from ros2cli.command import add_subparsers_on_demand from ros2cli.command import CommandExtension + class Mocap4r2Command(CommandExtension): """Various mocap4r2 related sub-commands.""" diff --git a/mocap4r2cli/mocap4r2cli/verb/__init__.py b/mocap4r2cli/mocap4r2cli/verb/__init__.py index 0f4d1ed..29e8b7b 100644 --- a/mocap4r2cli/mocap4r2cli/verb/__init__.py +++ b/mocap4r2cli/mocap4r2cli/verb/__init__.py @@ -12,25 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION -from ros2cli.plugin_system import satisfies_version - -import rclpy -from rclpy.lifecycle import State -from rclpy.time import Time -from rclpy.qos import QoSProfile, QoSDurabilityPolicy from lifecycle_msgs.srv import GetState -from mocap4r2_control_msgs.msg import MocapInfo from mocap4r2_control_msgs.msg import Control +from mocap4r2_control_msgs.msg import MocapInfo + +import rclpy +from rclpy.qos import QoSDurabilityPolicy, QoSProfile + +from ros2cli.plugin_system import PLUGIN_SYSTEM_VERSION +from ros2cli.plugin_system import satisfies_version def get_mocap_systems(node): - clock = node.get_clock() + clock = node.get_clock() start_time = clock.now() elapsed = clock.now() - start_time - + qos_profile = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, depth=10) @@ -39,7 +38,7 @@ def get_mocap_systems(node): node.create_subscription( MocapInfo, '/mocap4r2_environment', - lambda msg: mocap_systems.append(msg.mocap4r2_source), + lambda msg: mocap_systems.append(msg.mocap4r2_source), qos_profile) while rclpy.ok() and elapsed.nanoseconds / 1e9 < 0.5: @@ -48,29 +47,34 @@ def get_mocap_systems(node): return mocap_systems + def get_lc_status(node, mocap_lc_node): srv_name = '/' + mocap_lc_node + '/get_state' srv = node.create_client(GetState, srv_name) - + if not srv.wait_for_service(timeout_sec=1.0): - node.get_logger().error('Mocap System ' + mocap_lc_node + ' does not support MOCAP4ROS2 Control System') + node.get_logger().error('Mocap System ' + mocap_lc_node + + ' does not support MOCAP4ROS2 Control System') return (None, False) - + req = GetState.Request() future = srv.call_async(req) rclpy.spin_until_future_complete(node, future) - + return (future.result().current_state, True) + def send_start_control(node, mocap_lc_node): return send_control(node, mocap_lc_node, Control.START, Control.ACK_START) + def send_stop_control(node, mocap_lc_node): return send_control(node, mocap_lc_node, Control.STOP, Control.ACK_STOP) + def send_control(node, mocap_lc_node, command, ack_command): - clock = node.get_clock() + clock = node.get_clock() start_time = clock.now() elapsed = clock.now() - start_time @@ -99,7 +103,6 @@ def send_control(node, mocap_lc_node, command, ack_command): return ok - class VerbExtension: """ The extension point for 'mocap4r2' verb extensions. diff --git a/mocap4r2cli/mocap4r2cli/verb/start.py b/mocap4r2cli/mocap4r2cli/verb/start.py index 7c903d3..af07d09 100644 --- a/mocap4r2cli/mocap4r2cli/verb/start.py +++ b/mocap4r2cli/mocap4r2cli/verb/start.py @@ -12,20 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy -from rclpy.time import Time - -from rclpy.lifecycle import State from lifecycle_msgs.msg import State as LCState -from mocap4r2_control_msgs.msg import MocapInfo +from mocap4r2cli.api import MocapNameCompleter +from mocap4r2cli.verb import get_lc_status, get_mocap_systems, send_start_control, VerbExtension from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy -from mocap4r2cli.verb import VerbExtension, get_lc_status, get_mocap_systems, send_start_control -from mocap4r2cli.api import MocapNameCompleter class StartVerb(VerbExtension): """Start a MOCAP4ROS2 system.""" @@ -34,13 +29,13 @@ def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument( 'mocap_system', - help="Name of the MOCAP4ROS2 system to start") + help='Name of the MOCAP4ROS2 system to start') arg.completer = MocapNameCompleter() def main(self, *, args): with NodeStrategy(args) as node: mocap_systems = get_mocap_systems(node) - + if args.mocap_system in mocap_systems: lc_status, ok = get_lc_status(node, args.mocap_system) if ok: @@ -58,6 +53,7 @@ def main(self, *, args): node.get_logger().info(args.mocap_system + ' started') else: node.get_logger().error('Error getting status of ' + args.mocap_system) - + else: - node.get_logger().error('Mocap System ' + args.mocap_system + ' not in detected MOCAP4ROS2 Control System') + node.get_logger().error('Mocap System ' + args.mocap_system + + ' not in detected MOCAP4ROS2 Control System') diff --git a/mocap4r2cli/mocap4r2cli/verb/status.py b/mocap4r2cli/mocap4r2cli/verb/status.py index 9a253c4..7acc5b9 100644 --- a/mocap4r2cli/mocap4r2cli/verb/status.py +++ b/mocap4r2cli/mocap4r2cli/verb/status.py @@ -14,9 +14,11 @@ from lifecycle_msgs.msg import State as LCState +from mocap4r2cli.verb import get_lc_status, get_mocap_systems, VerbExtension + from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy -from mocap4r2cli.verb import VerbExtension, get_lc_status, get_mocap_systems + class StatusVerb(VerbExtension): """Prints the status of the MOCAP4ROS2 systems.""" @@ -27,13 +29,14 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: mocap_systems = get_mocap_systems(node) - + for mocap_system in mocap_systems: lc_status, ok = get_lc_status(node, mocap_system) if ok: status = 'UNKNOWN' - if lc_status.id == LCState.PRIMARY_STATE_UNKNOWN or lc_status.id == LCState.PRIMARY_STATE_FINALIZED: + if (lc_status.id == LCState.PRIMARY_STATE_UNKNOWN or + lc_status.id == LCState.PRIMARY_STATE_FINALIZED): status = 'NOT READY' elif lc_status.id == LCState.PRIMARY_STATE_UNCONFIGURED: status = 'NOT YET AVAILABLE' diff --git a/mocap4r2cli/mocap4r2cli/verb/stop.py b/mocap4r2cli/mocap4r2cli/verb/stop.py index b983c13..53d07c3 100644 --- a/mocap4r2cli/mocap4r2cli/verb/stop.py +++ b/mocap4r2cli/mocap4r2cli/verb/stop.py @@ -12,20 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy -from rclpy.time import Time - - -from rclpy.lifecycle import State from lifecycle_msgs.msg import State as LCState -from mocap4r2_control_msgs.msg import MocapInfo +from mocap4r2cli.api import MocapNameCompleter +from mocap4r2cli.verb import get_lc_status, get_mocap_systems, send_stop_control, VerbExtension from ros2cli.node.strategy import add_arguments from ros2cli.node.strategy import NodeStrategy -from mocap4r2cli.verb import VerbExtension, get_lc_status, get_mocap_systems, send_stop_control -from mocap4r2cli.api import MocapNameCompleter class StopVerb(VerbExtension): """Stop a MOCAP4ROS2 system.""" @@ -34,13 +28,13 @@ def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument( 'mocap_system', - help="Name of the MOCAP4ROS2 system to stop") + help='Name of the MOCAP4ROS2 system to stop') arg.completer = MocapNameCompleter() def main(self, *, args): with NodeStrategy(args) as node: mocap_systems = get_mocap_systems(node) - + if args.mocap_system in mocap_systems: lc_status, ok = get_lc_status(node, args.mocap_system) if ok: @@ -58,6 +52,7 @@ def main(self, *, args): node.get_logger().info(args.mocap_system + ' stopped') else: node.get_logger().error('Error getting status of ' + args.mocap_system) - + else: - node.get_logger().error('Mocap System ' + args.mocap_system + ' not in detected MOCAP4ROS2 Control System') + node.get_logger().error('Mocap System ' + args.mocap_system + + ' not in detected MOCAP4ROS2 Control System') diff --git a/mocap4r2cli/setup.py b/mocap4r2cli/setup.py index b5cf007..e581e58 100644 --- a/mocap4r2cli/setup.py +++ b/mocap4r2cli/setup.py @@ -1,10 +1,6 @@ from setuptools import find_packages from setuptools import setup - -from setuptools import find_packages -from setuptools import setup - package_name = 'mocap4r2cli' setup(