<?xml version="1.0"?>
<launch>
  <arg name="tf_prefix" default="" />

  <node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link" />
  <node pkg="tf2_ros" type="static_transform_publisher" name="imu_link_to_base_link" args="0 0 0.45 0 0 0 base_link imu_link" />
  <node pkg="tf2_ros" type="static_transform_publisher" name="surface_to_base_link" args="0 0 0.9 0 0 0 base_link surface" />


  <include file="$(find sick_line_guidance)/launch/sick_line_guidance.launch">
    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
  </include>
  <include file="$(find mlse_tf_base_link)/launch/msle_tf_base_link.launch">
    <arg name="yaml"     value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
  </include>

  <node pkg="diff_wheel_plugin"   type="diff_wheel_feedback" name="diff_wheel_feedback" output="screen"> 
    <rosparam file="$(find amr_startup)/config/plc_config.yaml" command="load" /> 
    <param name="start_bit"     value="111"/>
    <param name="end_bit"       value="114"/>
  </node>

  <node pkg="diff_wheel_plugin" type="diff_wheel_controller" name="diff_wheel_controller" output="screen">
    <rosparam file="$(find amr_startup)/config/motorInfomation.yaml" command="load" /> 
    <param name="port_name" type="str" value="/dev/ttyTHS0"/> 
  </node>

  <arg name="frame_id" default="imu_frame"/>
  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg frame_id)_to_imu_link" args="0 0 0 0 0 0 imu_link $(arg frame_id)" />
  <node pkg="wit_wt901ble_reader" name="wit_wt901ble_reader_node" type="wit_wt901ble_reader_node" output="screen">
    <param name="baudrate" type="int" value="115200"/>
    <param name="portname" type="string" value="/dev/USB_IMU"/>
    <param name="topic_name" type="string" value="/imu_data"/>
    <param name="frame_id" type="string" value="$(arg frame_id)"/>
    <param name="imu_diagnostics_topic_name" type="string" value="/imu_diagnostics"/>
  </node>
  
  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen"> 
    <rosparam file="$(find amr_startup)/config/ros_diff_drive_controller.yaml" command="load" />
    <param name="use_encoder" type="bool" value="false"/>
    <param name="type" type="int" value="2"/>
    <remap from="/ros_kinematics/odom" to="/odom" />
  </node>

  <!-- <include file="$(find manual_psx)/launch/ps4.launch"></include> -->

  <!-- <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
    <rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
  </node> -->

  <!-- Load URDF -->
  <include file="$(find robot_description)/launch/upload_urdf.launch">
      <arg name="robot_type" value="imr" />
      <arg name="tf_prefix" value="$(arg tf_prefix)" />
  </include>

  <!-- driver -->
  <arg name="pkg" value="olelidar"/>
  
  <arg name="driver" default="true"/>
  <arg name="f_device_ip" default="192.168.2.101"/>
  <arg name="f_device_port" default="2368"/>
  <arg name="local_ip" default="192.168.2.100"/>
  <arg name="multiaddr" default=""/>
  
  <!-- decoder -->
  <arg name="f_frame_id" default="front_laser_link"/>
  <arg name="f_topic_name" default="/f_scan"/>
  <arg name="f_r_max" default="25"/>
  <arg name="f_ang_start" default="-90"/>
  <arg name="f_ang_end" default="90"/>
  <arg name="decoder" default="true"/>
  <arg name="inverted" default="false"/>
  <arg name="f_debug" default="false"/>
  <env if="$(arg f_debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>

  <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg f_frame_id)_to_base_link" args="0.432 0 0.135 0 0 0 base_link $(arg f_frame_id)" />

  <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
    <param name="frame_id" type="string" value="$(arg f_frame_id)"/>
    <param name="r_max" type="int" value="$(arg f_r_max)"/>
    <param name="ang_start" type="int" value="$(arg f_ang_start)"/>
    <param name="ang_end" type="int" value="$(arg f_ang_end)"/>
    <param name="inverted" type="bool" value="$(arg inverted)"/>
    <param name="device_ip" type="string" value="$(arg f_device_ip)"/>
    <param name="device_port" type="int" value="$(arg f_device_port)"/>
    <param name="local_ip" type="string" value="$(arg local_ip)"/>
    <param name="multiaddr" type="string" value="$(arg multiaddr)"/>

    <remap from="~packet" to="packet"/>
    <remap from="~scan" to="$(arg f_topic_name)"/>
  </node>

  <node pkg="topic_tools" type="relay" name="f_scan_relay" args="$(arg f_topic_name) scan"/>

  <include file="$(find amr_startup)/launch/amr_control.launch"></include>

  <!-- <include file="$(find amr_startup)/launch/includes/rviz.launch"></include> -->

  <!-- <include file="$(find slam_toolbox)/launch/offline.launch"></include> -->

</launch>  