From 3c4dfac66ba498d1586ef2642e63e1dc2ed026a2 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Thu, 27 Jun 2024 14:28:50 -0400
Subject: [PATCH] Experiment with adding two robots to Gazebo sim

---
 stretch_gazebo/launch/base.launch             |  21 ++++
 stretch_gazebo/launch/multi_robot.launch      | 103 ++++++++++++++++++
 stretch_gazebo/launch/single_robot.launch     |  54 +++++++++
 .../scripts/publish_ground_truth_odom.py      |   2 +-
 .../scripts/publish_ground_truth_odom2.py     |  43 ++++++++
 .../urdf/stretch_gazebo_dex_wrist.urdf.xacro  |   3 +-
 ...stretch_gazebo_standard_gripper.urdf.xacro |   3 +-
 7 files changed, 226 insertions(+), 3 deletions(-)
 create mode 100644 stretch_gazebo/launch/base.launch
 create mode 100644 stretch_gazebo/launch/multi_robot.launch
 create mode 100644 stretch_gazebo/launch/single_robot.launch
 create mode 100755 stretch_gazebo/scripts/publish_ground_truth_odom2.py

diff --git a/stretch_gazebo/launch/base.launch b/stretch_gazebo/launch/base.launch
new file mode 100644
index 0000000..5019db2
--- /dev/null
+++ b/stretch_gazebo/launch/base.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <arg name="paused" default="true"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+  <arg name="world" default="worlds/empty.world"/>
+
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(arg world)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="verbose" value="true"/>
+  </include>
+
+</launch>
diff --git a/stretch_gazebo/launch/multi_robot.launch b/stretch_gazebo/launch/multi_robot.launch
new file mode 100644
index 0000000..3fc2e2b
--- /dev/null
+++ b/stretch_gazebo/launch/multi_robot.launch
@@ -0,0 +1,103 @@
+<launch>
+
+  <arg name="gpu_lidar" default="false"/>
+  <arg name="visualize_lidar" default="false"/>
+  <arg name="dex_wrist" default="false"/>
+  <param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
+
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
+
+  <group ns="robot_1">
+  
+      <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar) robot_num:=1"/>
+
+      <!-- push robot_description to factory and spawn robot in gazebo -->
+      <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+            args=" -urdf -model robot1 -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
+
+      <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
+      <param name="publish_frequency" type="double" value="30.0" />
+      </node>
+
+      <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/> -->
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/joints.yaml"
+                  ns="stretch_joint_state_controller" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/drive_config.yaml"
+                  ns="stretch_diff_drive_controller" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/arm.yaml"/>
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/head.yaml" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/gripper.yaml" />
+
+      <rosparam command="load"
+            file="$(find stretch_gazebo)/config/dex_wrist.yaml" if="$(arg dex_wrist)"/>
+
+      <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+            args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"
+            unless="$(arg dex_wrist)"/>
+
+      <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+            args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller"
+      if="$(arg dex_wrist)"/>
+
+      <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
+
+  </group>
+
+  <group ns="robot_2">
+  
+      <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar) robot_num:=2"/>
+
+      <!-- push robot_description to factory and spawn robot in gazebo -->
+      <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+            args=" -urdf -model robot2 -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14 -x 1.0 -y 0.0" respawn="false" output="screen" />
+
+      <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
+      <param name="publish_frequency" type="double" value="30.0" />
+      </node>
+
+      <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/> -->
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/joints.yaml"
+                  ns="stretch_joint_state_controller" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/drive_config.yaml"
+                  ns="stretch_diff_drive_controller" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/arm.yaml"/>
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/head.yaml" />
+
+      <rosparam command="load"
+                  file="$(find stretch_gazebo)/config/gripper.yaml" />
+
+      <rosparam command="load"
+            file="$(find stretch_gazebo)/config/dex_wrist.yaml" if="$(arg dex_wrist)"/>
+
+      <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+            args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"
+            unless="$(arg dex_wrist)"/>
+
+      <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+            args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller"
+      if="$(arg dex_wrist)"/>
+
+      <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom2.py" output="screen"/>
+
+  </group>
+
+</launch>
diff --git a/stretch_gazebo/launch/single_robot.launch b/stretch_gazebo/launch/single_robot.launch
new file mode 100644
index 0000000..310e3cd
--- /dev/null
+++ b/stretch_gazebo/launch/single_robot.launch
@@ -0,0 +1,54 @@
+<launch>
+
+  <arg name="gpu_lidar" default="false"/>
+  <arg name="visualize_lidar" default="false"/>
+  <arg name="dex_wrist" default="false"/>
+  <param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
+
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
+  <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) gpu_lidar:=$(arg gpu_lidar) visualize_lidar:=$(arg visualize_lidar)"/>
+
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+        args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+
+  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/> -->
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/joints.yaml"
+            ns="stretch_joint_state_controller" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/drive_config.yaml"
+            ns="stretch_diff_drive_controller" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/arm.yaml"/>
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/head.yaml" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/gripper.yaml" />
+
+  <rosparam command="load"
+        file="$(find stretch_gazebo)/config/dex_wrist.yaml" if="$(arg dex_wrist)"/>
+
+  <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"
+        unless="$(arg dex_wrist)"/>
+
+  <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller stretch_dex_wrist_controller"
+  if="$(arg dex_wrist)"/>
+
+  <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
+
+</launch>
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py
index ac76c62..878d233 100755
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ b/stretch_gazebo/scripts/publish_ground_truth_odom.py
@@ -19,7 +19,7 @@ odom=Odometry()
 header = Header()
 header.frame_id='/ground_truth'
 model = GetModelStateRequest()
