<launch>

    <!-- wpb_home core-->
    <node pkg="wpb_home_bringup" type="wpb_home_core" name="wpb_home_core" output="screen">
        <param name="serial_port" type="string" value="/dev/ftdi"/> 
        <rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
    </node>

    <!--- Run Rplidar -->
    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
        <param name="serial_port"         type="string" value="/dev/rplidar"/>  
        <param name="serial_baudrate"     type="int"    value="115200"/>
        <param name="frame_id"            type="string" value="laser"/>
        <param name="inverted"            type="bool"   value="false"/>
        <param name="angle_compensate"    type="bool"   value="true"/>
        <remap from="scan" to="scan_raw"/>
    </node>

    <!-- Run lidar filter -->
    <node pkg="wpb_home_bringup" type="wpb_home_lidar_filter" name="wpb_home_lidar_filter">
        <param name="pub_topic" value="/scan"/>
    </node>

    <!-- Run the map server -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find wpb_home_tutorials)/maps/map.yaml"/>

    <!-- Map tools  -->
    <node pkg="waterplus_map_tools" type="wp_manager" name="wp_manager" output="screen">
        <param name="load" type="string" value="/home/robot/waypoints.xml"/> 
    </node>

    <!--- Run AMCL -->
    <include file="$(find wpb_home_tutorials)/nav_lidar/amcl_omni.launch" />

    <!--- Run move base -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base"  output="screen">
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
        <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
        <param name="use_dijkstra" value="true"/>
        <param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />

        <param name= "controller_frequency" value="10" type="double"/>
    </node>

    <arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
    <arg name="gui" default="true" />

    <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
    <param name="use_gui" value="$(arg gui)"/>

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

    <!--- Run servers -->
    <include file="$(find wpb_home_behaviors)/launch/all_servers.launch" />
    
    <!--- speech recognition -->
    <include file="$(find xfyun_waterplus)/launch/iat_cn.launch" />
    
    <!-- Run xfyun TTS -->
    <node pkg="xfyun_waterplus" type="tts_node" name="xfyun_tts_node" /> 

    <!-- Run soundplay -->
    <node name="soundplay_node" pkg="sound_play" type="soundplay_node.py"/>

    <!--- Run Kinect -->
    <arg name="base_name"         default="kinect2"/>
    <arg name="sensor"            default=""/>
    <arg name="publish_tf"        default="false"/>
    <arg name="base_name_tf"      default="$(arg base_name)"/>
    <arg name="fps_limit"         default="-1.0"/>
    <arg name="calib_path"        default="$(find kinect2_bridge)/data/"/>
    <arg name="use_png"           default="false"/>
    <arg name="jpeg_quality"      default="90"/>
    <arg name="png_level"         default="1"/>
    <arg name="depth_method"      default="cpu"/>
    <arg name="depth_device"      default="-1"/>
    <arg name="reg_method"        default="cpu"/>
    <arg name="reg_device"        default="-1"/>
    <arg name="max_depth"         default="12.0"/>
    <arg name="min_depth"         default="0.1"/>
    <arg name="queue_size"        default="5"/>
    <arg name="bilateral_filter"  default="true"/>
    <arg name="edge_aware_filter" default="true"/>
    <arg name="worker_threads"    default="4"/>
    <arg name="machine"           default="localhost"/>
    <arg name="nodelet_manager"   default="$(arg base_name)"/>
    <arg name="start_manager"     default="true"/>
    <arg name="use_machine"       default="true"/>
    <arg name="respawn"           default="true"/>
    <arg name="use_nodelet"       default="true"/>
    <arg name="output"            default="log"/>

    <machine name="localhost" address="localhost" if="$(arg use_machine)"/>

    <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager"
        if="$(arg start_manager)" machine="$(arg machine)" />

    <!-- Nodelet version of kinect2_bridge -->
    <node pkg="nodelet" type="nodelet" name="$(arg base_name)_bridge" machine="$(arg machine)"
        args="load kinect2_bridge/kinect2_bridge_nodelet $(arg nodelet_manager)"
        respawn="$(arg respawn)" output="$(arg output)" if="$(arg use_nodelet)">
    <param name="base_name"         type="str"    value="$(arg base_name)"/>
    <param name="sensor"            type="str"    value="$(arg sensor)"/>
    <param name="publish_tf"        type="bool"   value="$(arg publish_tf)"/>
    <param name="base_name_tf"      type="str"    value="$(arg base_name_tf)"/>
    <param name="fps_limit"         type="double" value="$(arg fps_limit)"/>
    <param name="calib_path"        type="str"    value="$(arg calib_path)"/>
    <param name="use_png"           type="bool"   value="$(arg use_png)"/>
    <param name="jpeg_quality"      type="int"    value="$(arg jpeg_quality)"/>
    <param name="png_level"         type="int"    value="$(arg png_level)"/>
    <param name="depth_method"      type="str"    value="$(arg depth_method)"/>
    <param name="depth_device"      type="int"    value="$(arg depth_device)"/>
    <param name="reg_method"        type="str"    value="$(arg reg_method)"/>
    <param name="reg_device"        type="int"    value="$(arg reg_device)"/>
    <param name="max_depth"         type="double" value="$(arg max_depth)"/>
    <param name="min_depth"         type="double" value="$(arg min_depth)"/>
    <param name="queue_size"        type="int"    value="$(arg queue_size)"/>
    <param name="bilateral_filter"  type="bool"   value="$(arg bilateral_filter)"/>
    <param name="edge_aware_filter" type="bool"   value="$(arg edge_aware_filter)"/>
    <param name="worker_threads"    type="int"    value="$(arg worker_threads)"/>
    </node>

    <!-- Node version of kinect2_bridge -->
    <node pkg="kinect2_bridge" type="kinect2_bridge" name="$(arg base_name)_bridge" machine="$(arg machine)"
        respawn="$(arg respawn)" output="$(arg output)" unless="$(arg use_nodelet)">
    <param name="base_name"         type="str"    value="$(arg base_name)"/>
    <param name="sensor"            type="str"    value="$(arg sensor)"/>
    <param name="publish_tf"        type="bool"   value="$(arg publish_tf)"/>
    <param name="base_name_tf"      type="str"    value="$(arg base_name_tf)"/>
    <param name="fps_limit"         type="double" value="$(arg fps_limit)"/>
    <param name="calib_path"        type="str"    value="$(arg calib_path)"/>
    <param name="use_png"           type="bool"   value="$(arg use_png)"/>
    <param name="jpeg_quality"      type="int"    value="$(arg jpeg_quality)"/>
    <param name="png_level"         type="int"    value="$(arg png_level)"/>
    <param name="depth_method"      type="str"    value="$(arg depth_method)"/>
    <param name="depth_device"      type="int"    value="$(arg depth_device)"/>
    <param name="reg_method"        type="str"    value="$(arg reg_method)"/>
    <param name="reg_device"        type="int"    value="$(arg reg_device)"/>
    <param name="max_depth"         type="double" value="$(arg max_depth)"/>
    <param name="min_depth"         type="double" value="$(arg min_depth)"/>
    <param name="queue_size"        type="int"    value="$(arg queue_size)"/>
    <param name="bilateral_filter"  type="bool"   value="$(arg bilateral_filter)"/>
    <param name="edge_aware_filter" type="bool"   value="$(arg edge_aware_filter)"/>
    <param name="worker_threads"    type="int"    value="$(arg worker_threads)"/>
    </node>

    <!-- sd point cloud (512 x 424) -->
    <node pkg="nodelet" type="nodelet" name="$(arg base_name)_points_xyzrgb_sd" machine="$(arg machine)"
        args="load depth_image_proc/point_cloud_xyzrgb $(arg nodelet_manager)" respawn="$(arg respawn)">
    <remap from="rgb/camera_info"             to="$(arg base_name)/sd/camera_info"/>
    <remap from="rgb/image_rect_color"        to="$(arg base_name)/sd/image_color_rect"/>
    <remap from="depth_registered/image_rect" to="$(arg base_name)/sd/image_depth_rect"/>
    <remap from="depth_registered/points"     to="$(arg base_name)/sd/points"/>
    <param name="queue_size" type="int" value="$(arg queue_size)"/>
    </node>

    <!-- Run script -->
    <node pkg="wpb_home_apps" type="home_test" name="home_test" output="screen"/>

</launch>
