def get_value_for_key(self, key):
try:
value = self.data[key]
return value
except KeyError:
output.console_log_bold(f"Value requested from config.json with unknown key: ['{key}']")
sys.exit(1)
def programmatic_run_stop(self):
output.console_log_bold(
f"Running experiment run with programmatic stop.")
dir_path = os.path.dirname(os.path.realpath(__file__))
self.run_poll_proc = subprocess.Popen(
f"{sys.executable} {dir_path}/Scripts/PollRunCompletion.py",
shell=True)
def subprocess_check_output(command: str, name: str):
try:
return subprocess.check_output(command, shell=True)
except:
output.console_log_bold(
f"Something went wrong checking output for command: {command} for name: {name}"
)
sys.exit(1)
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 load_json(self, config_path):
try:
with open(config_path) as config_file:
return json.load(config_file)
except FileNotFoundError:
output.console_log("File not found, make sure file exists or path is correct...")
sys.exit(0)
except ValueError:
output.console_log("Decoding JSON has failed, please check validity of config file...")
sys.exit(0)
def subprocess_spawn(command: str, name: str):
try:
if ProcessProcedure.verbose:
return subprocess.Popen(command, shell=True)
return subprocess.Popen(command,
shell=True,
stdout=ProcessProcedure.block_out,
stderr=ProcessProcedure.block_out)
except:
output.console_log_bold(
f"Something went wrong spawning subprocess {name}")
sys.exit(1)
def subprocess_call(command: str, name: str):
try:
if ProcessProcedure.verbose:
subprocess.call(command, shell=True)
return
subprocess.call(command,
shell=True,
stdout=ProcessProcedure.block_out,
stderr=ProcessProcedure.block_out)
except:
output.console_log_bold(
f"Something went wrong calling command: {command} for name: {name}"
)
sys.exit(1)
def main(config_path_arg, verbose=False):
# CTRL+C (SIGINT), add necessary graceful exit procedures here.
def signal_handler(sig, frame):
output.console_log_bold("SIGINT, Terminating Robot Runner\n",
empty_line=True)
sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)
config_path = Path(config_path_arg)
output.console_log(f"Initialising experiment for {config_path.absolute()}")
ProcessProcedure.verbose = verbose # Set verbose, if True, subprocesses will output to terminal.
robot_runner = RobotRunner(config_path)
robot_runner.do_experiment()
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 native_shutdown(self):
output.console_log("Shutting down native run...")
# TODO: Implement this, impossible as of now because of know Linux Kernel bug
# on ARM devices. Raspberry Pi overheats and crashes due to inability to throttle CPU.
# Get all nodes, for each node ros2 lifecycle nodename shutdown
pass
# ======= ROS 1 =======
# 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):
self.ros.roscore_start()
self.wait_for_necessary_topics_and_nodes()
output.console_log_bold(
"All necessary nodes and topics available, everything is ready for experiment!"
)
self.ros.rosbag_start_recording_topics(
self.config.topics_to_record, # Topics to record
str(self.run_dir.absolute()) + '/topics', # Path to record .bag to
f"rosbag_run{self.current_run}" # Bagname to kill after run
)
self.run_runscript_if_present()
self.set_run_stop()
self.run_start()
self.run_wait_completed()
self.ros.rosbag_stop_recording_topics(f"rosbag_run{self.current_run}")
self.ros.native_shutdown()
def do_run(self):
# Check if a launch file is given, if so then run that as the experiment definition.
# Otherwise, run a run_script if present. If not; throw error: no experiment definition available.
if self.config.launch_file_path != "":
self.ros.roslaunch_launch_file(
launch_file=self.config.launch_file_path)
else:
if self.config.run_script_model.path == "" or self.config.run_script_model.path is None:
output.console_log_bold(
"ERROR! No launch file or run script present... No experiment definition available!"
)
sys.exit(1)
self.run_runscript_if_present()
self.wait_for_simulation() # Wait until Gazebo simulator is running
self.wait_for_necessary_topics_and_nodes()
output.console_log("Performing run...")
# Simulation running, start recording topics
self.ros.rosbag_start_recording_topics(
self.config.topics_to_record, # Topics to record
str(self.run_dir.absolute()) + '/topics', # Path to record .bag to
f"rosbag_run{self.current_run}" # Bagname to kill after run
)
if self.config.launch_file_path != "":
# If the user set a script to be run while running an experiment run, run it.
self.run_runscript_if_present()
# Set run stop, timed or programmatic
self.set_run_stop()
self.run_start()
self.run_wait_completed()
self.ros.rosbag_stop_recording_topics(f"rosbag_run{self.current_run}")
self.ros.sim_shutdown()
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 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 do_experiment(self):
self.config.exp_dir.mkdir(parents=True, exist_ok=True)
current_run: int = 1
while current_run <= self.config.replications:
run_controller = None
if self.config.use_simulator:
run_controller = SimRunContoller(self.config, current_run)
else:
run_controller = NativeRunController(self.config, current_run)
run_controller.do_run()
current_run += 1
time_btwn_runs = self.config.time_between_run
if time_btwn_runs > 0 and time_btwn_runs is not None:
output.console_log_bold(
f"Run fully ended, waiting for: {time_btwn_runs}ms == {time_btwn_runs / 1000}s"
)
time.sleep(time_btwn_runs / 1000)
if not self.config.use_simulator:
self.signal_experiment_end()
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 rosbag_start_recording_topics(self, topics, file_path, bag_name):
file_path += "-ros2"
output.console_log(f"Rosbag2 starts recording...")
output.console_log_bold("Recording topics: ")
# Build 'rosbag record -O filename [/topic1 /topic2 ...] __name:=bag_name' command
command = f" ros2 bag record --output {file_path}"
for topic in topics:
command += f" {topic}"
output.console_log_bold(f" * {topic}")
ProcessProcedure.subprocess_spawn(command, "ros2bag_record")
time.sleep(1) # Give rosbag recording some time to initiate
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 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 __init__(self, config_path):
now = datetime.now().strftime("%d_%m_%Y-%H:%M:%S")
self.data = self.load_json(config_path)
# ===== LOAD DEFAULT CONFIG VALUES =====
self.use_simulator = self.get_value_for_key('use_simulator')
self.name = self.get_value_for_key('name')
try:
self.ros_version = int(os.environ['ROS_VERSION'])
except ValueError:
output.console_log_bold(
"Unknown value for $ROS_VERSION env variable")
sys.exit(1)
self.replications = self.get_value_for_key('replications')
self.duration = self.get_value_for_key('duration')
self.launch_file_path = Path(
self.get_value_for_key('launch_file_path'))
script_json = self.get_value_for_key('run_script')
self.run_script_model = RunScriptModel(script_json['path'],
script_json['args'])
self.topics_must_be_available = self.get_value_for_key(
'topics_must_be_available')
self.nodes_must_be_available = self.get_value_for_key(
'nodes_must_be_available')
self.output_path = Path(self.get_value_for_key('output_path'))
self.topics_to_record = self.get_value_for_key('topics_to_record')
self.time_between_run = self.get_value_for_key('time_between_run')
# Build experiment specific, unique path
self.exp_dir = Path(
str(self.output_path.absolute()) + f"/{self.name}-{now}")
output.console_log("Experiment config successfully loaded in")
output.console_log_tabulate(self.data)
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)
import os
import sys
import subprocess
from pathlib import Path
from abc import ABC, abstractmethod
from Procedures.OutputProcedure import OutputProcedure as output
try:
ros_version = int(os.environ['ROS_VERSION'])
except ValueError:
output.console_log_bold("Unknown value for $ROS_VERSION env variable")
sys.exit(1)
### =========================================================
### | |
### | IROSController |
### | - Provide abstract, implementation specific |
### | methods for ROS1 or ROS2 to implement |
### | |
### | - Provide default, generic functions for both |
### | ROS versions like get_available_topics() |
### | |
### | * Any function which is implementation |
### | specific (ROS1 or ROS2) should be declared |
### | here as an abstract function |
### | |
### | * Any generic functionality between the two |
### | ROS types should be declared here |
### | as a function |
### | |
def roslaunch_launch_file(self, launch_file: Path):
output.console_log(f"ros2 launch {launch_file}")
command = f"ros2 launch {launch_file}"
self.roslaunch_proc = ProcessProcedure.subprocess_spawn(command, "ros2_launch_file")
def signal_handler(sig, frame):
output.console_log_bold("SIGINT, Terminating Robot Runner\n",
empty_line=True)
sys.exit(0)
def timed_run_stop(self):
output.console_log_bold(
f"Running experiment run for: {self.config.duration}ms == {self.config.duration / 1000}s"
)
self.timed_stop = True
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...")
def run(self):
output.console_log(f"Running script: {self.path}")
output.console_log(f"Script command: {self.command}")
ProcessProcedure.subprocess_spawn(self.command, "run_script_model")
import sys
from pathlib import Path
from Controllers.SearchController import SearchController
from Procedures.OutputProcedure import OutputProcedure as output
def main(config_path):
config_path = Path(config_path)
search_controller = SearchController(config_path)
search_controller.do_search()
if __name__ == "__main__":
argv_count = len(sys.argv)
if argv_count == 2 and sys.argv[1] == "--help": # Help CLI
output.console_log("usage: python3 %s [PATH_TO_CONFIG.JSON]" %
__file__)
# output.console_log_bold("optional: python3 %s --verbose [PATH_TO_CONFIG.JSON]" % __file__)
sys.exit(0)
elif argv_count == 2:
main(sys.argv[1])
# elif argv_count == 3 and sys.argv[1] == '--verbose':
# main(sys.argv[2], verbose=True)
else:
output.console_log(
"Incorrect usage, please run with --help to view possible arguments"
)
sys.exit(0)