Unity integration

.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:

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

arena_bringup/launch/start_arena.launch

@@ -32,7 +32,7 @@
   <!-- You can launch a single robot and his local_planner with arguments -->
   <arg name="model" default="burger" doc="robot model type [burger, jackal, ridgeback, agvota, rto, ...]" />
   <arg name="local_planner" default="teb" doc="local planner type [teb, dwa, mpc, rlca, arena, rosnav]" />
-  <arg name="simulator" default="flatland" doc="[flatland, gazebo]" />
+  <arg name="simulator" default="flatland" doc="[flatland, gazebo, unity]" />
 
   <arg name="complexity" default="1" doc="1 = Map known, Position known; 2 = Map known, Position unknown (AMCL); 3 = Map unknown, Position unknown (SLAM)" />
 
@@ -124,6 +124,13 @@
     <arg name="world" default="$(arg world_file)" />
   </include>
 
+  <!-- Unity -->
+  <include file="$(find arena_bringup)/launch/testing/simulators/unity.launch" if="$(eval arg('simulator') == 'unity')" >
+    <arg name="world" default="$(arg world_file)" />
+
+
+  </include>
+
   <!-- map server-->
   <include file="$(find arena_bringup)/launch/utils/map_server.launch">
     <arg name="map_path" value="$(arg map_path)" />

arena_bringup/launch/testing/robot.launch

@@ -46,7 +46,7 @@
             <remap from="/joint_states" to="$(arg namespace)/joint_states"/>
         </node>
     </group>
-  
+
 
     <!-- move_base plan manager: which provide basic global planner and cost map -->
     <include file="$(find arena_bringup)/launch/testing/move_base/move_base_$(arg local_planner).launch">

arena_bringup/launch/testing/simulators/unity.launch

@@ -0,0 +1,36 @@
+<launch>
+    <arg name="world" />
+
+    <arg name="tcp_ip" default="0.0.0.0"/>
+    <arg name="tcp_port" default="10000"/>
+
+    <arg 
+        name="world_file_path" 
+        default="$(find arena-simulation-setup)/worlds/$(arg world)/worlds/$(arg world).world" 
+    />
+
+    <param name="world_file_path" value="$(arg world_file_path)" />
+
+    <!-- <param name="robot_description" command="xacro $(find arena-simulation-setup)/robot/jackal/urdf/jackal.urdf.xacro" /> -->
+
+    <include file="$(find ros_tcp_endpoint)/launch/endpoint.launch">
+        <arg name="tcp_ip" value="$(arg tcp_ip)" />
+        <arg name="tcp_port" value="$(arg tcp_port)" />
+    </include>
+
+    <param 
+        name="world_urdf" 
+        command="rosrun pysdf world2urdf.py $(arg world_file_path) $(arg world)" 
+    />
+
+    <node 
+        pkg="unity_utils" 
+        name="load_world_service" 
+        type="load_world_service.py" 
+        output="screen"
+    />
+
+    <include file="$(find arena_bringup)/launch/utils/rviz.launch">
+        <arg name="show_rviz" value="true" />
+    </include>
+</launch>

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
+    WAYPOINT_MODE = 0
+
+
+class UnitySimulatorConstants:
+    UNITY_ROS_NAVIGATION = "UNITY_ROS_NAVIGATION"

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()
+        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)

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")

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