From e11840da5be93669ec04c32aaa9390f73ad8e2a2 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Mon, 21 Mar 2022 23:46:59 +0100
Subject: [PATCH] Minor experimentation

---
 src/iisy_config/launch/debug.launch.py       | 56 ++++++++++++++++++++
 src/iisy_config/launch/lbr_bringup.launch.py | 20 +++----
 src/iisy_config/urdf/iisy.urdf               |  4 +-
 3 files changed, 68 insertions(+), 12 deletions(-)
 create mode 100644 src/iisy_config/launch/debug.launch.py

diff --git a/src/iisy_config/launch/debug.launch.py b/src/iisy_config/launch/debug.launch.py
new file mode 100644
index 0000000..b50db80
--- /dev/null
+++ b/src/iisy_config/launch/debug.launch.py
@@ -0,0 +1,56 @@
+import os
+from launch.launch_description import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, SetEnvironmentVariable, UnsetEnvironmentVariable
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+    package_name = "gaz_simulation"
+    pkg_gazebo_ros = FindPackageShare('gazebo_ros').find('gazebo_ros')
+    path = FindPackageShare(package=package_name).find(package_name)
+    world_path = os.path.join(path, "world/test2.xml")
+
+    # Note: Environment variable GAZEBO_MODEL_PATH is extended as in 
+    # ROS2 control demos via environment hook https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
+    # Also see https://colcon.readthedocs.io/en/released/developer/environment.html#dsv-files
+    # Gazebo launch scripts append GAZEBO_MODEL_PATH with known paths, see https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ab1ae5c05eda62674b36df74eb3be8c93cdc8761/gazebo_ros/launch/gzclient.launch.py#L26
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=[
+            "-topic", "robot_description",
+            "-entity", "iisy"
+        ],
+        output="screen"
+    )
+
+    bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("iisy_config"),
+                "launch",
+                "lbr_bringup.launch.py"
+            ])
+        ),
+    )
+
+    return LaunchDescription([
+        bringup,
+        SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+            launch_arguments={'world': world_path}.items()
+        ),
+
+        UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
+
+        # Start Gazebo client
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
+        ),
+        spawn_entity
+    ])
\ No newline at end of file
diff --git a/src/iisy_config/launch/lbr_bringup.launch.py b/src/iisy_config/launch/lbr_bringup.launch.py
index 2a0f944..41b3846 100644
--- a/src/iisy_config/launch/lbr_bringup.launch.py
+++ b/src/iisy_config/launch/lbr_bringup.launch.py
@@ -37,15 +37,15 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Gazebo simulation 
-    simulation = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution([
-                FindPackageShare("iisy_config"),
-                "launch",
-                "lbr_simulation.launch.py"
-            ])
-        ),
-    )
+    #simulation = IncludeLaunchDescription(
+    #    PythonLaunchDescriptionSource(
+    #        PathJoinSubstitution([
+    #            FindPackageShare("iisy_config"),
+    #            "launch",
+    #            "lbr_simulation.launch.py"
+    #        ])
+    #    ),
+    #)
 
     # Move group
     move_group = IncludeLaunchDescription(
@@ -62,7 +62,7 @@ def launch_setup(context, *args, **kwargs):
     )
 
     return [
-        simulation,
+        #simulation,
         control,
         move_group
     ]
diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf
index c378924..4928f64 100644
--- a/src/iisy_config/urdf/iisy.urdf
+++ b/src/iisy_config/urdf/iisy.urdf
@@ -199,7 +199,7 @@
         <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
 
-    <!--<transmission name="trans_base_rot">
+    <transmission name="trans_base_rot">
         <type>transmission_interface/SimpleTransmission</type>
         <joint name="base_rot">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
@@ -258,7 +258,7 @@
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
-    </transmission>-->
+    </transmission>
 
     <ros2_control name="lbr_hardware_interface" type="system">
         <!-- define hardware including parameters, also gazebo -->