204 lines
7.0 KiB
XML
204 lines
7.0 KiB
XML
<?xml version="1.0" ?>
|
|
<sdf version="1.9">
|
|
<model name="custom_ugv">
|
|
<static>false</static>
|
|
|
|
<link name="base_link">
|
|
<pose>0 0 0.12 0 0 0</pose>
|
|
<inertial>
|
|
<mass>80.0</mass>
|
|
<inertia>
|
|
<ixx>3.0</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>4.0</iyy><iyz>0</iyz><izz>3.0</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
|
|
<surface>
|
|
<friction>
|
|
<ode><mu>10.0</mu><mu2>10.0</mu2></ode>
|
|
</friction>
|
|
</surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry><box><size>0.8 0.6 0.12</size></box></geometry>
|
|
<material><ambient>0.2 0.2 0.7 1</ambient><diffuse>0.2 0.2 0.8 1</diffuse></material>
|
|
</visual>
|
|
</link>
|
|
|
|
<link name="front_left_wheel">
|
|
<pose>0.25 0.35 0.08 -1.5708 0 0</pose>
|
|
<inertial>
|
|
<mass>2.0</mass>
|
|
<inertia>
|
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
|
</visual>
|
|
</link>
|
|
|
|
<link name="front_right_wheel">
|
|
<pose>0.25 -0.35 0.08 -1.5708 0 0</pose>
|
|
<inertial>
|
|
<mass>2.0</mass>
|
|
<inertia>
|
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
|
</visual>
|
|
</link>
|
|
|
|
<link name="rear_left_wheel">
|
|
<pose>-0.25 0.35 0.08 -1.5708 0 0</pose>
|
|
<inertial>
|
|
<mass>2.0</mass>
|
|
<inertia>
|
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
|
</visual>
|
|
</link>
|
|
|
|
<link name="rear_right_wheel">
|
|
<pose>-0.25 -0.35 0.08 -1.5708 0 0</pose>
|
|
<inertial>
|
|
<mass>2.0</mass>
|
|
<inertia>
|
|
<ixx>0.005</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>0.005</iyy><iyz>0</iyz><izz>0.008</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<collision name="collision">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<surface><friction><ode><mu>5.0</mu><mu2>5.0</mu2></ode></friction></surface>
|
|
</collision>
|
|
<visual name="visual">
|
|
<geometry><cylinder><radius>0.08</radius><length>0.05</length></cylinder></geometry>
|
|
<material><ambient>0.15 0.15 0.15 1</ambient></material>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- ArUco landing tag (ID 0) on top -->
|
|
<link name="aruco_tag">
|
|
<pose>0 0 0.185 0 0 0</pose>
|
|
<visual name="aruco_visual">
|
|
<geometry><box><size>0.5 0.5 0.005</size></box></geometry>
|
|
<material>
|
|
<ambient>1 1 1 1</ambient>
|
|
<diffuse>1 1 1 1</diffuse>
|
|
<pbr><metal><albedo_map>tags/land_tag.png</albedo_map></metal></pbr>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- Forward-facing camera for obstacle detection -->
|
|
<link name="front_camera_link">
|
|
<pose>0.42 0 0.22 0 0 0</pose>
|
|
<inertial>
|
|
<mass>0.05</mass>
|
|
<inertia>
|
|
<ixx>0.00001</ixx><ixy>0</ixy><ixz>0</ixz>
|
|
<iyy>0.00001</iyy><iyz>0</iyz><izz>0.00001</izz>
|
|
</inertia>
|
|
</inertial>
|
|
<visual name="camera_visual">
|
|
<geometry><box><size>0.03 0.03 0.03</size></box></geometry>
|
|
<material>
|
|
<ambient>0.1 0.1 0.1 1</ambient>
|
|
<diffuse>0.1 0.1 0.1 1</diffuse>
|
|
</material>
|
|
</visual>
|
|
<sensor name="front_camera" type="camera">
|
|
<always_on>1</always_on>
|
|
<update_rate>10</update_rate>
|
|
<visualize>1</visualize>
|
|
<topic>/ugv/camera/forward</topic>
|
|
<camera>
|
|
<horizontal_fov>1.3962634</horizontal_fov>
|
|
<image>
|
|
<width>640</width>
|
|
<height>480</height>
|
|
<format>R8G8B8</format>
|
|
</image>
|
|
<clip>
|
|
<near>0.1</near>
|
|
<far>50</far>
|
|
</clip>
|
|
</camera>
|
|
</sensor>
|
|
</link>
|
|
|
|
<joint name="aruco_joint" type="fixed">
|
|
<parent>base_link</parent>
|
|
<child>aruco_tag</child>
|
|
</joint>
|
|
|
|
<joint name="front_camera_joint" type="fixed">
|
|
<parent>base_link</parent>
|
|
<child>front_camera_link</child>
|
|
</joint>
|
|
|
|
<joint name="front_left_wheel_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>front_left_wheel</child>
|
|
<axis><xyz>0 0 1</xyz></axis>
|
|
</joint>
|
|
<joint name="front_right_wheel_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>front_right_wheel</child>
|
|
<axis><xyz>0 0 1</xyz></axis>
|
|
</joint>
|
|
<joint name="rear_left_wheel_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rear_left_wheel</child>
|
|
<axis><xyz>0 0 1</xyz></axis>
|
|
</joint>
|
|
<joint name="rear_right_wheel_joint" type="revolute">
|
|
<parent>base_link</parent>
|
|
<child>rear_right_wheel</child>
|
|
<axis><xyz>0 0 1</xyz></axis>
|
|
</joint>
|
|
|
|
<plugin filename="gz-sim-diff-drive-system"
|
|
name="gz::sim::systems::DiffDrive">
|
|
<left_joint>front_left_wheel_joint</left_joint>
|
|
<left_joint>rear_left_wheel_joint</left_joint>
|
|
<right_joint>front_right_wheel_joint</right_joint>
|
|
<right_joint>rear_right_wheel_joint</right_joint>
|
|
<wheel_separation>0.70</wheel_separation>
|
|
<wheel_radius>0.08</wheel_radius>
|
|
<max_linear_acceleration>1.0</max_linear_acceleration>
|
|
<min_linear_acceleration>-1.0</min_linear_acceleration>
|
|
<max_angular_acceleration>2.0</max_angular_acceleration>
|
|
<min_angular_acceleration>-2.0</min_angular_acceleration>
|
|
<topic>/ugv/cmd_vel</topic>
|
|
<odom_topic>/ugv/odometry</odom_topic>
|
|
<odom_publisher_frequency>20</odom_publisher_frequency>
|
|
</plugin>
|
|
</model>
|
|
</sdf>
|