<!-- 
******************************************************************************************
*  Copyright (c) 2023 Yang Haodong, All Rights Reserved                                  *
*                                                                                        *
*  @brief    move base module parameter configure.                                       *
*  @author   Haodong Yang,                                                               *
*  @version  1.0.0                                                                       *
*  @date     2022.06.30                                                                  *
*  @license  GNU General Public License (GPL)                                            *
******************************************************************************************
-->

<launch>
    <!-- robot's namespace -->
    <arg name="robot_namespace" />
    <!-- whether use namespace or not -->
    <arg name="start_ns" default="false" />
    <!-- robot's type -->
    <arg name="robot" default="turtlebot3_waffle" />
    <!-- global path planner name -->
    <arg name="global_planner" default="a_star" />
    <!-- local planner name -->
    <arg name="local_planner" default="dwa" />

    <!-- move base module -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <!-- global planner plugin -->
        <!-- graph search -->
        <param name="base_global_planner" value="graph_planner/GraphPlanner"
            if="$(eval arg('global_planner')=='a_star'
                    or arg('global_planner')=='jps' 
                    or arg('global_planner')=='gbfs'
                    or arg('global_planner')=='dijkstra'
                    or arg('global_planner')=='d_star'
                    or arg('global_planner')=='lpa_star'
                    or arg('global_planner')=='voronoi'
                    or arg('global_planner')=='d_star_lite')" />
        <param name="GraphPlanner/planner_name" value="$(arg global_planner)"
            if="$(eval arg('global_planner')=='a_star'
                    or arg('global_planner')=='jps' 
                    or arg('global_planner')=='gbfs'
                    or arg('global_planner')=='dijkstra'
                    or arg('global_planner')=='d_star'
                    or arg('global_planner')=='lpa_star'
                    or arg('global_planner')=='voronoi'
                    or arg('global_planner')=='d_star_lite')" />
        <rosparam file="$(find sim_env)/config/planner/graph_planner_params.yaml" command="load"
            if="$(eval arg('global_planner')=='a_star'
                    or arg('global_planner')=='jps' 
                    or arg('global_planner')=='gbfs'
                    or arg('global_planner')=='dijkstra'
                    or arg('global_planner')=='d_star'
                    or arg('global_planner')=='lpa_star'
                    or arg('global_planner')=='voronoi'
                    or arg('global_planner')=='d_star_lite')"/>

        <!-- sample search -->
        <param name="base_global_planner" value="sample_planner/SamplePlanner"
            if="$(eval arg('global_planner')=='rrt'
                    or arg('global_planner')=='rrt_star'
                    or arg('global_planner')=='informed_rrt'
                    or arg('global_planner')=='rrt_connect')" />
        <param name="SamplePlanner/planner_name" value="$(arg global_planner)"
            if="$(eval arg('global_planner')=='rrt'
                    or arg('global_planner')=='rrt_star'
                    or arg('global_planner')=='informed_rrt'
                    or arg('global_planner')=='rrt_connect')" />
        <rosparam file="$(find sim_env)/config/planner/sample_planner_params.yaml" command="load" 
            if="$(eval arg('global_planner')=='rrt'
                    or arg('global_planner')=='rrt_star'
                    or arg('global_planner')=='informed_rrt'
                    or arg('global_planner')=='rrt_connect')"/>

        <!-- evolutionary search -->
        <param name="base_global_planner" value="evolutionary_planner/EvolutionaryPlanner"
            if="$(eval arg('global_planner')=='aco'
                    or arg('global_planner')=='ga')" />
        <param name="EvolutionaryPlanner/planner_name" value="$(arg global_planner)"
            if="$(eval arg('global_planner')=='aco'
                    or arg('global_planner')=='ga')" />
        <rosparam file="$(find sim_env)/config/planner/evolutionary_planner_params.yaml" command="load" 
            if="$(eval arg('global_planner')=='aco'
                    or arg('global_planner')=='ga')"/>

        <!-- local planner plugin -->
        <param name="base_local_planner" value="dwa_planner/DWAPlanner"
            if="$(eval arg('local_planner')=='dwa')" />
        <rosparam file="$(find sim_env)/config/planner/dwa_planner_params.yaml" command="load"
            if="$(eval arg('local_planner')=='dwa')" />

        <param name="base_local_planner" value="pid_planner/PIDPlanner"
            if="$(eval arg('local_planner')=='pid')" />
        <rosparam file="$(find sim_env)/config/planner/pid_planner_params.yaml" command="load"
            if="$(eval arg('local_planner')=='pid')" />

        <!-- loading navigation parameters -->
        <rosparam
            file="$(eval find('sim_env') + '/config/' + arg('robot') + '/costmap_common_params_' 
                + arg('robot') + '.yaml')"
            command="load" ns="global_costmap" />
        <rosparam
            file="$(eval find('sim_env') + '/config/' + arg('robot') + '/costmap_common_params_'
                + arg('robot') + '.yaml')"
            command="load" ns="local_costmap" />
        <rosparam file="$(find sim_env)/config/costmap/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find sim_env)/config/costmap/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find sim_env)/config/move_base_params.yaml" command="load" />

        <!-- set coordinate transformation namespace -->
        <param name="global_costmap/scan/sensor_frame" value="$(arg robot_namespace)/base_scan"
            if="$(arg start_ns)" />
        <param name="global_costmap/obstacle_layer/scan/sensor_frame"
            value="$(arg robot_namespace)/base_scan" if="$(arg start_ns)" />
        <param name="global_costmap/global_frame" value="map" />
        <param name="global_costmap/robot_base_frame" value="$(arg robot_namespace)/base_footprint"
            if="$(arg start_ns)" />

        <param name="local_costmap/scan/sensor_frame" value="$(arg robot_namespace)/base_scan"
            if="$(arg start_ns)" />
        <param name="local_costmap/obstacle_layer/scan/sensor_frame"
            value="$(arg robot_namespace)/base_scan" if="$(arg start_ns)" />
        <param name="local_costmap/global_frame" value="$(arg robot_namespace)/odom"
            if="$(arg start_ns)" />
        <param name="local_costmap/robot_base_frame" value="$(arg robot_namespace)/base_footprint"
            if="$(arg start_ns)" />

        <!-- centralize map -->
        <remap from="map" to="/map" />
    </node>
</launch>