<launch>
    <arg name="driver_baudrate" default="$(env PIBOT_DRIVER_BAUDRATE)"  doc="pibot driver baudrate"/>
    <arg name="use_mag" default="false"  doc="use mag in pibot imu"/>
    <arg name="use_madgwick" default="true"  doc="use madgwick filter in pibot imu"/>
    <arg name="use_odom" default="true"  doc="use odom in pibot bringup"/>


	<!-- PIBOT DRIVER -->
    <node name="pibot_driver" pkg="pibot_bringup" type="pibot_driver" output="screen">
        <param name="base_frame" value="base_link" />
        <param name="baudrate" value="$(arg driver_baudrate)" />
        <param name="cmd_vel_topic" value="cmd_vel" />
        <param name="odom_frame" value="odom" />
        <param name="odom_topic" value="wheel_odom" />
        <param name="out_pid_debug_enable" value="false" />
        <param name="port" value="/dev/pibot" />
        <!-- <param name="publish_tf" value="false" /> -->
         <param name="publish_tf" value="false" />
        <param name="freq" value="100" />
    </node>

	<!-- PIBOT IMU -->
    <node pkg="pibot_imu" type="pibot_imu" name="pibot_imu" output="screen" respawn="false">
        <rosparam>
            imu/accelerometer_bias: {x: 0.005436, y: 0.014684, z: -0.395418}
            imu/gyroscope_bias: {x: -0.035592, y: 0.080670, z: 0.001216}
        </rosparam>
        <param name="imu/use_mag_msg" value="$(arg use_mag)" />
        <param name="imu/perform_calibration" value="true" />
        <param name="imu/calibration_samples" value="200" />
    </node>


    <!-- <node pkg="pibot_bringup" type="publish_tf_node" name="publish_tf_node"/> -->

	<!-- TF IMU TO BASE -->
    <node pkg="tf" type="static_transform_publisher" name="base_imu_to_base_link"
        args="0 0.0 0 0 0.0  0.0 /base_link /imu_link 40" />  
<!-- 
    <node pkg="tf" type="static_transform_publisher" name="fix_map" args="0 0 0 0 0 0 /map /odom 50" /> -->
    <!-- <node pkg="tf" type="static_transform_publisher" name="fix_odom" args="0 0 0 0 0 0 /odom /base_link 50" /> -->
    <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /map /odom 1000"/>    

    <!-- use madgwick imu filter -->
    <node if    ="$(arg use_madgwick)" pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_madgwick" output="screen" respawn="false" >
        <param name="use_mag" value="$(arg use_mag)" />
        <param name="publish_tf" value="false" />
        <param name="world_frame" value="nwu" />
        <remap from="imu/data" to="imu" />
    </node>

    <!-- use complementary imu filter -->
    <node unless="$(arg use_madgwick)" pkg="imu_complementary_filter" type="complementary_filter_node" name="complementary_filter_gain_node" output="screen" respawn="false" >
        <param name="do_bias_estimation" value="true"/>
        <param name="do_adaptive_gain" value="true"/>
        <param name="use_mag" value="$(arg use_mag)"/>
        <param name="gain_acc" value="0.01"/>
        <param name="gain_mag" value="0.01"/>
        <param name="publish_debug_topics" value="false"/>
        <param name="publish_tf" value="false"/>
        <remap from="imu/data" to="imu" />
    </node>

    <group ns="/" if="$(arg use_odom)" >
        <!-- robot pose ekf -->
        <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
            <!-- <param name="output_frame" value="odom" /> -->
              <param name="output_frame" value="odom" />
            <param name="base_footprint_frame" value="base_link"/>
            <param name="freq" value="200.0"/>
            <param name="sensor_timeout" value="1.0"/>  
            <param name="odom_used" value="true"/>
            <param name="imu_used" value="true"/>
            <param name="vo_used" value="false"/>
            <param name="debug" value="false"/>
            <remap from="odom" to="wheel_odom" />
            <remap from="imu_data" to="imu" />
        </node>

        <!-- odom type adapter -->
        <node pkg="pibot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
            <remap from="input" to="robot_pose_ekf/odom_combined"/>
            <remap from="output" to="odom"/>
        </node>
    </group>
</launch>
