Skip to content

Commit

Permalink
Lintering
Browse files Browse the repository at this point in the history
Signed-off-by: Francisco Martín Rico <[email protected]>
  • Loading branch information
fmrico committed Jan 29, 2024
1 parent 50b1e5b commit a05254b
Show file tree
Hide file tree
Showing 7 changed files with 43 additions and 48 deletions.
3 changes: 2 additions & 1 deletion mocap4r2cli/mocap4r2cli/api/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand Down
1 change: 1 addition & 0 deletions mocap4r2cli/mocap4r2cli/command/mocap4r2.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""

Expand Down
37 changes: 20 additions & 17 deletions mocap4r2cli/mocap4r2cli/verb/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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:
Expand All @@ -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

Expand Down Expand Up @@ -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.
Expand Down
18 changes: 7 additions & 11 deletions mocap4r2cli/mocap4r2cli/verb/start.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand All @@ -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:
Expand All @@ -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')
9 changes: 6 additions & 3 deletions mocap4r2cli/mocap4r2cli/verb/status.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand All @@ -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'
Expand Down
19 changes: 7 additions & 12 deletions mocap4r2cli/mocap4r2cli/verb/stop.py
Original file line number Diff line number Diff line change
Expand Up @@ -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."""
Expand All @@ -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:
Expand All @@ -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')
4 changes: 0 additions & 4 deletions mocap4r2cli/setup.py
Original file line number Diff line number Diff line change
@@ -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(
Expand Down

0 comments on commit a05254b

Please sign in to comment.