<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="bcr_bot">

    <gazebo>
        <static>false</static>
    </gazebo>

    <!-- .....................MULTI WHEEL DIFF DRIVE ................................... -->

    <gazebo>
        <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
            <ros>
                <namespace>$(arg robot_namespace)</namespace>
                <remapping>cmd_vel:=cmd_vel</remapping>
                <remapping>odom:=odom</remapping>
            </ros>
            <legacy_mode>false</legacy_mode>
            <update_rate>50.0</update_rate>
            <left_joint>middle_left_wheel_joint</left_joint>
            <right_joint>middle_right_wheel_joint</right_joint>
            <wheel_separation>${traction_track_width+traction_wheel_width-0.01}</wheel_separation>
            <wheel_diameter>${2*traction_wheel_radius+0.01}</wheel_diameter>
            <robot_base_frame>base_footprint</robot_base_frame>
            <max_wheel_torque>${traction_max_wheel_torque}</max_wheel_torque>
            <command_topic>cmd_vel</command_topic>
            <odometry_topic>$(arg wheel_odom_topic)</odometry_topic>
            <odometry_frame>odom</odometry_frame>
            <publish_odom_tf>$(arg publish_wheel_odom_tf)</publish_odom_tf>
            <publish_wheel_tf>true</publish_wheel_tf>
            <publish_odom>true</publish_odom>
            <max_wheel_acceleration>5.0</max_wheel_acceleration>
            <!-- Odometry source, 0 for ENCODER, 1 for WORLD, defaults to WORLD -->
            <xacro:if value="${odometry_source == 'world'}">
                <odometry_source>1</odometry_source>
            </xacro:if>
            <xacro:if value="${odometry_source == 'encoder'}">
                <odometry_source>0</odometry_source>
            </xacro:if>
        </plugin>
    </gazebo>

    <!--............................... IMU PLUGIN ..................................... -->

    <gazebo reference="imu_frame">
        <sensor name="imu" type="imu">
            <always_on>true</always_on>
            <update_rate>5</update_rate>
            <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
                <ros>
                    <namespace>$(arg robot_namespace)</namespace>
                    <remapping>~/out:=imu</remapping>
                </ros>
            </plugin>
        </sensor>
    </gazebo>

    <!--............................... Ground truth PLUGIN .............................-->


    <gazebo>
        <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
            <ros>
                <!-- Add namespace and remap the default topic -->
                <namespace>$(arg robot_namespace)</namespace>
                <remapping>odom:=p3d_ground_truth</remapping>
            </ros>
            <always_on>true</always_on>
            <update_rate>30.0</update_rate>
            <body_name>base_footprint</body_name>
            <gaussian_noise>0.00</gaussian_noise>
            <frame_name>$(arg ground_truth_frame)</frame_name>
        </plugin>
    </gazebo>

    <!-- ........................... 2D LIDAR PLUGIN ................................... -->

    <xacro:if value="$(arg two_d_lidar_enabled)">

        <gazebo reference="two_d_lidar">
            <gravity>true</gravity>
            <sensor type="ray" name="two_d_lidar">
                <pose>0 0 0 0 0 0</pose>
                <visualize>false</visualize>
                <update_rate>${two_d_lidar_update_rate}</update_rate>
                <ray>
                    <scan>
                        <horizontal>
                            <samples>${two_d_lidar_sample_size}</samples>
                            <resolution>1</resolution>
                            <min_angle>${radians(two_d_lidar_min_angle)}</min_angle>
                            <max_angle>${radians(two_d_lidar_max_angle)}</max_angle>
                        </horizontal>
                    </scan>
                    <range>
                        <min>${two_d_lidar_min_range}</min>
                        <max>${two_d_lidar_max_range}</max>
                        <resolution>0.01</resolution>
                    </range>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.0</stddev>
                    </noise>
                </ray>
                <plugin name="gazebo_ros_laser" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <remapping>~/out:=$(arg robot_namespace)/scan</remapping>
                    </ros>
                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_name>two_d_lidar</frame_name>
                </plugin>
            </sensor>
        </gazebo>
    </xacro:if>

    <!-- ........................... CAMERA PLUGIN ................................... -->

    <xacro:if value="$(arg camera_enabled)">
        <gazebo reference="kinect_camera">
            <sensor name="kinect_camera" type="depth">
                <pose> 0 0 0 0 0 0 </pose>
                <visualize>true</visualize>
                <update_rate>30</update_rate>
                <camera>
                    <horizontal_fov>1.089</horizontal_fov>
                    <image>
                        <format>R8G8B8</format>
                        <width>640</width>
                        <height>480</height>
                    </image>
                    <clip>
                        <near>0.05</near>
                        <far>8.0</far>
                    </clip>
                </camera>
                <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
                    <ros>
                        <namespace>$(arg robot_namespace)</namespace>
                    </ros>
                    <frame_name>kinect_camera_optical</frame_name>
                    <min_depth>0.1</min_depth>
                    <max_depth>100</max_depth>
                </plugin>
            </sensor>
        </gazebo>
    </xacro:if>

    <!-- ...........................STEREO CAMERA PLUGIN ................................... -->

    <xacro:if value="$(arg stereo_camera_enabled)">
        <gazebo reference="stereo_camera">
            <sensor type="multicamera" name="stereo_camera">
                <update_rate>10.0</update_rate>
                <always_on>true</always_on>
                <camera name="left">
                    <pose>0 0 0 0 0 0</pose>
                    <horizontal_fov>1.3962634</horizontal_fov>
                    <image>
                        <width>1024</width>
                        <height>1024</height>
                        <format>R8G8B8</format>
                    </image>
                    <clip>
                        <near>0.3</near>
                        <far>20</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.0</stddev>
                    </noise>
                    <distortion>
                        <k1>0.0</k1>
                        <k2>0.0</k2>
                        <k3>0.0</k3>
                        <p1>0.0</p1>
                        <p2>0.0</p2>
                        <center>0.5 0.5</center>
                    </distortion>
                </camera>

                <camera name="right">
                    <pose>0 -0.12 0 0 0 0</pose>
                    <horizontal_fov>1.3962634</horizontal_fov>
                    <image>
                        <width>1024</width>
                        <height>1024</height>
                        <format>R8G8B8</format>
                    </image>
                    <clip>
                        <near>0.3</near>
                        <far>20</far>
                    </clip>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.0</stddev>
                    </noise>
                    <distortion>
                        <k1>0.0</k1>
                        <k2>0.0</k2>0
                        <k3>0.0</k3>
                        <p1>0.0</p1>
                        <p2>0.0</p2>
                        <center>0.5 0.5</center>
                    </distortion>
                </camera>

                <plugin name="stereo_camera_controller" filename="libgazebo_ros_camera.so">
                    <ros>
                        <namespace>$(arg robot_namespace)</namespace>
                        <argument>image_raw:=image_raw</argument>
                        <argument>camera_info:=camera_info</argument>
                    </ros>
                    <camera_name>stereo_camera</camera_name>
                    <frame_name>stereo_camera_optical</frame_name>
                    <hack_baseline>0.12</hack_baseline>
                </plugin>

            </sensor>
        </gazebo>
    </xacro:if>
</robot>