diff options
author | Greg Brown <gmb60@cam.ac.uk> | 2022-01-31 00:17:58 +0000 |
---|---|---|
committer | Greg Brown <gmb60@cam.ac.uk> | 2022-01-31 00:17:58 +0000 |
commit | 1678bf6e4f607658fe504f4870fd213d0bdf5f6a (patch) | |
tree | a809b40d8ac6ca4543046856167c44c13f1c2cbf /scripts/launch-robots.py | |
parent | f6ee0e36c9cc075e5f007c44e95dc3aaa7736a57 (diff) |
Add a launch file for gazebo.
Diffstat (limited to 'scripts/launch-robots.py')
-rwxr-xr-x | scripts/launch-robots.py | 58 |
1 files changed, 0 insertions, 58 deletions
diff --git a/scripts/launch-robots.py b/scripts/launch-robots.py deleted file mode 100755 index 8b09e0f..0000000 --- a/scripts/launch-robots.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python3 - -import math -import sys -from typing import Tuple - -ORIGIN = [0.0, 0.0] -DISTANCE = 1.5 - -TEMPLATE = """ - <group ns="robot{n}"> - <param name="tf_prefix" value="robot{n}" /> - <include file="$(find murl)/launch/one-robot.launch"> - <arg name="robot_name" value="Robot{n}"/> - <arg name="x_pos" value="{x}"/> - <arg name="y_pos" value="{y}"/> - <arg name="z_pos" value="{z}"/> - </include> - </group> -""" - -MAGIC_STRING = "<!-- INCLUDE ROBOTS -->" - -def get_pose(i : int, n : int) -> Tuple[float, float, float]: - angle = 2 * math.pi * i / n - x = ORIGIN[0] + DISTANCE * math.cos(angle) - y = ORIGIN[1] + DISTANCE * math.sin(angle) - z = 0.0 - return (x , y , z) - - -def make_robot_groups(n : int) -> str: - output = "" - for i in range(n): - x , y , z = get_pose(i, n) - output += TEMPLATE.format( - n = i, - x = round(x, 3), - y = round(y, 3), - z = round(z, 3)) - return output - - -def splice_robots(text : str, n : int) -> str: - return text.replace(MAGIC_STRING, make_robot_groups(n)) - - -def main(filename : str, n : int): - with open(filename, 'r') as f: - print(splice_robots(f.read(), n)) - - -if __name__ == '__main__': - if len(sys.argv) != 3: - sys.exit(1) - filename = sys.argv[1] - robots = int(sys.argv[2]) - main(filename, robots) |