From 1b7d99264d08c4bf014d777733a716c96d61c487 Mon Sep 17 00:00:00 2001
From: Jarek Karwowski <chromedivizer@gmail.com>
Date: Tue, 26 Sep 2023 00:44:39 +0200
Subject: [PATCH] `experiments` - more explicit world selection for scenarios,
 using "clean" sim worlds (HuBeRo actors now spawned with a service), updated
 HuBeRo repo version

- see this pull for spawning details https://github.com/rayvburn/hubero/pull/60
---
 tiago_experiments-melodic.rosinstall          |  2 +-
 .../launch/012.launch                         | 53 +++++++++++++++----
 .../launch/aws_hospital.launch                | 21 +++++---
 3 files changed, 57 insertions(+), 19 deletions(-)

diff --git a/tiago_experiments-melodic.rosinstall b/tiago_experiments-melodic.rosinstall
index 2d0cfd2..94f340c 100644
--- a/tiago_experiments-melodic.rosinstall
+++ b/tiago_experiments-melodic.rosinstall
@@ -1,5 +1,5 @@
 # HuBeRo provides dynamic human actors
-- git: {local-name: hubero, uri: 'https://github.com/rayvburn/hubero.git', version: 'task-request-ros-api-get-name-method'}
+- git: {local-name: hubero, uri: 'https://github.com/rayvburn/hubero.git', version: 'launch-for-spawning-actors-in-gazebo'}
 
 # worlds used in experiments
 - git: {local-name: tiago_sim_integration, uri: 'https://github.com/rayvburn/tiago_sim_integration.git', version: '012-hubero-integration'}
diff --git a/tiago_social_experiments_sim/launch/012.launch b/tiago_social_experiments_sim/launch/012.launch
index 9bff46f..1ced77c 100644
--- a/tiago_social_experiments_sim/launch/012.launch
+++ b/tiago_social_experiments_sim/launch/012.launch
@@ -64,8 +64,11 @@
         <arg name="publish_goal_delay" if="$(eval scenario == 'dynamic')" value="$(eval {'titanium': '110', 'steel': '110', 'iron': '70'}.get(arg('robot_type'), '70'))"/>
 
         <arg name="world_file_wo_people" value="$(find tiago_sim_integration)/worlds/lab_012_v2.world"/>
-        <arg name="world_file_people" unless="$(eval scenario == 'dynamic')" value="$(find tiago_sim_integration)/worlds/lab_012_v2_actor.world"/>
-        <arg name="world_file_people" if="$(eval scenario == 'dynamic')" value="$(find tiago_sim_integration)/worlds/lab_012_v2_actor_dynamic.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'full')" value="$(find tiago_sim_integration)/worlds/lab_012_v2.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'short')" value="$(find tiago_sim_integration)/worlds/lab_012_v2.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'hard')" value="$(find tiago_sim_integration)/worlds/lab_012_v2.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'static')" value="$(find tiago_sim_integration)/worlds/lab_012_v2.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'dynamic')" value="$(find tiago_sim_integration)/worlds/lab_012_v2_dynamic.world"/>
 
         <arg name="pose_sim" if="$(eval scenario == 'full')" value="-x 1.7 -y 5.6 -z -0.003 -R 0.0 -P 0.0 -Y -1.5708"/>
         <arg name="pose_sim" if="$(eval scenario == 'short')" value="-x 1.729204 -y -0.868255 -z -0.003 -R 0.0 -P 0.0 -Y -2.081408"/>
@@ -85,6 +88,7 @@
         <arg name="map_file" value="$(arg map_file)"/>
         <arg name="allow_clearing_map_obstacles" value="$(arg allow_clearing_map_obstacles)"/>
         <arg name="map_frame" value="$(arg map_frame)"/>
