<launch>
  <arg name="map_file"/>
  <arg name="use_map_server" default="true"/>
  <arg name="use_safety_limiter" default="false"/>
  <arg name="use_path_with_velocity" default="true"/>
  <arg name="simulate" default="false"/>
  <arg name="sim_robot_x" default="0.0"/>
  <arg name="sim_robot_y" default="0.0"/>
  <arg name="sim_robot_yaw" default="0.0"/>
  <arg name="cmd_vel_output" default="/cmd_vel"/>
  <arg name="vel" default="0.5"/>
  <arg name="acc" default="0.3"/>
  <arg name="ang_vel" default="0.6"/>
  <arg name="ang_acc" default="1.0"/>
  <arg name="look_forward" default="0.1"/>
  <arg name="curv_forward" default="0.2"/>
  <arg name="slow_and_precise" default="true"/>
  <arg name="linear_expand" default="0.08"/>
  <arg name="linear_spread" default="0.3"/>
  <arg name="output_info" default="screen"/>
  <arg name="robot_name" default="roisbot" />
  <arg name="robot_frame" default="default" />
  <arg name="use_odom" default="true"/>

  <param name="neonavigation_compatible" value="1"/>
  <rosparam command="load" file="$(find neonavigation_launch)/config/navigate.yaml"/>
  <node pkg="costmap_cspace" type="costmap_3d" name="costmap_3d" output="$(arg output_info)">
    <rosparam param="footprint" if="$(arg simulate)">
       [[0.35, -0.22], [0.35, 0.22], [-0.35, 0.22], [-0.35, -0.22]]
    </rosparam>
  </node>

  <node pkg="map_server" type="map_server" name="map_server" args="$(arg map_file)" if="$(arg use_map_server)"/>

  <node pkg="planner_cspace" type="planner_3d" name="planner_3d" output="$(arg output_info)">
    <param name="robot_frame" value="human1/base_footprint"/>
    <param name="use_path_with_velocity" value="$(arg use_path_with_velocity)"/>

  </node>


  <node pkg="trajectory_tracker" type="trajectory_tracker" name="spur">
    <remap from="cmd_vel" to="$(arg cmd_vel_output)" unless="$(arg use_safety_limiter)"/>
    <remap from="cmd_vel" to="cmd_vel_raw" if="$(arg use_safety_limiter)"/>
    <remap from="odom" to="human1/diff_drive_controller/odom" />
    <param name="max_vel" value="$(arg vel)"/>
    <param name="max_acc" value="$(arg acc)"/>
    <param name="max_angvel" value="$(arg ang_vel)"/>
    <param name="max_angacc" value="$(arg ang_acc)"/>
    <param name="curv_forward" value="$(arg curv_forward)"/>
    <param name="look_forward" value="$(arg look_forward)"/>
    <param name="rotate_ang" value="0.2" if="$(arg slow_and_precise)"/>
    <param name="rotate_ang" value="0.4" unless="$(arg slow_and_precise)"/>
    <param name="use_odom" value="$(arg use_odom)"/>
  </node>

  <node pkg="safety_limiter" type="safety_limiter" name="safety_limiter" if="$(arg use_safety_limiter)" output="screen">
    <remap from="cmd_vel_in" to="cmd_vel_raw"/>
    <remap from="cmd_vel" to="$(arg cmd_vel_output)"/>
    <rosparam param="footprint" if="$(arg simulate)">
    </rosparam>
    <param name="allow_empty_cloud" value="true"/>
    <param name="watchdog_interval" value="0.0"/>
  </node>
  <node pkg="planner_cspace" type="patrol" name="patrol"/>
  <group if="$(arg simulate)">
    <node pkg="tf2_ros" type="static_transform_publisher" name="stf1" args="$(arg sim_robot_x) $(arg sim_robot_y) 0 $(arg sim_robot_yaw) 0 0 map odom"/>
     <remap from="odom" to="human1/diff_drive_controller/odom"/>
  </group>

</launch>