def subprocess_terminate(proc: Popen, name: str):
output.console_log(f"Terminating process: {name}...")
proc.send_signal(signal.SIGINT)
os.kill(proc.pid, signal.SIGINT)
while proc.poll() is None:
output.console_log_animated(
f"Waiting for graceful exit of process: {name}...")
output.console_log(f"Successfully terminated process: {name}!",
empty_line=True)
def wait_for_simulation(self):
while not self.is_gazebo_running():
output.console_log_animated(
"Waiting for simulation to be running...")
output.console_log(
"Simulation detected to be running, everything is ready for experiment!"
)
time.sleep(1) # Grace period
def roscore_start(self):
output.console_log("Starting ROS Master (roscore)...")
self.roscore_proc = ProcessProcedure.subprocess_spawn("roscore", "roscore_start")
while not ProcessProcedure.process_is_running('roscore') and \
not ProcessProcedure.process_is_running('rosmaster') and \
not ProcessProcedure.process_is_running('rosout'):
output.console_log_animated("Waiting to confirm roscore running...")
output.console_log_bold("ROS Master (roscore) is running!")
time.sleep(1) # Give roscore some time to initiate
def signal_experiment_end(self):
output.console_log("Experiment ended, signalling end to robot...")
dir_path = os.path.dirname(os.path.realpath(__file__))
exp_end_proc = subprocess.Popen(
f"{sys.executable} {dir_path}/Scripts/SignalExperimentEnd.py",
shell=True)
while exp_end_proc.poll() is None:
output.console_log_animated(
"Waiting for robot to confirm experiment end...")
output.console_log_bold("Successfully ended experiment!")
def native_shutdown(self):
output.console_log("Shutting down native run...")
ProcessProcedure.subprocess_call('rosnode kill -a', "rosnode_kill")
ProcessProcedure.process_kill_by_name('rosmaster')
ProcessProcedure.process_kill_by_name('roscore')
ProcessProcedure.process_kill_by_name('rosout')
while ProcessProcedure.process_is_running('rosmaster') and \
ProcessProcedure.process_is_running('roscore') and \
ProcessProcedure.process_is_running('rosout'):
output.console_log_animated("Waiting for roscore to gracefully exit...")
output.console_log("Native run successfully shutdown!")
def do_run(self):
cmd_roslaunch = "roslaunch" if ros_version == 1 else "ros2 launch"
cmd_roslaunch += f" {self.launch_file_path}"
if self.launch_file_path != "":
cmd_roslaunch = self.launch_command
if self.verbose:
self.roslaunch_proc = subprocess.Popen(f"{cmd_roslaunch}", shell=True,
stdout=open(os.devnull, 'w'),
stderr=subprocess.STDOUT)
else:
self.roslaunch_proc = subprocess.Popen(f"{cmd_roslaunch} {self.launch_file_path}", shell=True)
while self.roslaunch_proc.poll() is None:
output.console_log_animated("Waiting for run to complete...")
def run_wait_completed(self):
start_time = time.time() * 1000
while self.running:
output.console_log_animated("Waiting for run to complete...")
# When run needs to stop using a timed stop
if self.timed_stop:
diff = (time.time() * 1000) - start_time
self.running = (diff <= self.config.duration)
# When run needs to stop using programmatic stop, poll if check still runs
if self.config.duration == 0:
self.running = self.run_poll_proc.poll(
) is None # TODO: check return code non-zero (error)
output.console_log("Run completed!", empty_line=True)
self.run_complete_gracefully()
def sim_shutdown(self):
output.console_log("Shutting down sim run...")
ProcessProcedure.subprocess_call("ros2 service call /reset_simulation std_srvs/srv/Empty {}", "ros2_reset_call")
ProcessProcedure.process_kill_by_name("gzserver")
ProcessProcedure.process_kill_by_name("gzclient")
ProcessProcedure.process_kill_by_name("_ros2_daemon")
ProcessProcedure.process_kill_by_name("ros2")
ProcessProcedure.process_kill_by_cmdline("/opt/ros/")
while ProcessProcedure.process_is_running("gzserver") or \
ProcessProcedure.process_is_running("gzclient") or \
ProcessProcedure.process_is_running("ros2") or \
ProcessProcedure.process_is_running("_ros2_daemon"):
output.console_log_animated("Waiting for graceful exit...")
output.console_log("Sim run successfully shutdown!")
def sim_shutdown(self):
output.console_log("Shutting down sim run...")
subprocess.call("rosservice call /gazebo/reset_simulation \"{}\"", shell=True)
self.roslaunch_proc.send_signal(signal.SIGINT)
try:
with open("/tmp/roslaunch.pid", 'r') as myfile:
pid = int(myfile.read())
os.kill(pid, signal.SIGINT)
except FileNotFoundError:
output.console_log("roslaunch pid not found, continuing normally...")
while self.roslaunch_proc.poll() is None:
output.console_log_animated("Waiting for graceful exit...")
ProcessProcedure.subprocess_call('rosnode kill -a', "rosnode_kill")
output.console_log("Sim run successfully shutdown!")
def __init__(self, config_path, verbose=False):
self.verbose = verbose
self.data = self.load_json(config_path)
self.launch_command = self.get_value_for_key('launch_command')
self.launch_file_path = self.get_value_for_key('launch_file_path')
dir_path = os.path.dirname(os.path.realpath(__file__))
while True:
while ros_version == 1 and not self.is_roscore_ready():
output.console_log_animated("Waiting for ROS Master...")
output.console_log_bold("ROS Master ready!", empty_line=True)
self.exp_end_proc = subprocess.Popen(f"{sys.executable} {dir_path}/Scripts/PollExperimentEnd.py",
shell=True)
output.console_log_bold("Grace period for poll_experiment_end")
time.sleep(5)
if self.exp_end_proc.poll() is None:
self.do_run()
else:
sys.exit(0)
def wait_for_necessary_topics_and_nodes(self):
while not self.ros.are_nodes_available(self.config.nodes_must_be_available) and \
self.ros.are_topics_available(self.config.topics_must_be_available):
output.console_log_animated(
"Waiting for necessary nodes and topics to be available...")