From cfb8c3b56854299eed657f6c9d40cb5daeb991b2 Mon Sep 17 00:00:00 2001 From: xfiderek Date: Fri, 6 Sep 2024 13:52:32 +0200 Subject: [PATCH] NASA_Challenge_[@xfiderek] Add ros_trick_bridge package (#42). Adds package that spawns trick simulation as a subprocess and controls its lifecycle as ROS2 managed noe --- .../launch/ros_trick_bridge.launch.py | 150 +++++++++++++ .../ros_src/ros_trick_bridge/package.xml | 18 ++ .../ros_trick_bridge_example_params.yaml | 8 + .../resource/ros_trick_bridge | 14 ++ .../ros_trick_bridge/__init__.py | 13 ++ .../ros_trick_bridge/ros_trick_bridge_node.py | 198 ++++++++++++++++++ .../trick_variable_server_client.py | 85 ++++++++ ros_trick/ros_src/ros_trick_bridge/setup.cfg | 4 + ros_trick/ros_src/ros_trick_bridge/setup.py | 30 +++ 9 files changed, 520 insertions(+) create mode 100644 ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py create mode 100644 ros_trick/ros_src/ros_trick_bridge/package.xml create mode 100644 ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml create mode 100644 ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge create mode 100644 ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py create mode 100644 ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py create mode 100644 ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py create mode 100644 ros_trick/ros_src/ros_trick_bridge/setup.cfg create mode 100644 ros_trick/ros_src/ros_trick_bridge/setup.py diff --git a/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py b/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py new file mode 100644 index 00000000..1a616cc5 --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/launch/ros_trick_bridge.launch.py @@ -0,0 +1,150 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://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 launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + LogInfo, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.events.matchers import matches_action +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LifecycleNode +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +from launch_ros.substitutions import FindPackageShare +from lifecycle_msgs.msg import Transition + + +def generate_launch_description(): + ld = LaunchDescription() + + namespace_arg = DeclareLaunchArgument( + "namespace", default_value="", description="ROS2 namespace" + ) + node_name_arg = DeclareLaunchArgument( + "node_name", default_value="ros_trick_bridge", description="Node name" + ) + autostart_arg = DeclareLaunchArgument( + "autostart", + default_value="True", + description="The bridge is a lifecycle node. Set this argument to true to transit to active on startup (start the simulation)", + ) + param_file_arg = DeclareLaunchArgument( + "param_file", + default_value=[ + FindPackageShare("ros_trick_bridge"), + "/params/ros_trick_bridge_example_params.yaml", + ], + description="ROS Trick Bridge config", + ) + + ros_trick_bridge_lifecycle_node = LifecycleNode( + package="ros_trick_bridge", + executable="ros_trick_bridge_node", + name=LaunchConfiguration("node_name"), + namespace=LaunchConfiguration("namespace"), + parameters=[LaunchConfiguration("param_file")], + output="both", + emulate_tty=True, + ) + + ros_trick_emit_configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(ros_trick_bridge_lifecycle_node), + transition_id=Transition.TRANSITION_CONFIGURE, + ), + condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])), + ) + ros_trick_emit_activate_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(ros_trick_bridge_lifecycle_node), + transition_id=Transition.TRANSITION_ACTIVATE, + ), + condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])), + ) + ros_trick_inactive_state_handler = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ros_trick_bridge_lifecycle_node, + goal_state="inactive", + entities=[ + ros_trick_emit_activate_event, + ], + ), + condition=IfCondition(PythonExpression([LaunchConfiguration("autostart")])), + ) + + # Add logging for debugging + ld.add_action(LogInfo(msg="Launching ROS Trick Bridge Lifecycle Node")) + ld.add_action(namespace_arg) + ld.add_action(node_name_arg) + ld.add_action(autostart_arg) + ld.add_action(param_file_arg) + ld.add_action(ros_trick_bridge_lifecycle_node) + ld.add_action(ros_trick_inactive_state_handler) + ld.add_action(ros_trick_emit_configure_event) + ld.add_action(LogInfo(msg="Emitting configure event")) + + # Add event handlers for logging state transitions + ld.add_action( + RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ros_trick_bridge_lifecycle_node, + start_state="unconfigured", + goal_state="configuring", + entities=[ + LogInfo(msg="Transitioning from 'unconfigured' to 'configuring'") + ], + ) + ) + ) + + ld.add_action( + RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ros_trick_bridge_lifecycle_node, + start_state="configuring", + goal_state="inactive", + entities=[ + LogInfo(msg="Transitioning from 'configuring' to 'inactive'") + ], + ) + ) + ) + + ld.add_action( + RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ros_trick_bridge_lifecycle_node, + start_state="inactive", + goal_state="activating", + entities=[LogInfo(msg="Transitioning from 'inactive' to 'activating'")], + ) + ) + ) + + ld.add_action( + RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=ros_trick_bridge_lifecycle_node, + start_state="activating", + goal_state="active", + entities=[LogInfo(msg="Transitioning from 'activating' to 'active'")], + ) + ) + ) + + return ld diff --git a/ros_trick/ros_src/ros_trick_bridge/package.xml b/ros_trick/ros_src/ros_trick_bridge/package.xml new file mode 100644 index 00000000..389ce4f2 --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/package.xml @@ -0,0 +1,18 @@ + + + + ros_trick_bridge + 0.0.0 + Bridge between ROS and NASA Trick simulator + Blazej Fiderek (xfiderek) + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml b/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml new file mode 100644 index 00000000..b7cfcbaa --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml @@ -0,0 +1,8 @@ +ros_trick_bridge: + ros__parameters: + sim_directory: /opt/trick_sims/SIM_trick_canadarm + sim_inputfile: RUN_2DPlanar/input.py + sim_executable: S_main_Linux_11.4_x86_64.exe + sim_args: '' + publish_clock: true + clock_publish_period: 0.02 diff --git a/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge b/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge new file mode 100644 index 00000000..c21c341a --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge @@ -0,0 +1,14 @@ +// Copyright 2024 Blazej Fiderek (xfiderek) +// +// 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 +// +// https://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. + diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py new file mode 100644 index 00000000..be353d1a --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py @@ -0,0 +1,13 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://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. diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py new file mode 100644 index 00000000..7c6b921c --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/ros_trick_bridge_node.py @@ -0,0 +1,198 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://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. + + +import os +import subprocess +import threading +import time + +import rclpy +from builtin_interfaces.msg import Time +from rclpy.executors import SingleThreadedExecutor +from rclpy.lifecycle import Node, State, TransitionCallbackReturn +from ros_trick_bridge.trick_variable_server_client import TrickVariableServerClient +from rosgraph_msgs.msg import Clock + + +class RosTrickBridgeNode(Node): + TRICK_TICK_VALUE_VAR_NAME = "trick_sys.sched.time_tic_value" + TRICK_TIME_TICS_VAR_NAME = "trick_sys.sched.time_tics" + + def __init__(self, node_name: str, **kwargs): + self._trick_subprocess = None + self._std_print_period = 0.1 + self._stdout_print_thread = None + + self._trick_variable_server_client = None + self._clock_publisher = None + + super().__init__(node_name, **kwargs) + + # def on_init(self, state: State) -> TransitionCallbackReturn: + # self.get_logger().info('nddd') + + def on_configure(self, state: State) -> TransitionCallbackReturn: + # Declare ROS 2 parameters + self.declare_parameter("sim_directory", "/path/to/sim") + self.declare_parameter("sim_executable", "S_main_Linux_11.4_x86_64.exe") + self.declare_parameter("sim_inputfile", "RUN_2DPlanar/input.py") + self.declare_parameter("sim_args", "") + self.declare_parameter("trick_host", "localhost") + self.declare_parameter("trick_port", 49765) + self.declare_parameter("publish_clock", True) + self.declare_parameter("clock_publish_period", 0.1) + + self.get_logger().info(f"Configuring from state: {state.label}") + if not "TRICK_HOME" in os.environ: + error = "TRICK_HOME env variable is not set, terminating" + self.get_logger().error(error) + raise Exception(error) + + # Get the parameters + sim_directory = ( + self.get_parameter("sim_directory").get_parameter_value().string_value + "/" + ) + sim_executable = ( + self.get_parameter("sim_executable").get_parameter_value().string_value + ) + sim_inputfile = ( + self.get_parameter("sim_inputfile").get_parameter_value().string_value + ) + sim_args = self.get_parameter("sim_args").get_parameter_value().string_value + try: + cmd = [f"./{sim_executable}", sim_inputfile, sim_args] + self.get_logger().info(f"Starting trick with command: {cmd}") + self._trick_subprocess = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + cwd=sim_directory, + ) + self.get_logger().info("######################################") + self.get_logger().info("Trick subprocess started successfully.") + self.get_logger().info("######################################") + self.get_logger().info(f"Trick pid is {self._trick_subprocess.pid}") + except Exception as e: + self.get_logger().error("######################################") + self.get_logger().error(f"Failed to start trick subprocess: {e}") + self.get_logger().error("######################################") + return TransitionCallbackReturn.FAILURE + + # it must run on a separate thread to avoid IO blocking + self._stdout_print_thread = threading.Thread( + target=self._print_subprocess_stdout + ) + self._stdout_print_thread.start() + + # give a process some time to spin up + time.sleep(1.0) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: State) -> TransitionCallbackReturn: + if self.get_parameter("publish_clock").get_parameter_value().bool_value: + trick_data_callback = self._publish_clock_from_trick + self._clock_publisher = self.create_publisher( + msg_type=Clock, topic="/clock", qos_profile=10 + ) + else: + # we don't need any data from trick otherwise + trick_data_callback = None + + try: + self._trick_variable_server_client = TrickVariableServerClient( + self.get_parameter("trick_host").get_parameter_value().string_value, + self.get_parameter("trick_port").get_parameter_value().integer_value, + "ros_trick_bridge", + [self.TRICK_TICK_VALUE_VAR_NAME, self.TRICK_TIME_TICS_VAR_NAME], + self.get_parameter("clock_publish_period") + .get_parameter_value() + .double_value, + trick_data_callback, + ) + except Exception as e: + self.get_logger().error( + f"Failed to start trick variable server client: {e}" + ) + return TransitionCallbackReturn.FAILURE + + self.get_logger().info(f"Activating from state: {state.label}") + self._trick_variable_server_client.unfreeze_sim() + return super().on_activate(state) + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(f"Deactivating from state: {state.label}") + self._trick_variable_server_client.freeze_sim() + return super().on_deactivate(state) + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(f"Cleaning up from state: {state.label}") + self._teardown_sim() + + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(f"Shutting down from state: {state.label}") + self._teardown_sim() + + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info(f"Error from state: {state.label}") + return TransitionCallbackReturn.SUCCESS + + def _teardown_sim(self): + self._trick_variable_server_client = None + + # Terminate the subprocess if it's still running + if self._trick_subprocess and self._trick_subprocess.poll() is None: + self._trick_subprocess.terminate() + + def _publish_clock_from_trick(self, trick_data): + ticks_per_second = int(trick_data[self.TRICK_TICK_VALUE_VAR_NAME]) + curr_time_tics = int(trick_data[self.TRICK_TIME_TICS_VAR_NAME]) + seconds = curr_time_tics // ticks_per_second + nanoseconds = (curr_time_tics % ticks_per_second) * ( + 1000000000 // ticks_per_second + ) + clock_msg = Clock() + clock_msg.clock = Time() + clock_msg.clock.sec = seconds + clock_msg.clock.nanosec = nanoseconds + self._clock_publisher.publish(clock_msg) + + def _print_subprocess_stdout(self): + while True: + if self._trick_subprocess: + # Read from stdout + stdout_line = self._trick_subprocess.stdout.readline() + if stdout_line: + self.get_logger().info(stdout_line) + time.sleep(self._std_print_period) + + +def main(args=None): + rclpy.init(args=args) + executor = SingleThreadedExecutor() + lifecycle_node = RosTrickBridgeNode("ros_trick_bridge") + executor.add_node(lifecycle_node) + try: + executor.spin() + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + lifecycle_node._teardown_sim() + + +if __name__ == "__main__": + main() diff --git a/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py new file mode 100644 index 00000000..b5ad418a --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py @@ -0,0 +1,85 @@ +# Copyright 2024 Blazej Fiderek (xfiderek) +# +# 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 +# +# https://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. + + +import socket +import threading +import time + + +class TrickVariableServerClient: + def __init__( + self, + host, + port, + client_tag, + trick_variable_names, + trick_var_cycle_time=0.1, + on_receive_callback=None, + ): + self._client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self._client_socket.connect((host, port)) + + self._insock = self._client_socket.makefile("r") + + self._variable_names = trick_variable_names + self._latest_data = {} + self._on_receive_callback = on_receive_callback + self._trick_var_cycle_time = trick_var_cycle_time + + # send configuration info to trick and ask for variables + self._client_socket.send( + f'trick.var_set_client_tag("{client_tag}") \n'.encode() + ) + self._client_socket.send( + f"trick.var_cycle({str(trick_var_cycle_time)}) \n".encode() + ) + + self._receive_thread = threading.Thread(target=self._rcv_trick_data_from_socket) + self._receive_thread.start() + + def freeze_sim(self): + self._client_socket.send("trick.exec_freeze()\n".encode()) + self.stop_listening() + + def unfreeze_sim(self): + self.start_listening() + self._client_socket.send("trick.exec_run()\n".encode()) + + def start_listening(self): + command = b"" + for trick_var_name in self._variable_names: + command += f'trick.var_add("{trick_var_name}") \n'.encode() + self._client_socket.send("trick.var_ascii()\n".encode()) + self._client_socket.send(command) + + def stop_listening(self): + self._client_socket.send("trick.var_clear()\n".encode()) + + def send_data_to_sim(self, var_name: str, value: str) -> None: + self._client_socket.send(f"{var_name} = {value} \n".encode()) + + def _rcv_trick_data_from_socket(self): + while True: + if self._insock: + line = self._insock.readline() + if line: + variables = line.split("\t") + if variables[0] == "0": + for i, var_name in enumerate(self._variable_names): + self._latest_data[var_name] = variables[i + 1].strip() + if self._on_receive_callback: + self._on_receive_callback(self._latest_data.copy()) + else: + time.sleep(self._trick_var_cycle_time) diff --git a/ros_trick/ros_src/ros_trick_bridge/setup.cfg b/ros_trick/ros_src/ros_trick_bridge/setup.cfg new file mode 100644 index 00000000..19ca6234 --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros_trick_bridge +[install] +install_scripts=$base/lib/ros_trick_bridge diff --git a/ros_trick/ros_src/ros_trick_bridge/setup.py b/ros_trick/ros_src/ros_trick_bridge/setup.py new file mode 100644 index 00000000..f65525fa --- /dev/null +++ b/ros_trick/ros_src/ros_trick_bridge/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "ros_trick_bridge" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.py")), + (os.path.join("share", package_name, "params"), glob("params/*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Blazej Fiderek (xfiderek)", + maintainer_email="fiderekblazej@gmail.com", + description="", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "ros_trick_bridge_node = ros_trick_bridge.ros_trick_bridge_node:main" + ], + }, +)