diff --git a/README.md b/README.md index 8f803df..fd237aa 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,6 @@ # Arena Rosnav Task Generator -This is the task generator package designed to work with: - -- [arena-rosnav](https://github.com/Arena-Rosnav/arena-rosnav) -- [arena-rosnav-3d]() - -The task generator is especially designed for single robot simulations. +This is the task generator package designed to work with the [Arena Benchmark](https://github.com/Arena-Rosnav/arena-rosnav) infrastructure. ## Task Modes @@ -15,13 +10,6 @@ Our task generator package offers four task modes. We define a task as the proce Creates random static and dynamic obstacles when a new task is started. When starting the task a random goal and start position is selected. After the robot reaching the goal a new task is started. -### Manual Task - -Manual task mode is the same as the random task except that a goal must be set manually via the rviz *2D Nav Goal* button. - -![image](https://arena-rosnav-wiki.readthedocs.io/en/latest/images/manual_task.png) - - ### Staged Task The staged task mode is designed for the trainings process of arena-rosnav. In general, it behaves like the random task mode but there are multiple stages between one can switch. Between the stages, the amount of static and dynamic obstacles changes. The amount of obstacles is defined in a curriculum file, the path to said file is a key in the `paths` parameter. @@ -51,10 +39,10 @@ robot are defined. The scenario declaration file can be created with [arena-tools](https://github.com/Arena-Rosnav/arena-tools) and has to follow the specified file schema. -## Environment Factory +## Simulator Factory -To be able to use the task generator module in all our environments without changes, a unified interface between environment and task generator is needed. The interface contains a lot of functions to spawn, publish or move robots or obstacles, and a lot more. +To be able to use the task generator module in all our simulators without changes, a unified interface between simulator and task generator is needed. The interface contains a lot of functions to spawn, publish or move robots or obstacles, and a lot more. -At the moment we provide environment interfaces for **Flatland** and **Gazebo**. In order to add a new environment, in which the task generator should be used, a new environment interface has to be created in `/taks_generator/environments/` and has to be registrated in the environment factory. +At the moment we provide simulator interfaces for **Flatland** and **Gazebo**. In order to add a new simulator, in which the task generator should be used, a new simulator interface has to be created in `/taks_generator/simulators/` and has to be registrated in the simulator factory. -Your newly created environment interface should derive the **BaseEnvironment** located [here](TODO) and implement all functions. A detailed description of the functions is contained in the **BaseEnvironment** itself. +Your newly created simulator interface should derive the **BaseSimulator** located [here](TODO) and implement all functions. A detailed description of the functions is contained in the **BaseSimulator** itself. diff --git a/robot_setup/marl_map_empty.yaml b/robot_setup/marl_map_empty.yaml index 7a80485..e310576 100644 --- a/robot_setup/marl_map_empty.yaml +++ b/robot_setup/marl_map_empty.yaml @@ -3,14 +3,18 @@ robots: # planner: rosnav # amount: 6 # agent: rto_tlabs_marl - - model: agvota - planner: teb - amount: 1 - agent: new_jackal - - model: burger - planner: teb - amount: 1 - agent: new_jackal + - model: jackal + planner: rosnav + amount: 2 + agent: jackal_marl + - model: rto + planner: rosnav + amount: 2 + agent: rto_marl + - model: ridgeback + planner: rosnav + amount: 2 + agent: ridgeback_marl # - model: cob4 # planner: teb # amount: 1 diff --git a/robot_setup/rosnav_agents_marl.yaml b/robot_setup/rosnav_agents_marl.yaml index 92238c0..ec142a3 100644 --- a/robot_setup/rosnav_agents_marl.yaml +++ b/robot_setup/rosnav_agents_marl.yaml @@ -1,15 +1,15 @@ robots: # - model: ridgeback - # planner: rosnav - # amount: 1 + # planner: teb + # amount: 6 # agent: ridgeback_marl - model: rto - planner: rosnav - amount: 3 + planner: teb + amount: 6 agent: rto_tlabs_marl # - model: jackal - # planner: rosnav - # amount: 1 + # planner: teb + # amount: 6 # agent: jackal_marl # - model: jackal # planner: rosnav diff --git a/scenarios/marl_map_empty_obs0.json b/scenarios/marl_map_empty_obs0.json index 4100854..4abeab9 100644 --- a/scenarios/marl_map_empty_obs0.json +++ b/scenarios/marl_map_empty_obs0.json @@ -33,8 +33,60 @@ "goal": [29.94602317350325, 20.795265461388748, 0] }, { - "start": [3.0220509108329354, 20.442857447479454, 0], - "goal": [30.58035759853998, 7.262797727271735, 0] + "start": [2.778715176849392, 3.053770554072754, 0], + "goal": [30.146818089333973, 23.16670627326246, 0] + }, + { + "start": [5.237410725688214, 3.3359159449231095, 0], + "goal": [28.010574415752707, 23.16670627326246, 0] + }, + { + "start": [8.05886463419178, 3.0134640696655604, 0], + "goal": [24.826362147584394, 23.126399788855267, 0] + }, + { + "start": [2.980247598885362, 4.545110477138927, 0], + "goal": [31.114173715106627, 21.35291447493874, 0] + }, + { + "start": [2.778715176849392, 8.092081104971982, 0], + "goal": [30.469269964591522, 18.53146056643517, 0] + }, + { + "start": [2.9399411144781684, 11.27629337314029, 0], + "goal": [30.42896348018433, 16.516136346075484, 0] + }, + { + "start": [2.4965697859990357, 14.379892672494213, 0], + "goal": [30.18712457374117, 12.727326811799266, 0] + }, + { + "start": [3.141473536514136, 17.080427127776197, 0], + "goal": [30.348350511369944, 8.93851727752305, 0] + }, + { + "start": [3.101167052106943, 19.619735645429405, 0], + "goal": [30.388656995777136, 6.802273603941779, 0] + }, + { + "start": [2.8190216612565857, 22.803947913597714, 0], + "goal": [30.711108871034686, 2.932851100851173, 0] + }, + { + "start": [5.277717210095408, 22.965173851226492, 0], + "goal": [28.292719806603063, 3.0134640696655604, 0] + }, + { + "start": [8.099171118598974, 22.683028460376136, 0], + "goal": [25.672798320135467, 3.1343835228871413, 0] + }, + { + "start": [10.033882370144276, 22.683028460376136, 0], + "goal": [25.14881402284195, 3.6180613357734708, 0] + }, + { + "start": [10.598173151844989, 22.72333494478333, 0], + "goal": [23.77839355299736, 3.0134640696655604, 0] } ], "map": "map_empty", diff --git a/scripts/task_generator_node.py b/scripts/task_generator_node.py index 6ff3b86..f65a66e 100644 --- a/scripts/task_generator_node.py +++ b/scripts/task_generator_node.py @@ -13,9 +13,9 @@ from task_generator.constants import Constants, TaskMode from task_generator.tasks.utils import get_predefined_task -from task_generator.environments.environment_factory import EnvironmentFactory -from task_generator.environments.gazebo_environment import GazeboEnvironment -from task_generator.environments.flatland_environment import FlatlandRandomModel +from task_generator.simulators.simulator_factory import SimulatorFactory +from task_generator.simulators.gazebo_simulator import GazeboSimulator +from task_generator.simulators.flatland_simulator import FlatlandRandomModel class TaskGenerator: @@ -37,7 +37,7 @@ def __init__(self) -> None: rospy.Service("reset_task", Empty, self.reset_task_srv_callback) ## Vars - self.env_wrapper = EnvironmentFactory.instantiate(Utils.get_environment())("") + self.env_wrapper = SimulatorFactory.instantiate(Utils.get_simulator())("") rospy.loginfo(f"Launching task mode: {self.task_mode}") @@ -60,6 +60,7 @@ def __init__(self) -> None: self.number_of_resets = 0 self.reset_task() + ## Timers rospy.Timer(rospy.Duration(0.5), self.check_task_status) diff --git a/task_generator/constants.py b/task_generator/constants.py index 34e341b..491ada1 100644 --- a/task_generator/constants.py +++ b/task_generator/constants.py @@ -1,6 +1,6 @@ class Constants: GOAL_REACHED_TOLERANCE = 1.0 - TIMEOUT = 3.0 * 60 ## 3 min + TIMEOUT = 60 * 3 ## 3 min WAIT_FOR_SERVICE_TIMEOUT = 5 # 5 secs MAX_RESET_FAIL_TIMES = 3 @@ -15,9 +15,10 @@ class ObstacleManager: class RobotManager: SPAWN_ROBOT_SAFE_DIST = 0.4 - class Environment: + class Simulator: FLATLAND = "flatland" GAZEBO = "gazebo" + UNITY = "unity" class ArenaType: TRAINING = "training" @@ -30,10 +31,10 @@ class TaskMode: SCENARIO = "scenario" class Random: - MIN_DYNAMIC_OBS = 1 - MAX_DYNAMIC_OBS = 3 - MIN_STATIC_OBS = 1 - MAX_STATIC_OBS = 3 + MIN_DYNAMIC_OBS = 10 + MAX_DYNAMIC_OBS = 15 + MIN_STATIC_OBS = 10 + MAX_STATIC_OBS = 20 class Scenario: RESETS_DEFAULT = 5 @@ -60,11 +61,7 @@ class FlatlandRandomModel: "body": "base_link" } LINEAR_VEL = 0.2 - ANGLUAR_VEL_MAX = 0.2 -<<<<<<< HEAD - -======= ->>>>>>> master + ANGLUAR_VEL_MAX = 0.1 class Pedsim: START_UP_MODE = "default" @@ -89,4 +86,7 @@ class Pedsim: FORCE_FACTOR_OBSTACLE = 1.0 FORCE_FACTOR_SOCIAL = 5.0 FORCE_FACTOR_ROBOT = 0.0 - WAYPOINT_MODE = 0 \ No newline at end of file + WAYPOINT_MODE = 0 + +class UnitySimulatorConstants: + UNITY_ROS_NAVIGATION = "UNITY_ROS_NAVIGATION" diff --git a/task_generator/environments/environment_factory.py b/task_generator/environments/environment_factory.py deleted file mode 100644 index 67ec1d6..0000000 --- a/task_generator/environments/environment_factory.py +++ /dev/null @@ -1,23 +0,0 @@ -from .base_environment import BaseEnvironment - -class EnvironmentFactory: - registry = {} - - @classmethod - def register(cls, name): - def inner_wrapper(wrapped_class): - assert name not in cls.registry, f"Environment '{name}' already exists!" - assert issubclass(wrapped_class, BaseEnvironment) - - cls.registry[name] = wrapped_class - return wrapped_class - - return inner_wrapper - - @classmethod - def instantiate(cls, name: str): - assert name in cls.registry, f"Environment '{name}' is not registered!" - - environment = cls.registry[name] - - return environment diff --git a/task_generator/manager/obstacle_manager.py b/task_generator/manager/obstacle_manager.py index e8f2d7d..763638b 100644 --- a/task_generator/manager/obstacle_manager.py +++ b/task_generator/manager/obstacle_manager.py @@ -2,21 +2,21 @@ class ObstacleManager: - def __init__(self, namespace, map_manager, environment): + def __init__(self, namespace, map_manager, simulator): self.map_manager = map_manager self.namespace = namespace - self.environment = environment + self.simulator = simulator def start_scenario(self, scenario): - self.environment.spawn_pedsim_agents(scenario["obstacles"]["dynamic"]) + self.simulator.spawn_pedsim_agents(scenario["obstacles"]["dynamic"]) def reset_scenario(self, scenario): - self.environment.reset_pedsim_agents() + self.simulator.reset_pedsim_agents() - self.environment.remove_all_obstacles() + self.simulator.remove_all_obstacles() for obstacle in scenario["obstacles"]["static"]: - self.environment.spawn_obstacle( + self.simulator.spawn_obstacle( [*obstacle["pos"], 0], yaml_path=obstacle["yaml_path"], ) @@ -27,7 +27,7 @@ def reset_random( static_obstacles=Constants.ObstacleManager.STATIC_OBSTACLES, forbidden_zones=[] ): - self.environment.remove_all_obstacles() + self.simulator.remove_all_obstacles() for _ in range(dynamic_obstacles): position = self.map_manager.get_random_pos_on_map( @@ -35,7 +35,7 @@ def reset_random( forbidden_zones=forbidden_zones ) - self.environment.spawn_random_dynamic_obstacle(position=position) + self.simulator.spawn_random_dynamic_obstacle(position=position) for _ in range(static_obstacles): position = self.map_manager.get_random_pos_on_map( @@ -43,4 +43,4 @@ def reset_random( forbidden_zones=forbidden_zones ) - self.environment.spawn_random_static_obstacle(position=position) \ No newline at end of file + self.simulator.spawn_random_static_obstacle(position=position) \ No newline at end of file diff --git a/task_generator/manager/robot_manager.py b/task_generator/manager/robot_manager.py index b930839..2d3f376 100644 --- a/task_generator/manager/robot_manager.py +++ b/task_generator/manager/robot_manager.py @@ -21,14 +21,13 @@ class RobotManager: position of a robot for all task modes. """ - def __init__(self, namespace, map_manager, environment, robot_setup): + def __init__(self, namespace, map_manager, simulator, robot_setup): self.namespace = namespace self.namespace_prefix = "" if namespace == "" else "/" + namespace + "/" self.ns_prefix = lambda *topic: os.path.join(self.namespace, *topic) - self.map_manager = map_manager - self.environment = environment + self.simulator = simulator self.start_pos = [0, 0] self.goal_pos = [0, 0] @@ -37,12 +36,13 @@ def __init__(self, namespace, map_manager, environment, robot_setup): self.is_goal_reached = False self.robot_setup = robot_setup + self.record_data = rospy.get_param('record_data', False)# and rospy.get_param('task_mode', 'scenario') == 'scenario' - def set_up_robot(self, launch_robot_controller=True): + def set_up_robot(self): if Utils.get_arena_type() == Constants.ArenaType.TRAINING: self.robot_radius = rospy.get_param("robot_radius") - self.environment.spawn_robot(self.namespace, self.robot_setup["model"], self._robot_name()) + self.simulator.spawn_robot(self.namespace, self.robot_setup["model"], self._robot_name()) self.move_base_goal_pub = rospy.Publisher(self.ns_prefix(self.namespace, "move_base_simple", "goal"), PoseStamped, queue_size=10) self.pub_goal_timer = rospy.Timer(rospy.Duration(0.25), self.publish_goal_periodically) @@ -56,9 +56,6 @@ def set_up_robot(self, launch_robot_controller=True): if Utils.get_arena_type() == Constants.ArenaType.TRAINING: return - if not launch_robot_controller: - return - self.launch_robot(self.robot_setup) self.robot_radius = rospy.get_param( @@ -90,10 +87,14 @@ def reset(self, forbidden_zones=[], start_pos=None, goal_pos=None): forbidden_zones, start_pos, goal_pos ) + if self.record_data: + rospy.set_param(os.path.join(self.namespace, "goal"), str(list(self.goal_pos))) + rospy.set_param(os.path.join(self.namespace, "start"), str(list(self.start_pos))) + self.publish_goal(self.goal_pos) self.move_robot_to_start() - self.set_is_goal_goached(self.start_pos, self.goal_pos) + self.set_is_goal_reached(self.start_pos, self.goal_pos) time.sleep(0.1) @@ -154,7 +155,7 @@ def move_robot_to_start(self): self.move_robot_to_pos(self.start_pos) def move_robot_to_pos(self, pos): - self.environment.move_robot(pos, name=self.namespace) + self.simulator.move_robot(pos, name=self.namespace) def _default_position(self, pos, callback_pos): if not pos == None: @@ -166,10 +167,14 @@ def launch_robot(self, robot_setup): roslaunch_file = roslaunch.rlutil.resolve_launch_arguments( ["arena_bringup", "robot.launch"] ) + + print("START WITH MODEL", robot_setup["model"]) + args = [ f"model:={robot_setup['model']}", f"local_planner:={robot_setup['planner']}", f"namespace:={self.namespace}", + f"record_data:={self.record_data}", *([f"agent_name:={robot_setup.get('agent')}"] if robot_setup.get('agent') else []) ] @@ -202,12 +207,12 @@ def launch_robot(self, robot_setup): def robot_pos_callback(self, data): current_position = data.pose.pose.position - self.set_is_goal_goached( + self.set_is_goal_reached( [current_position.x, current_position.y], self.goal_pos ) - def set_is_goal_goached(self, start, goal): + def set_is_goal_reached(self, start, goal): distance_to_goal = math.sqrt( (start[0] - goal[0]) ** 2 + (start[1] - goal[1]) ** 2 diff --git a/task_generator/environments/base_environment.py b/task_generator/simulators/base_simulator.py similarity index 67% rename from task_generator/environments/base_environment.py rename to task_generator/simulators/base_simulator.py index 762afb0..d40e32f 100644 --- a/task_generator/environments/base_environment.py +++ b/task_generator/simulators/base_simulator.py @@ -1,7 +1,9 @@ import os +import rospkg +import subprocess +import yaml - -class BaseEnvironment: +class BaseSimulator: def __init__(self, namespace): self._namespace = namespace self._ns_prefix = lambda *topic: os.path.join(self._namespace, *topic) @@ -22,7 +24,7 @@ def after_reset_task(self): def remove_all_obstacles(self): """ - Removes all obstacles from the current environment. Does not remove + Removes all obstacles from the current simulator. Does not remove the robot! """ raise NotImplementedError() @@ -40,7 +42,7 @@ def spawn_random_dynamic_obstacle(self, **args): """ raise NotImplementedError() - def spawn_random_static_obstacles(self, **args): + def spawn_random_static_obstacle(self, **args): """ Spawn a single random static obstacle. @@ -65,7 +67,7 @@ def move_robot(self, pos, name=None): def spawn_robot(self): """ - Spawn a robot in the environment. + Spawn a robot in the simulator. A position is not specified because the robot is moved at the desired position anyway. """ @@ -81,4 +83,28 @@ def reset_pedsim_agents(self): raise NotImplementedError() def spawn_obstacle(self, position, yaml_path=""): - raise NotImplementedError() \ No newline at end of file + raise NotImplementedError() + + @staticmethod + def get_robot_description(robot_name, namespace): + arena_sim_path = rospkg.RosPack().get_path("arena-simulation-setup") + + return subprocess.check_output([ + "rosrun", + "xacro", + "xacro", + os.path.join(arena_sim_path, "robot", robot_name, "urdf", f"{robot_name}.urdf.xacro"), + f"robot_namespace:={namespace}" + ]).decode("utf-8") + + @staticmethod + def read_robot_parameters(robot_name): + robot_param_path = os.path.join( + rospkg.RosPack().get_path("arena-simulation-setup"), + "robot", + robot_name, + "model_params.yaml" + ) + + with open(robot_param_path, "r") as file: + return yaml.safe_load(file) \ No newline at end of file diff --git a/task_generator/environments/flatland_environment.py b/task_generator/simulators/flatland_simulator.py similarity index 87% rename from task_generator/environments/flatland_environment.py rename to task_generator/simulators/flatland_simulator.py index 2b5058d..d4d29d0 100644 --- a/task_generator/environments/flatland_environment.py +++ b/task_generator/simulators/flatland_simulator.py @@ -21,20 +21,20 @@ from task_generator.utils import Utils from ..constants import Constants, FlatlandRandomModel -from .base_environment import BaseEnvironment -from .environment_factory import EnvironmentFactory +from .base_simulator import BaseSimulator +from .simulator_factory import SimulatorFactory T = Constants.WAIT_FOR_SERVICE_TIMEOUT -@EnvironmentFactory.register("flatland") -class FlatlandEnvironment(BaseEnvironment): +@SimulatorFactory.register("flatland") +class FlatlandSimulator(BaseSimulator): """ This is the flatland encoder for connecting flatland with the arena-benchmark task generator. The class implements all methods - defined in `BaseEnvironment`. + defined in `BaseSimulator`. For flatland to work properly, a dedicated .yaml file has to be created for each used model. This @@ -56,18 +56,14 @@ def __init__(self, namespace): self._namespace = namespace self._ns_prefix = "" if namespace == "" else "/" + namespace + "/" - self._goal_pub = rospy.Publisher( - f"{self._ns_prefix}goal", PoseStamped, queue_size=1, latch=True - ) - self._move_robot_pub = rospy.Publisher( self._ns_prefix + "move_model", MoveModelMsg, queue_size=10 ) - self._robot_name = rospy.get_param("robot_model") - self._robot_radius = rospy.get_param("robot_radius") - self._is_training_mode = rospy.get_param("train_mode") - self._step_size = rospy.get_param("step_size") + self._robot_name = rospy.get_param("robot_model", "") + self._robot_radius = rospy.get_param("robot_radius", "") + self._is_training_mode = rospy.get_param("train_mode", "") + self._step_size = rospy.get_param("step_size", "") # self._robot_yaml_path = rospy.get_param("robot_yaml_path") self._tmp_model_path = rospy.get_param("tmp_model_path", "/tmp") @@ -105,7 +101,7 @@ def after_reset_task(self): def remove_all_obstacles(self): for obs in range(self._obstacles_amount): - obs_name = FlatlandEnvironment.create_obs_name(obs) + obs_name = FlatlandSimulator.create_obs_name(obs) self._delete_model(obs_name) @@ -134,7 +130,7 @@ def reset_pedsim_agents(self): pass def spawn_obstacle(self, position, yaml_path=""): - name = FlatlandEnvironment.create_obs_name(self._obstacles_amount) + name = FlatlandSimulator.create_obs_name(self._obstacles_amount) self._spawn_model(yaml_path, name, self._namespace, position) @@ -151,7 +147,7 @@ def _spawn_random_obstacle( ): model = self._generate_random_obstacle(is_dynamic=is_dynamic, **args) - obstacle_name = FlatlandEnvironment.create_obs_name( + obstacle_name = FlatlandSimulator.create_obs_name( self._obstacles_amount ) @@ -204,21 +200,6 @@ def _spawn_model(self, yaml_path, name, namespace, position, srv=None): srv(request) - def publish_goal(self, goal): - goal_msg = PoseStamped() - goal_msg.header.seq = 0 - goal_msg.header.stamp = rospy.get_rostime() - goal_msg.header.frame_id = "map" - goal_msg.pose.position.x = goal[0] - goal_msg.pose.position.y = goal[1] - - goal_msg.pose.orientation.w = 0 - goal_msg.pose.orientation.x = 0 - goal_msg.pose.orientation.y = 0 - goal_msg.pose.orientation.z = 1 - - self._goal_pub.publish(goal_msg) - def move_robot(self, pos, name=None): pose = Pose2D() pose.x = pos[0] @@ -338,8 +319,8 @@ def _update_plugin_topics(self, file_content, namespace): plugins = file_content["plugins"] for plugin in plugins: - if FlatlandEnvironment.PLUGIN_PROPS_TO_EXTEND.get(plugin["type"]): - prop_names = FlatlandEnvironment.PLUGIN_PROPS_TO_EXTEND.get(plugin["type"]) + if FlatlandSimulator.PLUGIN_PROPS_TO_EXTEND.get(plugin["type"]): + prop_names = FlatlandSimulator.PLUGIN_PROPS_TO_EXTEND.get(plugin["type"]) for name in prop_names: plugin[name] = os.path.join(namespace, plugin[name]) diff --git a/task_generator/environments/gazebo_environment.py b/task_generator/simulators/gazebo_simulator.py similarity index 95% rename from task_generator/environments/gazebo_environment.py rename to task_generator/simulators/gazebo_simulator.py index 9720c6c..d64b154 100644 --- a/task_generator/environments/gazebo_environment.py +++ b/task_generator/simulators/gazebo_simulator.py @@ -8,18 +8,17 @@ from pedsim_srvs.srv import SpawnPeds from std_msgs.msg import Empty from std_srvs.srv import Empty, SetBool, Trigger -from task_generator.environments.environment_factory import EnvironmentFactory from tf.transformations import quaternion_from_euler from ..constants import Constants -from .base_environment import BaseEnvironment -from .environment_factory import EnvironmentFactory +from .base_simulator import BaseSimulator +from .simulator_factory import SimulatorFactory T = Constants.WAIT_FOR_SERVICE_TIMEOUT -@EnvironmentFactory.register("gazebo") -class GazeboEnvironment(BaseEnvironment): +@SimulatorFactory.register("gazebo") +class GazeboSimulator(BaseSimulator): def __init__(self, namespace): super().__init__(namespace) @@ -125,7 +124,7 @@ def spawn_robot(self, name, robot_name, namespace_appendix=""): robot_namespace = self._ns_prefix(namespace_appendix) - robot_description = GazeboEnvironment.get_robot_description( + robot_description = GazeboSimulator.get_robot_description( robot_name, robot_namespace ) rospy.set_param(os.path.join(robot_namespace, "robot_description"), robot_description) diff --git a/task_generator/simulators/simulator_factory.py b/task_generator/simulators/simulator_factory.py new file mode 100644 index 0000000..8425999 --- /dev/null +++ b/task_generator/simulators/simulator_factory.py @@ -0,0 +1,23 @@ +from .base_simulator import BaseSimulator + +class SimulatorFactory: + registry = {} + + @classmethod + def register(cls, name): + def inner_wrapper(wrapped_class): + assert name not in cls.registry, f"Simulator '{name}' already exists!" + assert issubclass(wrapped_class, BaseSimulator) + + cls.registry[name] = wrapped_class + return wrapped_class + + return inner_wrapper + + @classmethod + def instantiate(cls, name: str): + assert name in cls.registry, f"Simulator '{name}' is not registered!" + + simulator = cls.registry[name] + + return simulator diff --git a/task_generator/simulators/unity_simulator.py b/task_generator/simulators/unity_simulator.py new file mode 100644 index 0000000..8efb0d3 --- /dev/null +++ b/task_generator/simulators/unity_simulator.py @@ -0,0 +1,181 @@ +import rospy +import os + +from tf.transformations import quaternion_from_euler +from geometry_msgs.msg import Pose, Quaternion + +from .base_simulator import BaseSimulator +from .simulator_factory import SimulatorFactory +from ..constants import UnitySimulatorConstants + +from unity_msgs.srv import SpawnRobot, SpawnRobotRequest, MoveModel, MoveModelRequest + +@SimulatorFactory.register("unity") +class UnitySimulator(BaseSimulator): + def __init__(self, namespace): + + print("STARTING UP") + + rospy.wait_for_service("/unity/spawn_robot") + rospy.wait_for_service("/unity/move_model") + + self._spawn_robot_srv = rospy.ServiceProxy("/unity/spawn_robot", SpawnRobot) + self._move_model_srv = rospy.ServiceProxy("/unity/move_model", MoveModel) + + self.map_manager = None + pass + + def before_reset_task(self): + """ + Is executed each time before the task is reseted. This is useful in + order to pause the simulation. + """ + pass + + def after_reset_task(self): + """ + Is executed after the task is reseted. This is useful to unpause the + simulation. + """ + pass + + def remove_all_obstacles(self): + """ + Removes all obstacles from the current simulator. Does not remove + the robot! + """ + pass + + def spawn_random_dynamic_obstacle(self, **args): + """ + Spawn a single random dynamic obstacle. + + Args: + position: [int, int, int] denoting the x, y and angle. + min_radius: minimal radius of the obstacle + max_radius: maximal radius of the obstacle + linear_vel: linear velocity + angular_vel_max: maximal angular velocity + """ + pass + + def spawn_random_static_obstacle(self, **args): + """ + Spawn a single random static obstacle. + + Args: + position: [int, int, int] denoting the x, y and angle. + min_radius: minimal radius of the obstacle + max_radius: maximal radius of the obstacle + """ + pass + + def publish_goal(self, goal): + """ + Publishes the goal. + """ + pass + + def move_robot(self, pos, name=None): + """ + Move the robot to the given position. + """ + + request = MoveModelRequest() + + request.model_name = name + + pose = Pose() + pose.position.x = pos[0] + pose.position.z = pos[1] + + print("MOVE MODEL TO ", pos) + + pose.orientation = Quaternion( + *quaternion_from_euler(0.0, pos[2], 0.0, axes="sxyz") + ) + request.pose = pose + request.reference_frame = "world" + + self._move_model_srv(request) + + def spawn_robot(self, name, robot_name, namespace_appendix=""): + """ + Spawn a robot in the simulator. + A position is not specified because the robot is moved at the + desired position anyway. + """ + print("SPAWN ROBOT CALLED") + + robot_description = UnitySimulator.get_robot_description(robot_name, namespace_appendix) + + robot_urdf_file_path = os.path.join( + os.environ[UnitySimulatorConstants.UNITY_ROS_NAVIGATION], + f"{robot_name}.urdf" + ) + + robot_parameters = UnitySimulator.read_robot_parameters(robot_name) + + UnitySimulator.write_urdf_file_to_unity_dir( + robot_urdf_file_path, + robot_description + ) + + request = SpawnRobotRequest() + + request.model_name = name + request.model_urdf_path = robot_urdf_file_path + request.model_namespace = namespace_appendix + request.reference_frame = "world" + + linear_range = UnitySimulator.get_robot_linear_range( + robot_parameters["actions"]["continuous"]["linear_range"] + ) + + request.additional_data = [ + robot_parameters["laser"]["angle"]["min"], + robot_parameters["laser"]["angle"]["max"], + robot_parameters["laser"]["angle"]["increment"], + robot_parameters["laser"]["range"], + robot_parameters["laser"]["num_beams"], + + *linear_range, + *robot_parameters["actions"]["continuous"]["angular_range"] + ] + + self._spawn_robot_srv(request) + + UnitySimulator.delete_robot_file_in_unity_dir(robot_urdf_file_path) + + def spawn_pedsim_agents(self, agents): + """ + + """ + pass + + def reset_pedsim_agents(self): + pass + + def spawn_obstacle(self, position, yaml_path=""): + pass + + @staticmethod + def get_robot_linear_range(linear_range): + if isinstance(linear_range, dict): + return [*linear_range["x"], *linear_range["y"]] + + return [*linear_range, 0, 0] + + @staticmethod + def write_urdf_file_to_unity_dir(robot_file_path, robot_urdf): + with open(robot_file_path, "w") as file: + file.write(robot_urdf) + file.close() + + @staticmethod + def delete_robot_file_in_unity_dir(robot_file_path): + try: + os.remove(robot_file_path) + except: + rospy.logwarn("World file could not be deleted") + diff --git a/task_generator/tasks/scenario.py b/task_generator/tasks/scenario.py index c475c7e..e73a0be 100644 --- a/task_generator/tasks/scenario.py +++ b/task_generator/tasks/scenario.py @@ -104,4 +104,4 @@ def _check_robot_manager_length(self): if scenario_robots_length > setup_robot_length: self.scenario_file["robots"] = self.scenario_file["robots"][:setup_robot_length] - rospy.logwarn("Scenario file contains more robots that setup.") \ No newline at end of file + rospy.logwarn("Scenario file contains more robots than setup.") \ No newline at end of file diff --git a/task_generator/tasks/staged.py b/task_generator/tasks/staged.py index 86b22b5..287e0b3 100644 --- a/task_generator/tasks/staged.py +++ b/task_generator/tasks/staged.py @@ -48,8 +48,6 @@ def __init__( self._init_debug_mode(paths) - print("NAMESPACE PREFIX", self.namespace_prefix) - self._sub_next = rospy.Subscriber( f"{self.namespace_prefix}next_stage", Bool, self.next_stage ) @@ -141,10 +139,6 @@ def _reset_robot_and_obstacles( **kwargs ) - def _set_up_robot_managers(self): - for manager in self.robot_managers: - manager.set_up_robot(launch_robot_controller=False) - def _init_stage(self, stage): static_obstacles = self._stages[stage]["static"] dynamic_obstacles = self._stages[stage]["dynamic"] diff --git a/task_generator/tasks/task_factory.py b/task_generator/tasks/task_factory.py index 8e4aa30..e5b4ba2 100644 --- a/task_generator/tasks/task_factory.py +++ b/task_generator/tasks/task_factory.py @@ -6,7 +6,7 @@ class TaskFactory: @classmethod def register(cls, name): def inner_wrapper(wrapped_class): - assert name not in cls.registry, f"Environment '{name}' already exists!" + assert name not in cls.registry, f"Simulator '{name}' already exists!" assert issubclass(wrapped_class, BaseTask) cls.registry[name] = wrapped_class @@ -16,10 +16,10 @@ def inner_wrapper(wrapped_class): @classmethod def instantiate(cls, name: str, *args, **kwargs): - assert name in cls.registry, f"Environment '{name}' is not registered!" - environment = cls.registry[name] + assert name in cls.registry, f"Simulator '{name}' is not registered!" + simulator = cls.registry[name] - if issubclass(environment, BaseTask): - return environment(*args, **kwargs) + if issubclass(simulator, BaseTask): + return simulator(*args, **kwargs) else: - return environment + return simulator diff --git a/task_generator/tasks/utils.py b/task_generator/tasks/utils.py index fad4dd0..6975a35 100644 --- a/task_generator/tasks/utils.py +++ b/task_generator/tasks/utils.py @@ -5,9 +5,10 @@ import os from task_generator.constants import Constants -from task_generator.environments.environment_factory import EnvironmentFactory -from task_generator.environments.gazebo_environment import GazeboEnvironment -from task_generator.environments.flatland_environment import FlatlandRandomModel +from task_generator.simulators.simulator_factory import SimulatorFactory +from task_generator.simulators.gazebo_simulator import GazeboSimulator +from task_generator.simulators.flatland_simulator import FlatlandSimulator +from task_generator.simulators.unity_simulator import UnitySimulator from task_generator.manager.map_manager import MapManager from task_generator.manager.obstacle_manager import ObstacleManager from task_generator.manager.robot_manager import RobotManager @@ -20,12 +21,12 @@ from map_distance_server.srv import GetDistanceMap -def get_predefined_task(namespace, mode, environment=None, **kwargs): +def get_predefined_task(namespace, mode, simulator=None, **kwargs): """ Gets the task based on the passed mode """ - if environment == None: - environment = EnvironmentFactory.instantiate(Utils.get_environment())(namespace) + if simulator == None: + simulator = SimulatorFactory.instantiate(Utils.get_simulator())(namespace) rospy.wait_for_service("/distance_map") @@ -35,11 +36,11 @@ def get_predefined_task(namespace, mode, environment=None, **kwargs): map_manager = MapManager(map_response) - environment.map_manager = map_manager + simulator.map_manager = map_manager - obstacle_manager = ObstacleManager(namespace, map_manager, environment) + obstacle_manager = ObstacleManager(namespace, map_manager, simulator) - robot_managers = create_robot_managers(namespace, map_manager, environment) + robot_managers = create_robot_managers(namespace, map_manager, simulator) # For every robot # - Create a unique namespace name @@ -57,7 +58,7 @@ def get_predefined_task(namespace, mode, environment=None, **kwargs): return task -def create_robot_managers(namespace, map_manager, environment): +def create_robot_managers(namespace, map_manager, simulator): # Read robot setup file robot_setup_file = rospy.get_param('/robot_setup_file', "") @@ -71,7 +72,7 @@ def create_robot_managers(namespace, map_manager, environment): robots = read_robot_setup_file(robot_setup_file) if Utils.get_arena_type() == Constants.ArenaType.TRAINING: - return [RobotManager(namespace, map_manager, environment, robots[0])] + return [RobotManager(namespace, map_manager, simulator, robots[0])] robot_managers = [] @@ -82,7 +83,7 @@ def create_robot_managers(namespace, map_manager, environment): name = f"{robot['model']}_{r}_{len(robot_managers)}" robot_managers.append( - RobotManager(namespace + "/" + name, map_manager, environment, robot) + RobotManager(namespace + "/" + name, map_manager, simulator, robot) ) return robot_managers diff --git a/task_generator/utils.py b/task_generator/utils.py index bd621af..8e4ff43 100644 --- a/task_generator/utils.py +++ b/task_generator/utils.py @@ -3,8 +3,8 @@ class Utils: - def get_environment(): - return rospy.get_param("environment", "flatland").lower() + def get_simulator(): + return rospy.get_param("simulator", "flatland").lower() def get_arena_type(): return os.getenv("ARENA_TYPE", "training").lower()