diff --git a/.rosinstall b/.rosinstall
index 22e7e282..4e26fc65 100755
--- a/.rosinstall
+++ b/.rosinstall
@@ -28,6 +28,10 @@
local-name: ../utils/3rd-party-robot-packages
uri: https://github.com/Arena-Rosnav/3rd-party-robot-packages
+- git:
+ local-name: ../utils/unity-ros-navigation
+ uri: https://github.com/Arena-Rosnav/unity-ros-navigation
+
### ARENA INFRASTRUCTURE
- git:
diff --git a/README.md b/README.md
index 05128f3c..7e1bfa3f 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,6 @@
+# For Unity Integration
+unity-ros package needs to be installed. Thus, run rosws update and compile with catkin_make.
+
## Simple Installation
Clone repo in any catkin ws or create new catkin ws
diff --git a/arena_bringup/launch/start_arena.launch b/arena_bringup/launch/start_arena.launch
index f94f79f6..e1b86c03 100755
--- a/arena_bringup/launch/start_arena.launch
+++ b/arena_bringup/launch/start_arena.launch
@@ -32,7 +32,7 @@
-
+
@@ -124,6 +124,13 @@
+
+
+
+
+
+
+
diff --git a/arena_bringup/launch/testing/robot.launch b/arena_bringup/launch/testing/robot.launch
index 76faaaa4..7f286b1a 100644
--- a/arena_bringup/launch/testing/robot.launch
+++ b/arena_bringup/launch/testing/robot.launch
@@ -46,7 +46,7 @@
-
+
diff --git a/arena_bringup/launch/testing/simulators/unity.launch b/arena_bringup/launch/testing/simulators/unity.launch
new file mode 100644
index 00000000..f418be62
--- /dev/null
+++ b/arena_bringup/launch/testing/simulators/unity.launch
@@ -0,0 +1,36 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/task_generator/task_generator/constants.py b/task_generator/task_generator/constants.py
index 141d1bac..b8db9b5d 100644
--- a/task_generator/task_generator/constants.py
+++ b/task_generator/task_generator/constants.py
@@ -38,6 +38,7 @@ class Random:
class Scenario:
RESETS_DEFAULT = 5
+
class FlatlandRandomModel:
BODY = {
"name": "base_link",
@@ -62,6 +63,7 @@ class FlatlandRandomModel:
LINEAR_VEL = 0.2
ANGLUAR_VEL_MAX = 0.2
+
class Pedsim:
START_UP_MODE = "default"
WAIT_TIME = 0.0
@@ -85,4 +87,8 @@ 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"
\ No newline at end of file
diff --git a/task_generator/task_generator/simulators/base_simulator.py b/task_generator/task_generator/simulators/base_simulator.py
index d1f25fd9..56956381 100644
--- a/task_generator/task_generator/simulators/base_simulator.py
+++ b/task_generator/task_generator/simulators/base_simulator.py
@@ -1,4 +1,7 @@
import os
+import rospkg
+import subprocess
+import yaml
class BaseSimulator:
@@ -81,4 +84,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/task_generator/simulators/unity_simulator.py b/task_generator/task_generator/simulators/unity_simulator.py
new file mode 100644
index 00000000..0f5e6b77
--- /dev/null
+++ b/task_generator/task_generator/simulators/unity_simulator.py
@@ -0,0 +1,180 @@
+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")
\ No newline at end of file
diff --git a/task_generator/task_generator/tasks/utils.py b/task_generator/task_generator/tasks/utils.py
index fe4b58eb..30bf008b 100644
--- a/task_generator/task_generator/tasks/utils.py
+++ b/task_generator/task_generator/tasks/utils.py
@@ -7,6 +7,7 @@
from task_generator.simulators.simulator_factory import SimulatorFactory
from task_generator.simulators.gazebo_simulator import GazeboSimulator
+from task_generator.simulators.unity_simulator import UnitySimulator
from task_generator.simulators.flatland_simulator import FlatlandRandomModel
from task_generator.manager.map_manager import MapManager
from task_generator.manager.obstacle_manager import ObstacleManager