From b02a3a0689dc8f6f69a3c0e2a042da74c17bcac2 Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Tue, 15 Mar 2022 07:07:01 +0100
Subject: [PATCH] Better launch process, not as hacky anymore.

---
 src/gaz_simulation/launch/moveit_launch.py | 161 +++++++++++++++------
 src/gaz_simulation/launch/test_launch.py   |  18 +--
 2 files changed, 120 insertions(+), 59 deletions(-)

diff --git a/src/gaz_simulation/launch/moveit_launch.py b/src/gaz_simulation/launch/moveit_launch.py
index a9f928d..ba77ec6 100644
--- a/src/gaz_simulation/launch/moveit_launch.py
+++ b/src/gaz_simulation/launch/moveit_launch.py
@@ -1,10 +1,34 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import Command
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+        print("ERROR: load_file "+package_name+"|"+file_path)
+        return None
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+        print("ERROR: load_file "+package_name+"|"+file_path)
+        return None
 
 def generate_launch_description():
     robot_name = "iisy"
@@ -13,24 +37,59 @@ def generate_launch_description():
     path = FindPackageShare(package=package_name).find(package_name)
     world_path = os.path.join(path, "world/test2.xml")
     urdf_path = os.path.join(path, "urdf/iisy.urdf")
+
+    robot_description_config = load_file('gaz_simulation', 'urdf/iisy.urdf')
+    robot_description = {'robot_description' : robot_description_config}
+
+    robot_description_semantic_config = load_file('iisy_config', 'config/iisy.srdf')
+    robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yaml')
+    robot_description_kinematics = { 'robot_description_kinematics' : kinematics_yaml }
+
+    ompl_planning_pipeline_config = { 'move_group' : {
+        'planning_plugin' : 'ompl_interface/OMPLPlanner',
+        'request_adapters' : """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""" ,
+        'start_state_max_bounds_error' : 0.1 } }
+    ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yaml')
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    controllers_yaml = load_yaml('iisy_config', 'config/gazebo_controllers.yaml')
+    moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml,
+                           'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'}
+
+    trajectory_execution = {'moveit_manage_controllers': True,
+                            'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+                            'trajectory_execution.allowed_goal_duration_margin': 0.5,
+                            'trajectory_execution.allowed_start_tolerance': 0.01}
+
+    planning_scene_monitor_parameters = {"publish_planning_scene": True,
+                 "publish_geometry_updates": True,
+                 "publish_state_updates": True,
+                 "publish_transforms_updates": True}
+
+
+    #octomap_config = {'octomap_frame': 'camera_rgb_optical_frame', 
+    #                  'octomap_resolution': 0.05,
+    #                  'max_range': 5.0}
+
+    #octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
+
     return LaunchDescription([
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
-        #    launch_arguments={'world': world_path}.items()
-        #),
-        
-        ExecuteProcess(
-            cmd=[[
-                'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
-            ]],
-            shell=True
+        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'))
         ),
+
         Node(
             package='tf2_ros',
             namespace='gaz_simulation',
@@ -38,6 +97,7 @@ def generate_launch_description():
             name='lidar_1_broadcaster',
             arguments=["1", "-1.9", "1", "1.5707963267949", "0", "0", "map", "lidar_1_link"]
         ),
+
         Node(
             package='tf2_ros',
             namespace='gaz_simulation',
@@ -45,6 +105,7 @@ def generate_launch_description():
             name='lidar_2_broadcaster',
             arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
         ),
+
         Node(
             package='gazebo_ros',
             executable='spawn_entity.py',
@@ -56,19 +117,39 @@ def generate_launch_description():
                        '-Y', '0'],  # Yaw
             output='screen'
         ),
-        # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
+
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=['-entity', robot_name,
+                       '-topic', 'robot_description',
+                       '-x', '-1',
+                       '-y', '-1',
+                       '-z', '1',
+                       '-Y', '0'],  # Yaw
+            output='screen'
+        ),
+
+
+        #Node(
+        #    package='robot_state_publisher',
+        #    executable='robot_state_publisher',
+        #    name='robot_state_publisher',
+        #    output='both',
+        #    parameters=[robot_description]
+        #),
+
         Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
             parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
         ),
 
-        # Publish the joint states of the robot
         Node(
             package='joint_state_publisher',
             executable='joint_state_publisher',
             name='joint_state_publisher'
-        )
+        ),
 
         # ,
         # Node(
@@ -77,38 +158,22 @@ def generate_launch_description():
         #    name='gazebo',
         #    arguments=["test.xml"]
         # )
-    ])
 
-
-<?xml version="1.0"?>
-<launch>
-  <arg name="paused" default="false"/>
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
-       default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>
-
-  <!-- startup simulated world -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <let name="world_name" default="worlds/empty.world"/>
-    <let name="paused" value="true"/>
-    <let name="gui" value="$(arg gazebo_gui)"/>
-  </include>
-
-  <!-- send robot urdf to param server -->
-  <param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />
-
-  <!-- unpause only after loading robot model -->
-  <let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
-  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
-  <arg name="world_pose" value="-x 0 -y 0 -z 0" />
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
-    respawn="false" output="screen" />
-
-  <!-- Load joint controller parameters for Gazebo -->
-  <param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
-  <!-- Spawn Gazebo ROS controllers -->
-  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
-  <!-- Load ROS controllers -->
-  <include file="$(dirname)/ros_controllers.launch"/>
-
-</launch>
+        
+        #Node(package='moveit_ros_move_group',
+        #    executable='move_group',
+        #    #prefix='xterm -fs 10 -e gdb --ex run --args',
+        #    output='screen',
+        #    parameters=[
+        #        robot_description,
+        #        robot_description_semantic,
+        #        kinematics_yaml,
+        #        ompl_planning_pipeline_config,
+        #        trajectory_execution,
+        #        moveit_controllers,
+        #        planning_scene_monitor_parameters,
+        #        #octomap_config,
+        #        #octomap_updater_config
+        #    ]
+        #)
+    ])
\ No newline at end of file
diff --git a/src/gaz_simulation/launch/test_launch.py b/src/gaz_simulation/launch/test_launch.py
index 42f24c6..8b39d6e 100644
--- a/src/gaz_simulation/launch/test_launch.py
+++ b/src/gaz_simulation/launch/test_launch.py
@@ -1,7 +1,7 @@
 import os
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import Command
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -15,18 +15,14 @@ def generate_launch_description():
     world_path = os.path.join(path, "world/test2.xml")
     urdf_path = os.path.join(path, "urdf/iisy.urdf")
     return LaunchDescription([
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
-        #    launch_arguments={'world': world_path}.items()
-        #),
-        
-        ExecuteProcess(
-            cmd=[[
-                'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
-            ]],
-            shell=True
+        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(