<?xml version="1.0"?>

<launch>
  <arg name="scan_topic"           default="scan"/>
  <arg name="joy_config"           default="elecom_joy"/>
  <arg name="joy_dev"              default="/dev/input/js0"/>
  <arg name="waypoints_file"       default="$(find tsukuba2022)/config/waypoints/waypoints.yaml"/>
  <arg name="save_joy_button"      default="0"/>
  <arg name="cmd_vel"              default="/cmd_vel"/>
  <arg name="default_rad"          default="1.0"/>
  

  <!-- Node for saving waypoints -->
  <node name="waypoint_saver" pkg="waypoint_saver" type="waypoint_saver" output="screen">
    <param name="save_joy_button"       value="$(arg save_joy_button)"/>
    <param name="robot_frame"           value="base_footprint"/>
    <param name="filename"              value="$(arg waypoints_file)"/>
    <param name="default_rad"           value="$(arg default_rad)"/>
    
    <remap from="waypoints_joy"     to="joy"/>
    <remap from="waypoints_viz"     to="clicked_point"/>
    <remap from="finish_pose"       to="move_base_simple/goal"/>
  </node>
  

  <!-- Nodes for joystick operation of a differential two-wheeled robot -->
  <include file="$(find tsukuba2022)/launch/teleop_joy.launch">
    <arg name="joy_config" value="$(arg joy_config)"/>
    <arg name="joy_dev"    value="$(arg joy_dev)"/>
    <arg name="cmd_vel"    value="$(arg cmd_vel)"/>
  </include>
  

  <!-- The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 
       as a ROS node called slam_gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
    <remap from="scan" to="$(arg scan_topic)"/>
    <rosparam file="$(find tsukuba2022)/params/gmapping_common_params.yaml" command="load"/>
  </node>
  

  <!-- Node for rviz visualization -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find waypoint_saver)/rviz_cfg/record_waypoints.rviz"/>

</launch>