+        <!-- Set `external_map` to false for scenarios in which HuBeRo actors are not spawned -->
 
         <arg name="goal_file" if="$(eval scenario == 'full')" value="$(arg goal_file)"/>
         <arg name="goal_file" if="$(eval scenario == 'short')" value="$(arg goal_file)"/>
@@ -106,26 +110,55 @@
     <!--
         Launch HuBeRo actors ROS interfaces. Note that it will significantly affect simulation performance.
     -->
-    <group if="$(arg people_sim)">
-        <include file="$(find hubero_ros)/launch/actor.launch">
+    <group if="$(eval people_sim == true and (scenario == 'static' or scenario == 'hard' or scenario == 'short' or scenario == 'full'))">
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
             <arg name="actor_name" value="actor1"/>
-            <arg name="local_planner" value="teb"/>
+            <arg name="pose_x" value="0.10"/>
+            <arg name="pose_y" value="-4.00"/>
+            <arg name="pose_yaw" value="-2.35"/>
             <arg name="map_file" value="$(arg map_file)"/>
-            <arg name="use_shared_map" value="$(arg shared_map)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
             <arg name="map_frame" value="$(arg map_frame)"/>
             <arg name="tf_world_map" value="$(arg tf_world_map)"/>
             <arg name="map_bounds" value="$(arg map_bounds)"/>
         </include>
-        <include file="$(find hubero_ros)/launch/actor.launch">
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
             <arg name="actor_name" value="actor2"/>
-            <arg name="local_planner" value="teb"/>
+            <arg name="pose_x" value="-0.6"/>
+            <arg name="pose_y" value="-3.05"/>
+            <arg name="pose_yaw" value="0.75"/>
             <arg name="map_file" value="$(arg map_file)"/>
-            <arg name="use_shared_map" value="$(arg shared_map)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
+            <arg name="map_frame" value="$(arg map_frame)"/>
+            <arg name="tf_world_map" value="$(arg tf_world_map)"/>
+            <arg name="map_bounds" value="$(arg map_bounds)"/>
+        </include>
+    </group>
+
+    <group if="$(eval scenario == 'dynamic' and people_sim == true)">
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
+            <arg name="actor_name" value="actor1"/>
+            <arg name="pose_x" value="+0.3"/>
+            <arg name="pose_y" value="+4.75"/>
+            <arg name="pose_yaw" value="1.70"/>
+            <arg name="map_file" value="$(arg map_file)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
+            <arg name="map_frame" value="$(arg map_frame)"/>
+            <arg name="tf_world_map" value="$(arg tf_world_map)"/>
+            <arg name="map_bounds" value="$(arg map_bounds)"/>
+        </include>
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
+            <arg name="actor_name" value="actor2"/>
+            <arg name="pose_x" value="-0.6"/>
+            <arg name="pose_y" value="+4.2"/>
+            <arg name="pose_yaw" value="+2.10"/>
+            <arg name="map_file" value="$(arg map_file)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
             <arg name="map_frame" value="$(arg map_frame)"/>
             <arg name="tf_world_map" value="$(arg tf_world_map)"/>
             <arg name="map_bounds" value="$(arg map_bounds)"/>
         </include>
         <!-- scenario node - orchestrates actors movements -->
-        <node if="$(eval scenario == 'dynamic')" pkg="tiago_social_scenarios_sim" type="lab_012_dynamic_hubero" name="lab_012_dynamic_hubero" output="screen" args="1"/>
+        <node pkg="tiago_social_scenarios_sim" type="lab_012_dynamic_hubero" name="lab_012_dynamic_hubero" output="screen" args="1"/>
     </group>
 </launch>
diff --git a/tiago_social_experiments_sim/launch/aws_hospital.launch b/tiago_social_experiments_sim/launch/aws_hospital.launch
index bcd7664..5a7b75a 100644
--- a/tiago_social_experiments_sim/launch/aws_hospital.launch
+++ b/tiago_social_experiments_sim/launch/aws_hospital.launch
@@ -68,7 +68,7 @@
         <arg name="world_file_wo_people" value="$(find aws_robomaker_hospital_world)/worlds/hospital.world"/>
         <!-- Scenario-specific -->
         <arg name="world_file_people" if="$(eval scenario == 'normal' and people_lite == true)" default="$(find aws_robomaker_hospital_world)/worlds/hospital.world"/>
