<launch>
  <arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
  <arg name="local_planner"   default="pnkx"  doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
  <arg name="global_planner"  default="custom" doc="Global planner can be either sbpl two_points custom"/>
  <arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path or vda5050_msgs::Order"/> 
  <arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim"/>
  <arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
  <arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->

  <param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
  <rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
  <node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
    <!-- robot type -->
    <rosparam param="footprint" if="$(eval robot_type == 'imr')">
      [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
    </rosparam>
    <!-- Footprint cho imr -->
    <rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
      [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
    </rosparam>

    <rosparam param="footprint" if="$(eval robot_type == 'T800')">
      [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
    </rosparam>

    <param name="primitive_filename" value="$(arg primitive_filename)" />
    <param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
    
    <rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
    <rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
    <rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
    <rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />

    <!-- global costmap params -->
    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
    <rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
    <rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />

    <!-- local costmap params -->
    <rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
    <rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
    <rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />

    <rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
    
    <remap from="map" to="/map" />
    <remap from="marker" to="move_base_node/markers" />
    <!-- <remap from="cmd_vel" to="/cmd_vel_1" /> -->
  </node>
</launch>
