<launch>

  <!-- important generally, but specifically utilised by the current app manager -->
  <param name="robot/name" value="$(optenv ROBOT turtlebot)"/>
  <param name="robot/type" value="turtlebot"/>


  <param name="robot_description" command="cat $(find igibson-ros)/turtlebot/turtlebot.urdf"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
  </node>

  <node name="turtlebot_gibson_sim" pkg="igibson-ros" type="turtlebot_rgbd.py" output="screen">
  </node>
  
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="5.0" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find igibson-ros)/rviz/navigation.rviz" />

  <node pkg="tf" type="static_transform_publisher" name="gibson_world_transform" args="0 0 0 0 0 0 /world /map 100"/>
  <node pkg="igibson-ros" type="publish_true_map_to_odom_tf.py" name="map_odom_tf"/>

  <!--
  <node name="gibson_sim_noise" pkg="igibson-ros" type="noise_injection.py">
  </node>
  -->

  <!-- Velocity muxer -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
        args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml" />
    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
    <remap from="cmd_vel_mux/input/navi" to="/navigation_velocity_smoother/raw_cmd_vel" />
  </node>

 <include file="$(find turtlebot_teleop)/launch/ps3_teleop.launch">
 </include>

  <arg name="map_file" default="$(find igibson-ros)/map/graphnav/area_1.yaml"/>
  <rosparam param="map_file" subst_value="True">$(arg map_file)</rosparam>
  <node pkg="map_server" type="map_server"  name="map_server" args="$(arg map_file)" />

  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="map"/>
  <arg name="odom_topic" default="odom" />
  <arg name="laser_topic" default="gibson_ros/laserscan" />
  <arg name="custom_param_file" default="$(find igibson-ros)/param/dummy.yaml"/>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find igibson-ros)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find igibson-ros)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find igibson-ros)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find igibson-ros)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find igibson-ros)/param/gt_navigation/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find igibson-ros)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find igibson-ros)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find igibson-ros)/param/navfn_global_planner_params.yaml" command="load" />
    <!-- external params file that could be loaded into the move_base namespace -->
    <rosparam file="$(arg custom_param_file)" command="load" />

    <!-- reset frame_id parameters using user input data -->
    <param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
    <param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
    <param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
    <param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
  </node>

  <node pkg="nodelet" type="nodelet" args="manager"
        name="record_player_manager" output="screen"/>

</launch>