-        <arg name="world_file_people" if="$(eval scenario == 'normal' and people_lite != true)" default="$(find aws_robomaker_hospital_world)/worlds/hospital_crowded_hubero.world"/>
+        <arg name="world_file_people" if="$(eval scenario == 'normal' and people_lite != true)" default="$(find aws_robomaker_hospital_world)/worlds/hospital_crowded.world"/>
         <arg name="world_file_people" if="$(eval scenario == 'wall_avoidance')" default="$(find aws_robomaker_hospital_world)/worlds/hospital.world"/>
         <arg name="world_file_people" if="$(eval scenario == 'crowd_avoidance')" default="$(find aws_robomaker_hospital_world)/worlds/hospital_reception_crowd.world"/>
 
@@ -85,6 +85,7 @@
         <arg name="map_file" value="$(arg map_file)"/>
         <arg name="allow_clearing_map_obstacles" value="$(arg allow_clearing_map_obstacles)"/>
         <arg name="map_frame" value="$(arg map_frame)"/>
+        <!-- Set `external_map` to false for scenarios in which HuBeRo actors are not spawned -->
 
         <arg name="goal_file" if="$(eval scenario == 'normal')" value="$(find tiago_social_experiments_sim)/config/aws_hospital_normal_finish_pose.yaml"/>
         <arg name="goal_file" if="$(eval scenario == 'wall_avoidance')" value="$(find tiago_social_experiments_sim)/config/aws_hospital_wall_avoidance_finish_pose.yaml"/>
@@ -97,25 +98,29 @@
         for faster prototyping
     -->
     <group if="$(eval scenario == 'normal' and people_sim == true and people_lite == false)">
-        <include file="$(find hubero_ros)/launch/actor.launch">
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
             <arg name="actor_name" value="actor1"/>
-            <arg name="local_planner" value="teb"/>
+            <arg name="pose_x" value="-7.6"/>
+            <arg name="pose_y" value="-7.95"/>
+            <arg name="pose_yaw" value="0.82"/>
             <arg name="map_file" value="$(arg map_file)"/>
-            <arg name="use_shared_map" value="$(arg shared_map)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
             <arg name="map_frame" value="$(arg map_frame)"/>
             <arg name="tf_world_map" value="$(arg tf_world_map)"/>
             <arg name="map_bounds" value="$(arg map_bounds)"/>
         </include>
-        <include file="$(find hubero_ros)/launch/actor.launch">
+        <include file="$(find hubero_bringup_gazebo_ros)/launch/spawn_actor.launch">
             <arg name="actor_name" value="actor2"/>
-            <arg name="local_planner" value="teb"/>
+            <arg name="pose_x" value="-7.50"/>
+            <arg name="pose_y" value="-22.95"/>
+            <arg name="pose_yaw" value="1.78"/>
             <arg name="map_file" value="$(arg map_file)"/>
-            <arg name="use_shared_map" value="$(arg shared_map)"/>
+            <arg name="shared_map" value="$(arg shared_map)"/>
             <arg name="map_frame" value="$(arg map_frame)"/>
             <arg name="tf_world_map" value="$(arg tf_world_map)"/>
             <arg name="map_bounds" value="$(arg map_bounds)"/>
         </include>
-        <!-- scenario node - orchestrates actors movement for a specific scenario -->
+        <!-- scenario node - orchestrates actors movements -->
         <node pkg="tiago_social_scenarios_sim" type="aws_hospital_normal_hubero" name="aws_hospital_normal_hubero" output="screen" args="1"/>
     </group>
 </launch>