<?xml version="1.0"?>
<launch>
    <!-- The robot URDF model and world file -->
    <arg name="model"            default="$(find xacro)/xacro '$(find tsukuba2022)/urdf/orange2022.xacro'" />
    <arg name="calibration"      default="$(find tsukuba2022)/params/VLP16db.yaml"/>
    <arg name="scan_dev"         default="/dev/sensors/hokuyo_urg"/>
    <arg name="imu_dev"          default="/dev/sensors/imu"/>
    <arg name="motor_driver_dev" default="/dev/ZLAC8015D"/>
    <arg name="estop_dev"        default="/dev/E-Stop"/>
    <arg name="debug"            default="false"/>
    <arg name="cmd_vel"          default="/cmd_vel"/>

    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
    <param name="robot_description" command="$(arg model)"/>
    
    <!-- Node to setting up Arduino serial communication for IMU sensor -->
    <node pkg="rosserial_python" type="serial_node.py" name="serial_node_IMU">
      <param name="port" value="$(arg imu_dev)"/>
      <param name="baud" value="115200"/>
    </node>
    
    
    <!-- Node for setting up 2D-LiDAR via USB connection -->
    <node pkg="urg_node" type="urg_node" name="urg_node">
      <param name="frame_id"                  value="/hokuyo_link"/>
      <param name="serial_port"               value="$(arg scan_dev)"/>
      <param name="angle_min"   type="double" value="-1.22"/>
      <param name="angle_max"   type="double" value="1.22"/>
      <remap from="/scan"       to="/hokuyo_scan"/>
    </node>

    <!-- Run velodyne_pointcloud/TransformNodelet in a nodelet manager for a VLP-16 -->
    <!-- Start nodelet manager -->
    <node pkg="nodelet" type="nodelet" name="velodyne_link_nodelet_manager" args="manager" output="screen"/>
    
    <!-- Load driver nodelet into it -->
    <node pkg="nodelet" type="nodelet" name="velodyne_link_nodelet_manager_driver" args="load velodyne_driver/DriverNodelet velodyne_link_nodelet_manager">
      <param name="device_ip"              value=""/>
      <param name="frame_id"               value="velodyne_link"/>
      <param name="model"                  value="VLP16"/>
      <param name="pcap"                   value=""/>
      <param name="port"                   value="2368"/>
      <param name="read_fast"              value="false"/>
      <param name="read_once"              value="false"/>
      <param name="repeat_delay"           value="0.0"/>
      <param name="rpm"                    value="600.0"/>
      <param name="gps_time"               value="false"/>
      <param name="pcap_time"              value="false"/>
      <param name="cut_angle"              value="-0.01"/>
      <param name="timestamp_first_packet" value="false"/>
    </node>
    
    <!-- Start transform nodelet -->
    <node pkg="nodelet" type="nodelet" name="velodyne_link_nodelet_manager_transform" args="load velodyne_pointcloud/TransformNodelet velodyne_link_nodelet_manager">
      <param name="model"          value="VLP16"/>
      <param name="calibration"    value="$(arg calibration)"/>
      <param name="fixed_frame"    value=""/>
      <param name="target_frame"   value=""/>
      <param name="max_range"      value="130.0"/>
      <param name="min_range"      value="0.4"/>
      <param name="organize_cloud" value="false"/>
    </node>

    <!-- Ground segmentation -->
    <node name="ground_segmentation" pkg="linefit_ground_segmentation_ros" type="ground_segmentation_node">
        <rosparam command="load" file="$(find tsukuba2022)/params/segmentation_params.yaml"/>
        <param name="input_topic"           value="/velodyne_points"/>
        <param name="ground_output_topic"   value="/ground_cloud"/>
        <param name="obstacle_output_topic" value="obstacle_cloud"/>
    </node>

    <!-- Convert 3d pointcloud to 2d laserscan -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
        <!--<remap from="cloud_in" to="velodyne_points"/>-->
        <remap from="cloud_in" to="ground_segmentation/obstacle_cloud"/>
        <remap from="scan"     to="velodyne_scan"/>
        <rosparam command="load" file="$(find tsukuba2022)/params/pointcloud_to_laserscan_params.yaml"/>
    </node>

    <!-- Sensor fusion -->
    <node name="combine_dr_measurements" pkg="robot_pose_ekf" type="robot_pose_ekf">
        <remap from="odom"     to="/odom"/>
        <remap from="imu_data" to="imu"/>
        <param name="freq"                 value="30.0"/>
        <param name="sensor_timeout"       value="1.0"/>
        <param name="publish_tf"           value="true"/>
        <param name="odom_used"            value="true"/>
        <param name="imu_used"             value="true"/>
        <param name="vo_used"              value="false"/>
        <param name="output_frame"         value="odom"/>
        <param name="base_footprint_frame" value="base_footprint"/>
    </node>
    
    <!-- Load joint controller configurations from YAML file to parameter server -->
    <!--<rosparam file="$(find tsukuba2022)/config/orange2022_control.yaml" command="load"/>-->

    <!-- Node for loading the controllers -->
    <!-- publish /orange2022/odom and /orange2022/cmd_vel -->
    <!--<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller diff_drive_controller"/>-->

    <!-- Convert joint states to TF transforms for rviz, etc -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"/>
    
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
        <param name="use_gui" value="true"/>
        <param name="rate"    value="50"/>
    </node>
    
    <!-- Start motor driver -->
    <include file="$(find zlac8015d_ros)/launch/motor_driver_node.launch">
      <arg name="port"                value="$(arg motor_driver_dev)"/>
      <arg name="estop_port"          value="$(arg estop_dev)"/>
      <!--# Control mode
      # 1: relative position control mode
      # Default: Subscribe to "/zlac8015d/pos/cmd_deg" and "/zlac8015d/pos/cmd_dist"
      # 3: Speed rpm control mode
      # Default: Subscribe to "/zlac8015d/twist/cmd_vel", "/zlac8015d/vel/cmd_vel" and "/zlac8015d/vel/cmd_rpm"-->
      <arg name="control_mode"        value="3"/>
      <arg name="twist_cmd_vel_topic" value="$(arg cmd_vel)"/>
      <arg name="debug"               value="$(arg debug)"/>
      <arg name="publish_TF"          value="false"/>
      <arg name="TF_child_frame"      value="base_footprint"/>
      <arg name="publish_odom"        value="true"/>
      <arg name="odom_child_frame"    value="base_footprint"/>
    </include>
    
    
    <!-- Merge hokuyo_scan & scan from velodyne pointcloud -->
    <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="screen">
      <param name="destination_frame" value="velodyne_link"/>
      <param name="scan_destination_topic" value="/scan"/>
      <param name="laserscan_topics" value ="/velodyne_scan /hokuyo_scan" /> <!-- LIST OF THE LASER SCAN TOPICS TO SUBSCRIBE -->
      <param name="angle_min" value="-2.3565"/>
      <param name="angle_max" value="2.03565"/>
      <param name="angle_increment" value="0.008711645"/>
      <param name="scan_time" value="0.0333333"/>
      <param name="range_min" value="0.30"/>
      <param name="range_max" value="50.0"/>
    </node>
</launch>
