From c89874b8832a3bcccb474805419fd4d70cc12266 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Sat, 31 Jul 2021 00:59:09 -0400
Subject: [PATCH 1/3] Support non-GPU lidar and swappable worlds in Stretch
Gazebo
---
stretch_gazebo/launch/gazebo.launch | 5 +-
stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 106 ++++++++++++------
2 files changed, 77 insertions(+), 34 deletions(-)
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index ddf6a02..2239c08 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -7,8 +7,11 @@
+
+
+
@@ -17,7 +20,7 @@
-
+
+
+
@@ -237,39 +239,77 @@
Gazebo/Green
-
-
- Gazebo/Black
-
- 0 0 0 0 0 0
- false
- 5.5
-
-
-
- 2000
- 1
- ${-M_PI}
- ${M_PI}
-
-
-
- 0.08
- 12.0
- 0.01
-
-
- gaussian
- 0.0
- 0.001
-
-
-
- scan
- laser
-
-
-
+
+
+
+ Gazebo/Black
+
+ 0 0 0 0 0 0
+ true
+ 5.5
+
+
+
+ 2000
+ 1
+ ${-M_PI}
+ ${M_PI}
+
+
+
+ 0.08
+ 12.0
+ 0.01
+
+
+ gaussian
+ 0.0
+ 0.001
+
+
+
+ scan
+ laser
+
+
+
+
+
+
+
+
+ Gazebo/Black
+
+ 0 0 0 0 0 0
+ true
+ 5.5
+
+
+
+ 2000
+ 1
+ ${-M_PI}
+ ${M_PI}
+
+
+
+ 0.08
+ 12.0
+ 0.01
+
+
+ gaussian
+ 0.0
+ 0.001
+
+
+
+ scan
+ laser
+
+
+
+
From 07f53be79f48fa441146d880f105fd79c146d743 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Sat, 31 Jul 2021 02:21:21 -0400
Subject: [PATCH 2/3] Launch simulated robot in stow position
---
stretch_gazebo/launch/gazebo.launch | 4 ++--
stretch_gazebo/scripts/publish_ground_truth_odom.py | 11 ++++++++++-
2 files changed, 12 insertions(+), 3 deletions(-)
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index 2239c08..c2dc117 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -1,6 +1,6 @@
-
+
@@ -24,7 +24,7 @@
+ args=" -urdf -model robot -param robot_description -J joint_lift 0.2 -J joint_wrist_yaw 3.14" respawn="false" output="screen" />
diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py
index f6ee321..ac76c62 100755
--- a/stretch_gazebo/scripts/publish_ground_truth_odom.py
+++ b/stretch_gazebo/scripts/publish_ground_truth_odom.py
@@ -3,14 +3,17 @@
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')
+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()
@@ -20,6 +23,12 @@ model.model_name='robot'
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
From c7b832ec376f8b00163a2a6dcaf420bcf20ee40f Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Sat, 31 Jul 2021 02:30:28 -0400
Subject: [PATCH 3/3] Add flag to visualize lidar in Gazebo
---
stretch_gazebo/launch/gazebo.launch | 3 ++-
stretch_gazebo/urdf/stretch_gazebo.urdf.xacro | 5 +++--
2 files changed, 5 insertions(+), 3 deletions(-)
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index c2dc117..6b75bcd 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -8,6 +8,7 @@
+
@@ -20,7 +21,7 @@
-
+
+
@@ -245,7 +246,7 @@
Gazebo/Black
0 0 0 0 0 0
- true
+ $(arg visualize_lidar)
5.5
@@ -281,7 +282,7 @@
Gazebo/Black
0 0 0 0 0 0
- true
+ $(arg visualize_lidar)
5.5