<?xml version='1.0'?>
<sdf version='1.6'>
  <model name='harvester_robot'>
    <pose>0 0 3.3 0 0 0</pose> 
    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine> 
    </plugin>

    <link name='h_base_link'>
      <inertial>
        <mass>150.0</mass> 
        <inertia>
          <ixx>128.5</ixx><ixy>0</ixy><ixz>0</ixz>
          <iyy>200.5</iyy><iyz>0</iyz>
          <izz>328.0</izz> 
        </inertia>
      </inertial>
      <collision name='collision_left_side'>
        <pose>0 1.4 0 0 0 0</pose> <geometry><box><size>4.0 0.4 0.2</size></box></geometry> 
      </collision>
      <collision name='collision_right_side'>
        <pose>0 -1.4 0 0 0 0</pose> <geometry><box><size>4.0 0.4 0.2</size></box></geometry> 
      </collision>
      <collision name='collision_top_plate'>
        <pose>0 0 0.5 0 0 0</pose> <geometry><box><size>4.0 2.4 0.1</size></box></geometry> 
      </collision>
      <visual name='base_visual'>
        <geometry><box><size>4.0 3.2 0.2</size></box></geometry> <material><ambient>0.1 0.3 0.6 1</ambient><diffuse>0.1 0.3 0.6 1</diffuse></material> 
      </visual>
    </link>

    <link name='axle_fl'><pose>1.8 1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
    <link name='axle_fr'><pose>1.8 -1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
    <link name='axle_bl'><pose>-1.8 1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>
    <link name='axle_br'><pose>-1.8 -1.4 -1.6 0 0 0</pose> <visual name='visual'><geometry><box><size>0.2 0.2 3.0</size></box></geometry><material><ambient>0.2 0.2 0.2 1</ambient></material></visual> </link>

    <joint name='steer_fl' type='fixed'><parent>h_base_link</parent><child>axle_fl</child></joint>
    <joint name='steer_fr' type='fixed'><parent>h_base_link</parent><child>axle_fr</child></joint>
    <joint name='steer_bl' type='fixed'><parent>h_base_link</parent><child>axle_bl</child></joint>
    <joint name='steer_br' type='fixed'><parent>h_base_link</parent><child>axle_br</child></joint>

    <link name='wheel_fl'>
      <pose>1.8 1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
      </collision>
      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
    </link>

    <link name='wheel_fr'>
      <pose>1.8 -1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
      </collision>
      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
    </link>

    <link name='wheel_bl'>
      <pose>-1.8 1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
      </collision>
      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
    </link>

    <link name='wheel_br'>
      <pose>-1.8 -1.4 -2.9 1.5707 0 0</pose> <inertial><mass>10.0</mass><inertia><ixx>0.1</ixx><ixy>0</ixy><ixz>0</ixz><iyy>0.1</iyy><iyz>0</iyz><izz>0.1</izz></inertia></inertial>
      <collision name='collision'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry>
        <surface><friction><ode><mu>1.0</mu><mu2>0.4</mu2><fdir1>1 0 0</fdir1><slip1>0.0</slip1><slip2>0.0</slip2></ode></friction><contact><ode><kp>1000000.0</kp><kd>100.0</kd><min_depth>0.001</min_depth></ode></contact></surface>
      </collision>
      <visual name='visual'><geometry><cylinder><radius>0.4</radius><length>0.1</length></cylinder></geometry><material><ambient>0 0 0 1</ambient></material></visual>
    </link>

    <link name="lidar_left_link">
      <pose>-1.0 0.5 -2.4 0 0 -0.785398</pose>
      <inertial><mass>0.1</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial>
      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry><material><ambient>0 1 0 1</ambient></material></visual>
      <sensor name='gpu_lidar_l' type='gpu_lidar'>
        <pose>0 0 0 0 0 0</pose>
        <frame_id>lidar_left_link</frame_id>
        <topic>lidar_left</topic>
        <update_rate>10</update_rate>
        <lidar>
          <scan>
            <horizontal><samples>180</samples><resolution>1</resolution><min_angle>-0.785398</min_angle><max_angle>0.785398</max_angle></horizontal>
          </scan>
          <range><min>0.08</min><max>10</max><resolution>0.01</resolution></range>
        </lidar>
      </sensor>
    </link>

    <link name="lidar_right_link">
      <pose>-1.0 -0.5 -2.4 0 0 0.785398</pose>
      <inertial><mass>0.1</mass><inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia></inertial>
      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry><material><ambient>1 0.5 0 1</ambient></material></visual>
      <sensor name='gpu_lidar_r' type='gpu_lidar'>
        <pose>0 0 0 0 0 0</pose>
        <frame_id>lidar_right_link</frame_id>
        <topic>lidar_right</topic>
        <update_rate>10</update_rate>
        <lidar>
          <scan>
            <horizontal><samples>180</samples><resolution>1</resolution><min_angle>-0.785398</min_angle><max_angle>0.785398</max_angle></horizontal>
          </scan>
          <range><min>0.08</min><max>10</max><resolution>0.01</resolution></range>
        </lidar>
      </sensor>
    </link>

    <link name="camera_link">
      <pose>1.95 0 -1 0 0.15 0</pose> <inertial><mass>0.1</mass> <inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia> </inertial>
      <visual name="visual"><geometry><box><size>0.1 0.1 0.1</size></box></geometry> <material><ambient>0.1 0.1 0.1 1</ambient></material> </visual>
      <sensor name="camera" type="camera">
        <camera><horizontal_fov>1.047</horizontal_fov> <image><width>1920</width><height>1080</height></image> <clip><near>0.1</near><far>100</far></clip> </camera>
        <always_on>1</always_on> <update_rate>30</update_rate> <visualize>true</visualize> <topic>camera</topic> 
      </sensor>
    </link>

    <link name="gps_link">
      <pose>0 0 0.2 0 0 0</pose> <inertial><mass>0.1</mass> <inertia><ixx>0.0001</ixx><iyy>0.0001</iyy><izz>0.0001</izz></inertia> </inertial>
      <visual name="visual"><geometry><cylinder><radius>0.1</radius><length>0.05</length></cylinder></geometry> <material><ambient>0.8 0.1 0.1 1</ambient></material> </visual>
      <sensor name="navsat" type="navsat"><always_on>1</always_on> <update_rate>10</update_rate> <topic>gps</topic> </sensor>
    </link>

    <joint name="lidar_left_joint" type="fixed"><parent>h_base_link</parent><child>lidar_left_link</child></joint>
    <joint name="lidar_right_joint" type="fixed"><parent>h_base_link</parent><child>lidar_right_link</child></joint>
    <joint name="gps_joint" type="fixed"><parent>h_base_link</parent><child>gps_link</child></joint> 
    <joint name="camera_joint" type="fixed"><parent>h_base_link</parent><child>camera_link</child></joint> 

    <joint name='wheel_fl_joint' type='revolute'><parent>axle_fl</parent><child>wheel_fl</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
    <joint name='wheel_fr_joint' type='revolute'><parent>axle_fr</parent><child>wheel_fr</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
    <joint name='wheel_bl_joint' type='revolute'><parent>axle_bl</parent><child>wheel_bl</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
    <joint name='wheel_br_joint' type='revolute'><parent>axle_br</parent><child>wheel_br</child><axis><xyz expressed_in='__model__'>0 1 0</xyz><limit><effort>10000</effort></limit></axis></joint>
  
    <include>
      <uri>file:///home/marcin/arm_ws/src/robotarm_description/urdf/robot_arm.urdf</uri>
      <name>arm</name>
      <pose>0 0 -0.1 3.14159 0 0</pose> 
    </include>

    <joint name="chassis_to_arm_joint" type="fixed">
      <!-- <pose relative_to="h_base_link">0 0 -0.2 0 0 0</pose> -->
      <parent>h_base_link</parent>
      <child>arm::base_link</child>
    </joint>

    <plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
      <left_joint>wheel_fl_joint</left_joint><left_joint>wheel_bl_joint</left_joint>
      <right_joint>wheel_fr_joint</right_joint><right_joint>wheel_br_joint</right_joint>
      <wheel_separation>2.8</wheel_separation> <wheel_radius>0.4</wheel_radius> <max_wheel_torque>500.0</max_wheel_torque>
      <max_linear_acceleration>5.0</max_linear_acceleration><max_angular_acceleration>5.0</max_angular_acceleration>
      <topic>/cmd_vel</topic> 
    </plugin>

  </model>
</sdf>