text: # Copyright (c) 2021 PAL Robotics S.L.
  # Modified by José Miguel Guerrero Hernández
  #
  # Licensed under the Apache License, Version 2.0 (the "License");
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  #
  #     http://www.apache.org/licenses/LICENSE-2.0
  #
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  
  from launch import LaunchDescription
  from launch.actions import DeclareLaunchArgument
  from launch.substitutions import LaunchConfiguration
  from launch_ros.actions import Node
  from launch_pal.include_utils import include_launch_py_description
  
  import os
  from ament_index_python.packages import get_package_share_directory
  import yaml
  
  def generate_launch_description():
      #    This format doesn't work because we have to expand gzpose into
      #    different args for spawn_entity.py
      #    gz_pose = DeclareLaunchArgument(
      #        'gzpose', default_value='-x 0 -y 0 -z 0.0 -R 0.0 -P 0.0 -Y 0.0 ',
      #        description='Spawn gazebo position as provided to spawn_entity.py'
      #    )
  
      # @TODO: load PID gains? used in gazebo_ros_control fork
      # @TODO: load tiago_pal_hardware_gazebo
      config = os.path.join(
          get_package_share_directory('computer_vision'),
          'config',
          'params.yaml'
          )
  
      with open(config, "r") as stream:
          try:
              conf = (yaml.safe_load(stream))
              
          except yaml.YAMLError as exc:
              print(exc)
  
      model_name = DeclareLaunchArgument(
          'model_name', default_value='tiago',
          description='Gazebo model name'
      )
  
      tiago_state_publisher = include_launch_py_description(
          'tiago_description',
          ['launch', 'robot_state_publisher.launch.py'])
  
      tiago_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                          arguments=['-topic', 'robot_description',
                                     '-entity', LaunchConfiguration(
                                         'model_name'),
                                         conf['computer_vision']['tiago_position']['x'],
                                         conf['computer_vision']['tiago_position']['y'],
                                         conf['computer_vision']['tiago_position']['z'],
                                         conf['computer_vision']['tiago_position']['roll'],
                                         conf['computer_vision']['tiago_position']['pitch'],
                                         conf['computer_vision']['tiago_position']['yaw'],
                                     # LaunchConfiguration('gzpose'),
                                     ],
                          output='screen')
  
      return LaunchDescription([
          # gz_pose,
          model_name,
          tiago_state_publisher,
          tiago_entity,
      ])
  