Skip to content

Remove hardcoded resets and kill all threads #289

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
javizqh merged 1 commit into from
Jun 26, 2026
Merged

Remove hardcoded resets and kill all threads #289

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion robotics_application_manager/manager/launcher/launcher_console.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,8 +52,9 @@ def is_running(self):
return self.running

def terminate(self):
LogManager.logger.info(f"Terminating console tool")
self.console_vnc.terminate()
for thread in self.threads:
for thread in self.threads[:]:
if thread.is_alive():
thread.terminate()
thread.join()
Expand Down
98 changes: 18 additions & 80 deletions robotics_application_manager/manager/launcher/launcher_gzsim.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,62 +20,6 @@
from gz.transport13 import Node


def call_gzservice(service, reqtype, reptype, timeout, req):
command = f"gz service -s {service} --reqtype {reqtype} --reptype {reptype} --timeout {timeout} --req '{req}'"
try:
p = subprocess.Popen(
[
f"{command}",
],
shell=True,
stdout=sys.stdout,
stderr=subprocess.STDOUT,
bufsize=1024,
universal_newlines=True,
)
p.wait(10)
except:
p.kill()

LogManager.logger.exception(f"Unable to complete call: {service}")
raise Exception(f"Unable to complete call: {service}")


def call_service(service, service_type, request_data="{}"):
command = f"ros2 service call {service} {service_type} '{request_data}'"
try:
p = subprocess.Popen(
[
f"{command}",
],
shell=True,
stdout=sys.stdout,
stderr=subprocess.STDOUT,
bufsize=1024,
universal_newlines=True,
)
p.wait(10)
except:
p.kill()

LogManager.logger.exception(f"Unable to complete call: {service}")
raise Exception(f"Unable to complete call: {service}")


def is_ros_service_available(service_name):
try:
result = subprocess.run(
["ros2", "service", "list", "--include-hidden-services"],
capture_output=True,
text=True,
check=True,
)
return service_name in result.stdout
except subprocess.CalledProcessError as e:
LogManager.logger.exception(f"Error checking service availability: {e}")
return False


class LauncherGzsim(ILauncher):
display: str
internal_port: int
Expand Down Expand Up @@ -129,40 +73,37 @@ def is_running(self):
return self.running

def terminate(self):
LogManager.logger.info(f"Terminating gz tool")
self.gz_vnc.terminate()
for thread in self.threads:
thread.terminate()
thread.join()
for thread in self.threads[:]:
if thread.is_alive():
thread.terminate()
thread.join()
self.threads.remove(thread)
self.running = False

def died(self):
pass

