import os
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import (DeclareLaunchArgument, GroupAction,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import PushRosNamespace
 
def generate_launch_description():
 
  # Set the path to different files and folders.
  bringup_dir = get_package_share_directory('nav2_bringup')
  launch_dir = os.path.join(bringup_dir, 'launch')
  pkg_share = FindPackageShare(package='two_wheeled_robot').find('two_wheeled_robot')
  default_launch_dir = os.path.join(pkg_share, 'launch')
  robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml') 
  robot_name_in_urdf = 'two_wheeled_robot'
  default_rviz_config_path = os.path.join(pkg_share, 'rviz/nav2_default_view.rviz')
  nav2_dir = FindPackageShare(package='nav2_bringup').find('nav2_bringup') 
  nav2_launch_dir = os.path.join(pkg_share, 'launch') 
  static_map_path = os.path.join(pkg_share, 'maps', 'my_map.yaml')
  nav2_params_path = os.path.join(pkg_share, 'params', 'nav2_params.yaml')
  nav2_bt_path = FindPackageShare(package='nav2_bt_navigator').find('nav2_bt_navigator')
  behavior_tree_xml_path = os.path.join(nav2_bt_path, 'behavior_trees', 'navigate_w_replanning_and_recovery.xml')
  bringup_dir=get_package_share_directory('nav2_bringup')  
  # Launch configuration variables specific to simulation
  autostart = LaunchConfiguration('autostart')
  headless = LaunchConfiguration('headless')
  map_yaml_file = LaunchConfiguration('map')
  namespace = LaunchConfiguration('namespace')
  params_file = LaunchConfiguration('params_file')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  slam = LaunchConfiguration('slam')
  use_namespace = LaunchConfiguration('use_namespace')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')

  remappings = [('/tf', 'tf'),
                ('/tf_static', 'tf_static')]
   
  # Declare the launch arguments  
  declare_namespace_cmd = DeclareLaunchArgument(
    name='namespace',
    default_value='',
    description='Top-level namespace')
    
  declare_use_namespace_cmd = DeclareLaunchArgument(
    name='use_namespace',
    default_value='False',
    description='Whether to apply a namespace to the navigation stack')
         
  declare_autostart_cmd = DeclareLaunchArgument(
    name='autostart', 
    default_value='true',
    description='Automatically startup the nav2 stack')
 
 
  declare_map_yaml_cmd = DeclareLaunchArgument(
    name='map',
    
    default_value=os.path.join(
            pkg_share, 'maps', 'my_map.yaml'),
    description='Full path to map file to load')
           
  declare_params_file_cmd = DeclareLaunchArgument(
    name='params_file',
    default_value=nav2_params_path,
    description='Full path to the ROS2 parameters file to use for all launched nodes')
     
  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')
 
  declare_simulator_cmd = DeclareLaunchArgument(
    name='headless',
    default_value='False',
    description='Whether to execute gzclient')
 
  declare_slam_cmd = DeclareLaunchArgument(
    name='slam',
    default_value='False',
    description='Whether to run SLAM')
     
  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')
 
  declare_use_rviz_cmd = DeclareLaunchArgument(
    name='use_rviz',
    default_value='True',
    description='Whether to start RVIZ')
     
  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='True',
    description='Use simulation (Gazebo) clock if true')
 
  # Start robot localization using an Extended Kalman filter
  start_robot_localization_cmd = Node(
    package='robot_localization',
    executable='ekf_node',
    name='ekf_filter_node',
    output='screen',
    parameters=[robot_localization_file_path, 
    {'use_sim_time': use_sim_time}])
     
  
  bringup_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'bringup_launch.py')),
        launch_arguments={'namespace': namespace,
                          'use_namespace': use_namespace,
                          'slam': slam,
                          'map': map_yaml_file,
                          'use_sim_time': use_sim_time,
                          'params_file': params_file,
                          'autostart': autostart}.items())
  #bringup_cmd_group = GroupAction([
        #PushRosNamespace(
            #condition=IfCondition(use_namespace),
            #namespace=namespace),

        #IncludeLaunchDescription(
            #PythonLaunchDescriptionSource(os.path.join(launch_dir,
                                                    #   'localization_launch.py')),
            #condition=IfCondition(PythonExpression(['not ', slam])),
            #launch_arguments={'namespace': namespace,
                              #'map': map_yaml_file,
                             # 'use_sim_time': use_sim_time,
                            #  'autostart': autostart,
                           #   'params_file': params_file,
                              #'use_lifecycle_mgr': 'false'}.items()),
                              
        
        

      
                              
                           #])
  #static1 = Node(package = "tf2_ros", 
                       #executable = "static_transform_publisher",
                       #arguments = ["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"])
 
  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.z
  
 
  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])  
    
  node = Node(package = "tf2_ros", 
                       executable = "static_transform_publisher",
                       arguments = ["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"])
                       
  node1 = Node(package = "tf2_ros", 
                       executable = "static_transform_publisher",
                       arguments = ["0", "0", "0", "0", "0", "0", "map", "odom"])
  
  #node2 = Node(package = "tf2_ros", 
                       #executable = "static_transform_publisher",
                       #arguments = ["-0.10", "0", "0.05", "0", "0", "0", "base_link", "imu_link"])
                       

    
  ld = LaunchDescription() 
  # Declare the launch options
  ld.add_action(declare_namespace_cmd)
  ld.add_action(declare_use_namespace_cmd)
  ld.add_action(declare_autostart_cmd)
  ld.add_action(declare_map_yaml_cmd)
  ld.add_action(declare_params_file_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_slam_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(start_robot_localization_cmd)
  ld.add_action(node)
  ld.add_action(node1)
  #ld.add_action(node2)
  ld.add_action(start_rviz_cmd)
  #ld.add_action(bringup_cmd_group)
  ld.add_action(bringup_cmd)
  return ld