-model.model_name='robot'
+model.model_name='robot1'
 models = []
 r = rospy.Rate(20)
 
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom2.py b/stretch_gazebo/scripts/publish_ground_truth_odom2.py
new file mode 100755
index 0000000..0030580
--- /dev/null
+++ b/stretch_gazebo/scripts/publish_ground_truth_odom2.py
@@ -0,0 +1,43 @@
+#! /usr/bin/env python3
+
+from gazebo_msgs.srv import GetModelState, GetModelStateRequest, GetWorldProperties
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Header
+from std_srvs.srv import Empty
+import rospy
+import time
+
+rospy.init_node('ground_truth_odometry_publisher')
+odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10)
+
+rospy.wait_for_service('/gazebo/get_model_state')
+get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+
+odom=Odometry()
+header = Header()
+header.frame_id='/ground_truth'
+model = GetModelStateRequest()
+model.model_name='robot2'
+models = []
+r = rospy.Rate(20)
+
+pause_timeout = time.time() + 4.0
+while time.time() < pause_timeout:
+    rospy.logwarn("Waiting %.2f seconds to unpause physics", pause_timeout - time.time())
+    time.sleep(1.0)
+unpause_physics()
+
+while not rospy.is_shutdown():
+    if model.model_name not in models:
+        models = get_world_properties().model_names
+        rospy.logwarn("Waiting for %s to spawn to publish ground truth odometry", model.model_name)
+    else:
+        result = get_model_srv(model)
+        odom.pose.pose = result.pose
+        odom.twist.twist = result.twist
+        header.stamp = rospy.Time.now()
+        odom.header = header
+        odom_pub.publish(odom)
+    r.sleep()
diff --git a/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
index 83982c4..070069b 100644
--- a/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo_dex_wrist.urdf.xacro
@@ -2,6 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch">
   <xacro:arg name="gpu_lidar" default="false" />
   <xacro:arg name="visualize_lidar" default="false" />
+  <xacro:arg name="robot_num" default="1" />
 
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_main.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_aruco.xacro" />
@@ -12,7 +13,7 @@
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-      <robotNamespace>/</robotNamespace>
+      <robotNamespace>/robot_$(arg robot_num)</robotNamespace>
     </plugin>
   </gazebo>
 
diff --git a/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
index 17fb961..00bd1c0 100644
--- a/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
+++ b/stretch_gazebo/urdf/stretch_gazebo_standard_gripper.urdf.xacro
@@ -2,6 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="stretch">
   <xacro:arg name="gpu_lidar" default="false" />
   <xacro:arg name="visualize_lidar" default="false" />
+  <xacro:arg name="robot_num" default="1" />
 
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_main.xacro" />
   <xacro:include filename="$(find stretch_gazebo)/urdf/stretch_aruco.xacro" />
@@ -12,7 +13,7 @@
 
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-      <robotNamespace>/</robotNamespace>
+      <robotNamespace>/robot_$(arg robot_num)</robotNamespace>
     </plugin>
   </gazebo>