def pause(self):
call_gzservice(
"/world/default/control",
"gz.msgs.WorldControl",
"gz.msgs.Boolean",
"3000",
"pause: true",
Node().request(
f"/world/default/control",
WorldControl(pause=True),
WorldControl,
Boolean,
10000,
)

def unpause(self):
call_gzservice(
"/world/default/control",
"gz.msgs.WorldControl",
"gz.msgs.Boolean",
"3000",
"pause: false",
Node().request(
f"/world/default/control",
WorldControl(pause=False),
WorldControl,
Boolean,
10000,
)

def reset(self, robot_entity=None):
if is_ros_service_available("/drone0/platform/state_machine/_reset"):
call_service(
"/drone0/platform/state_machine/_reset",
"std_srvs/srv/Trigger",
"{}",
)
node = Node()

node.request(
Expand Down Expand Up @@ -190,9 +131,6 @@ def reset(self, robot_entity=None):
10000,
)

if is_ros_service_available("/drone0/controller/_reset"):
call_service("/drone0/controller/_reset", "std_srvs/srv/Trigger", "{}")

def get_dri_path(self):
directory_path = "/dev/dri"
dri_path = ""
Expand Down
2 changes: 1 addition & 1 deletion robotics_application_manager/manager/launcher/launcher_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ def run(self, entity="", start_pose=None, extra_config=None):

def terminate(self):
"""Terminate all robot launchers and clear the launchers list."""
LogManager.logger.info("Terminating robot launcher")
LogManager.logger.info("Terminating robots launchers")
if self.launchers:
for launcher in self.launchers:
launcher.terminate()
Expand Down
46 changes: 7 additions & 39 deletions robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,6 @@ def run(self, entity, robot_pose, extra_config, callback):

logging.getLogger("roslaunch").setLevel(logging.CRITICAL)

xserver_cmd = f"/usr/bin/Xorg -quiet -noreset +extension GLX +extension RANDR +extension RENDER -logfile ./xdummy.log -config ./xorg.conf :0"
xserver_thread = DockerThread(xserver_cmd)
xserver_thread.start()
self.threads.append(xserver_thread)

x, y, z, R, P, Y = robot_pose

if extra_config == "None":
Expand All @@ -48,6 +43,7 @@ def run(self, entity, robot_pose, extra_config, callback):

exercise_launch_thread = DockerThread(exercise_launch_cmd)
exercise_launch_thread.start()
self.threads.append(exercise_launch_thread)

# Wait until robot entity has spawned
node = Node()
Expand All @@ -67,37 +63,9 @@ def run(self, entity, robot_pose, extra_config, callback):
LogManager.logger.info("Robot spawned OK")

def terminate(self):
if self.threads is not None:
for thread in self.threads:
if thread.is_alive():
thread.terminate()
thread.join()
self.threads.remove(thread)

kill_cmd = "pkill -9 -f "
cmd = kill_cmd + "spawn_robot.launch.py"
subprocess.call(
cmd,
shell=True,
stdout=subprocess.PIPE,
bufsize=1024,
universal_newlines=True,
)

cmd = kill_cmd + "bridge"
subprocess.call(
cmd,
shell=True,
stdout=subprocess.PIPE,
bufsize=1024,
universal_newlines=True,
)

cmd = kill_cmd + "robot_state_publisher"
subprocess.call(
cmd,
shell=True,
stdout=subprocess.PIPE,
bufsize=1024,
universal_newlines=True,
)
LogManager.logger.info(f"Terminating robot launcher")
for thread in self.threads[:]:
if thread.is_alive():
thread.terminate()
thread.join()
self.threads.remove(thread)
31 changes: 8 additions & 23 deletions robotics_application_manager/manager/launcher/launcher_ros2_api.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
)
from robotics_application_manager.manager.docker_thread import DockerThread
import subprocess
from robotics_application_manager import LogManager

import logging

Expand Down Expand Up @@ -38,28 +39,12 @@ def run(self, callback):

exercise_launch_thread = DockerThread(exercise_launch_cmd)
exercise_launch_thread.start()
self.threads.append(exercise_launch_thread)

def terminate(self):
if self.threads is not None:
for thread in self.threads:
if thread.is_alive():
thread.terminate()
thread.join()
self.threads.remove(thread)

to_kill = ["launch.py"]
if self.type == "gz":
to_kill = ["drones_ws", "gz", "launch.py"]
else:
to_kill = ["gzserver", "launch.py"]

kill_cmd = "pkill -9 -f "
for i in to_kill:
cmd = kill_cmd + i
subprocess.call(
cmd,
shell=True,
stdout=subprocess.PIPE,
bufsize=1024,
universal_newlines=True,
)
LogManager.logger.info(f"Terminating world launcher")
for thread in self.threads[:]:
if thread.is_alive():
thread.terminate()
thread.join()
self.threads.remove(thread)
48 changes: 11 additions & 37 deletions robotics_application_manager/manager/launcher/launcher_rviz.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ class LauncherRviz(ILauncher):
running: bool = False
acceptsMsgs: bool = False
threads: List[Any] = []
console_vnc: Any = Vnc_server()
vnc: Any = Vnc_server()

def run(self, config_file, callback):
"""
Expand All @@ -42,24 +42,21 @@ def run(self, config_file, callback):
if config_file != None:
config = f"ros2 launch {config_file}"

print(config)
if ACCELERATION_ENABLED:
self.console_vnc.start_vnc_gpu(
self.vnc.start_vnc_gpu(
self.display, self.internal_port, self.external_port, DRI_PATH
)
# Write display config and start the console
console_cmd = f"export VGL_DISPLAY={DRI_PATH}; export DISPLAY={self.display}; vglrun {config}"
rviz_cmd = f"export VGL_DISPLAY={DRI_PATH}; export DISPLAY={self.display}; vglrun {config}"
else:
self.console_vnc.start_vnc(
self.display, self.internal_port, self.external_port
)
self.vnc.start_vnc(self.display, self.internal_port, self.external_port)
# Write display config and start the console
console_cmd = f"export DISPLAY={self.display};{config}"
rviz_cmd = f"export DISPLAY={self.display};{config}"

console_thread = DockerThread(console_cmd)
console_thread.start()
self.threads.append(console_thread)
rviz_thread = DockerThread(rviz_cmd)
rviz_thread.start()

self.threads.append(rviz_thread)
self.running = True

def pause(self):
Expand All @@ -75,8 +72,9 @@ def is_running(self):
return self.running

def terminate(self):
self.console_vnc.terminate()
for thread in self.threads:
LogManager.logger.info(f"Terminating rviz tool")
self.vnc.terminate()
for thread in self.threads[:]:
if thread.is_alive():
thread.terminate()
thread.join()
Expand All @@ -85,27 +83,3 @@ def terminate(self):

def died(self):
pass

# rviz_node_full = Node(
# package="rviz2",
# executable="rviz2",
# name="rviz2",
# output="log",
# arguments=["-d", rviz_full_config],
# parameters=[
# robot_description,
# robot_description_semantic,
# kinematics_yaml,

# pilz_planning_pipeline_config,

# joint_limits,
# pilz_cartesian_limits,

# trajectory_execution,
# moveit_controllers,
# planning_scene_monitor_parameters,
# move_group_capabilities,
# {"use_sim_time": True},
# ]
# )
46 changes: 0 additions & 46 deletions robotics_application_manager/manager/launcher/launcher_rviz_ros2.py
View file
Open in desktop

This file was deleted.

2 changes: 1 addition & 1 deletion robotics_application_manager/manager/launcher/launcher_tools.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ def run(self, consumer):
self.launchers.append(launcher)

def terminate(self):
LogManager.logger.info("Terminating tools launcher")
LogManager.logger.info("Terminating tools launchers")
for launcher in self.launchers:
if launcher.is_running():
launcher.terminate()
Expand Down
Loading
Loading