From b9c2643ae536eb67b122980941f505277345996b Mon Sep 17 00:00:00 2001 From: MaximilianSchreff Date: Tue, 19 Dec 2023 13:29:20 +0100 Subject: [PATCH 1/5] Changed coordinates to normal FLU format --- .../simulators/unity_simulator.py | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/task_generator/task_generator/simulators/unity_simulator.py b/task_generator/task_generator/simulators/unity_simulator.py index de1cf1649..5d05dd2df 100644 --- a/task_generator/task_generator/simulators/unity_simulator.py +++ b/task_generator/task_generator/simulators/unity_simulator.py @@ -75,11 +75,10 @@ def spawn_entity(self, entity): request.initial_pose = Pose( position=Point( x=entity.position[0], - y=0, - z=entity.position[1] + y=entity.position[1], + z=0.35 ), - orientation=Quaternion(*quaternion_from_euler(0.0, entity.position[2], 0.0, axes="sxyz") - ) + orientation=Quaternion(*quaternion_from_euler(0.0, 0.0, entity.position[2], axes="sxyz")) ) rospy.set_param(request.robot_namespace( @@ -97,13 +96,15 @@ def move_entity(self, name, pos): request.model_state = ModelState() request.model_state.model_name = name - pose = Pose() - # Keep in mind that y axis is up - pose.position.x = pos[0] - pose.position.y = 0.35 - pose.position.z = pos[1] - pose.orientation = Quaternion( - *quaternion_from_euler(0.0, pos[2], 0.0, axes="sxyz") + pose = Pose( + position=Point( + x = pos[0], + y = pos[1], + z = 0.35 + ), + orientation=Quaternion( + *quaternion_from_euler(0.0, 0.0, pos[2], axes="sxyz") + ) ) request.model_state.pose = pose request.model_state.reference_frame = "world" From 4d76ea41233ea25e77dde89c6b8362e6bd10ed1f Mon Sep 17 00:00:00 2001 From: MaximilianSchreff Date: Tue, 19 Dec 2023 16:54:36 +0100 Subject: [PATCH 2/5] Mistake when merging --- .../simulators/gazebo_simulator.py | 109 +++++++++--------- 1 file changed, 53 insertions(+), 56 deletions(-) diff --git a/task_generator/task_generator/simulators/gazebo_simulator.py b/task_generator/task_generator/simulators/gazebo_simulator.py index a42c68a00..c2a179ee1 100644 --- a/task_generator/task_generator/simulators/gazebo_simulator.py +++ b/task_generator/task_generator/simulators/gazebo_simulator.py @@ -1,12 +1,11 @@ import rospy -from gazebo_msgs.msg import ModelState -from gazebo_msgs.srv import SetModelState, SetModelStateRequest, DeleteModel, SpawnModel, SpawnModelRequest, DeleteModelRequest, DeleteModelResponse +import gazebo_msgs.msg as gazebo_msgs +import gazebo_msgs.srv as gazebo_srvs -from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +import geometry_msgs.msg as geometry_msgs -from std_msgs.msg import Empty -from std_srvs.srv import Empty +import std_srvs.srv as std_srvs from task_generator.simulators.simulator_factory import SimulatorFactory @@ -14,8 +13,9 @@ from tf.transformations import quaternion_from_euler from task_generator.constants import Constants from task_generator.simulators.base_simulator import BaseSimulator +from task_generator.simulators.simulator_factory import SimulatorFactory -from task_generator.shared import ModelType, RobotProps +from task_generator.shared import ModelType, PositionOrientation, RobotProps T = Constants.WAIT_FOR_SERVICE_TIMEOUT @@ -36,40 +36,40 @@ def __init__(self, namespace): super().__init__(namespace) self._goal_pub = rospy.Publisher( self._namespace("/goal"), - PoseStamped, + geometry_msgs.PoseStamped, queue_size=1, latch=True ) self._robot_name = rosparam_get(str, "robot_model", "") - rospy.loginfo("Waiting for gazebo services...") - rospy.wait_for_service(self._namespace( - "gazebo", "spawn_urdf_model"), timeout=T) - rospy.wait_for_service(self._namespace( - "gazebo", "spawn_sdf_model"), timeout=T) - rospy.wait_for_service(self._namespace( - "gazebo", "set_model_state"), timeout=20) - rospy.wait_for_service(self._namespace( - "gazebo", "delete_model"), timeout=T) + rospy.wait_for_service("/gazebo/spawn_urdf_model") + rospy.wait_for_service("/gazebo/spawn_sdf_model") + rospy.wait_for_service("/gazebo/set_model_state") + rospy.wait_for_service("/gazebo/set_model_state", timeout=20) self._spawn_model[ModelType.URDF] = rospy.ServiceProxy( - self._namespace("gazebo", "spawn_urdf_model"), SpawnModel + self._namespace( + "gazebo", "spawn_urdf_model"), gazebo_srvs.SpawnModel ) self._spawn_model[ModelType.SDF] = rospy.ServiceProxy( - self._namespace("gazebo", "spawn_sdf_model"), SpawnModel + self._namespace( + "gazebo", "spawn_sdf_model"), gazebo_srvs.SpawnModel ) self._move_model_srv = rospy.ServiceProxy( - self._namespace("gazebo", "set_model_state"), SetModelState, persistent=True + "/gazebo/set_model_state", gazebo_srvs.SetModelState, persistent=True ) self._unpause = rospy.ServiceProxy( - self._namespace("gazebo", "unpause_physics"), Empty - ) + "/gazebo/unpause_physics", std_srvs.Empty) self._pause = rospy.ServiceProxy( - self._namespace("gazebo", "pause_physics"), Empty - ) + "/gazebo/pause_physics", std_srvs.Empty) + + rospy.loginfo("Waiting for gazebo services...") + rospy.wait_for_service("gazebo/spawn_sdf_model") + rospy.wait_for_service("gazebo/delete_model") + + rospy.loginfo("service: spawn_sdf_model is available ....") self._remove_model_srv = rospy.ServiceProxy( - self._namespace("gazebo", "delete_model"), DeleteModel - ) + "gazebo/delete_model", gazebo_srvs.DeleteModel) def before_reset_task(self): self._pause() @@ -77,44 +77,45 @@ def before_reset_task(self): def after_reset_task(self): try: self._unpause() - except rospy.service.ServiceException as e: # gazebo isn't the most reliable + except rospy.service.ServiceException as e: # gazebo isn't the most reliable rospy.logwarn(e) # ROBOT - def move_entity(self, name, pos): + def move_entity(self, name, position): - request = SetModelStateRequest() - request.model_state = ModelState() + request = gazebo_srvs.SetModelStateRequest() + request.model_state = gazebo_msgs.ModelState() request.model_state.model_name = name - pose = Pose() - pose.position.x = pos[0] - pose.position.y = pos[1] - pose.position.z = 0.35 - pose.orientation = Quaternion( - *quaternion_from_euler(0.0, 0.0, pos[2], axes="sxyz") + pose = geometry_msgs.Pose() + pose.position.x = position.x + pose.position.y = position.y + pose.position.z = 0 + pose.orientation = geometry_msgs.Quaternion( + *quaternion_from_euler(0.0, 0.0, position.orientation, axes="sxyz") ) request.model_state.pose = pose request.model_state.reference_frame = "world" - self._move_model_srv(request) + return bool(self._move_model_srv(request).success) def spawn_entity(self, entity): - request = SpawnModelRequest() + request = gazebo_srvs.SpawnModelRequest() model = entity.model.get(self.MODEL_TYPES) request.model_name = entity.name request.model_xml = model.description - request.initial_pose = Pose( - position=Point( - x=entity.position[0], - y=entity.position[1], + request.initial_pose = geometry_msgs.Pose( + position=geometry_msgs.Point( + x=entity.position.x, + y=entity.position.y, z=0 ), - orientation=Quaternion(*quaternion_from_euler(0.0, 0.0, entity.position[2], axes="sxyz") - ) + orientation=geometry_msgs.Quaternion( + *quaternion_from_euler(0.0, 0.0, entity.position.orientation, axes="sxyz") + ) ) request.robot_namespace = self._namespace(entity.name) request.reference_frame = "world" @@ -126,25 +127,21 @@ def spawn_entity(self, entity): "tf_prefix"), str(request.robot_namespace)) res = self.spawn_model(model.type, request) - - return res.success + return bool(res.success) def delete_entity(self, name): - res: DeleteModelResponse = self._remove_model_srv( - DeleteModelRequest(model_name=name)) + res: gazebo_srvs.DeleteModelResponse = self._remove_model_srv( + gazebo_srvs.DeleteModelRequest(model_name=name)) return bool(res.success) - def _publish_goal(self, goal): - goal_msg = PoseStamped() + def _publish_goal(self, goal: PositionOrientation): + goal_msg = geometry_msgs.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 + goal_msg.pose.position.x = goal.x + goal_msg.pose.position.y = goal.y + goal_msg.pose.orientation = geometry_msgs.Quaternion( + *quaternion_from_euler(0.0, 0.0, goal.orientation, axes="sxyz")) self._goal_pub.publish(goal_msg) From b53082992e06e7b574e44437da0a541e47eb975b Mon Sep 17 00:00:00 2001 From: MaximilianSchreff Date: Tue, 19 Dec 2023 17:27:51 +0100 Subject: [PATCH 3/5] Fixed unity simulator --- .../task_generator/simulators/unity_simulator.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/task_generator/task_generator/simulators/unity_simulator.py b/task_generator/task_generator/simulators/unity_simulator.py index 5d05dd2df..70fe2f42d 100644 --- a/task_generator/task_generator/simulators/unity_simulator.py +++ b/task_generator/task_generator/simulators/unity_simulator.py @@ -89,7 +89,7 @@ def spawn_entity(self, entity): res = self.spawn_model(model.type, request) return res.success - def move_entity(self, name, pos): + def move_entity(self, name, position): rospy.loginfo("[Unity Simulator] Move Request for " + name) request = SetModelStateRequest() @@ -98,12 +98,12 @@ def move_entity(self, name, pos): request.model_state.model_name = name pose = Pose( position=Point( - x = pos[0], - y = pos[1], + x = position[0], + y = position[1], z = 0.35 ), orientation=Quaternion( - *quaternion_from_euler(0.0, 0.0, pos[2], axes="sxyz") + *quaternion_from_euler(0.0, 0.0, position[2], axes="sxyz") ) ) request.model_state.pose = pose From 278dc09924e2ba6ac78439391061a7694b315603 Mon Sep 17 00:00:00 2001 From: MaximilianSchreff Date: Wed, 20 Dec 2023 11:56:32 +0100 Subject: [PATCH 4/5] Fixed bug with peds --- .../manager/entity_manager/pedsim_manager.py | 3 ++- .../task_generator/simulators/unity_simulator.py | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/task_generator/task_generator/manager/entity_manager/pedsim_manager.py b/task_generator/task_generator/manager/entity_manager/pedsim_manager.py index 05f7a70cb..147ea669d 100644 --- a/task_generator/task_generator/manager/entity_manager/pedsim_manager.py +++ b/task_generator/task_generator/manager/entity_manager/pedsim_manager.py @@ -35,6 +35,7 @@ from typing import Callable, List from task_generator.simulators.gazebo_simulator import GazeboSimulator +from task_generator.simulators.unity_simulator import UnitySimulator from task_generator.utils import rosparam_get from tf.transformations import quaternion_from_euler, euler_from_quaternion @@ -528,7 +529,7 @@ def remove_obstacles(self, purge): for obstacle_id, obstacle in self._known_obstacles.items(): if purge >= obstacle.layer: - if isinstance(self._simulator, GazeboSimulator): + if isinstance(self._simulator, GazeboSimulator) or isinstance(self._simulator, UnitySimulator): # TODO remove this once actors can be deleted properly if isinstance(obstacle.obstacle, DynamicObstacle): diff --git a/task_generator/task_generator/simulators/unity_simulator.py b/task_generator/task_generator/simulators/unity_simulator.py index 70fe2f42d..c704f6012 100644 --- a/task_generator/task_generator/simulators/unity_simulator.py +++ b/task_generator/task_generator/simulators/unity_simulator.py @@ -81,10 +81,11 @@ def spawn_entity(self, entity): orientation=Quaternion(*quaternion_from_euler(0.0, 0.0, entity.position[2], axes="sxyz")) ) - rospy.set_param(request.robot_namespace( - "robot_description"), model.description) - rospy.set_param(request.robot_namespace( - "tf_prefix"), str(request.robot_namespace)) + if isinstance(entity, Robot): + rospy.set_param(request.robot_namespace( + "robot_description"), model.description) + rospy.set_param(request.robot_namespace( + "tf_prefix"), str(request.robot_namespace)) res = self.spawn_model(model.type, request) return res.success From bbc5e20e847ab4ea312abc70d24d77b290fcf706 Mon Sep 17 00:00:00 2001 From: Jan Golebiowski Date: Fri, 22 Dec 2023 14:21:13 +0100 Subject: [PATCH 5/5] Update .rosinstall for arena-unity --- .rosinstall | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.rosinstall b/.rosinstall index 8a7e61c01..c21197a6d 100755 --- a/.rosinstall +++ b/.rosinstall @@ -6,6 +6,11 @@ uri: https://github.com/Arena-Rosnav/flatland.git version: master +- git: + local-name: ../arena-unity + uri: https://github.com/Arena-Rosnav/arena-unity.git + version: main + # - git: # local-name: ../utils/arena-tools # uri: https://github.com/Arena-Rosnav/arena-tools.git