Update Install Scripts
This commit is contained in:
13
models/iris_with_ardupilot/model.config
Normal file
13
models/iris_with_ardupilot/model.config
Normal file
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Iris with ArduPilot</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.6">model.sdf</sdf>
|
||||
<author>
|
||||
<name>UAV-UGV Simulation</name>
|
||||
</author>
|
||||
<description>
|
||||
Iris quadcopter with ArduPilot SITL plugin for GPS-denied navigation testing.
|
||||
Includes forward and downward cameras for visual odometry.
|
||||
</description>
|
||||
</model>
|
||||
311
models/iris_with_ardupilot/model.sdf
Normal file
311
models/iris_with_ardupilot/model.sdf
Normal file
@@ -0,0 +1,311 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<model name="iris_with_ardupilot">
|
||||
<pose>0 0 0.194923 0 0 0</pose>
|
||||
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>1.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.029125</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.029125</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.055225</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
|
||||
<collision name="base_collision">
|
||||
<pose>0 0 -0.08 0 0 0</pose>
|
||||
<geometry>
|
||||
<box><size>0.47 0.47 0.11</size></box>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual name="base_visual">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>model://iris/meshes/iris.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<!-- Fallback visual if mesh not found -->
|
||||
<visual name="body_box">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box><size>0.3 0.3 0.1</size></box>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.2 0.2 0.8 1</ambient>
|
||||
<diffuse>0.2 0.2 0.8 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<!-- IMU Sensor -->
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>200</update_rate>
|
||||
<imu>
|
||||
<angular_velocity>
|
||||
<x><noise type="gaussian"><mean>0</mean><stddev>0.0003</stddev></noise></x>
|
||||
<y><noise type="gaussian"><mean>0</mean><stddev>0.0003</stddev></noise></y>
|
||||
<z><noise type="gaussian"><mean>0</mean><stddev>0.0003</stddev></noise></z>
|
||||
</angular_velocity>
|
||||
<linear_acceleration>
|
||||
<x><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></x>
|
||||
<y><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></y>
|
||||
<z><noise type="gaussian"><mean>0</mean><stddev>0.017</stddev></noise></z>
|
||||
</linear_acceleration>
|
||||
</imu>
|
||||
</sensor>
|
||||
</link>
|
||||
|
||||
<!-- Rotor 0 (Front Right CW) -->
|
||||
<link name="rotor_0">
|
||||
<pose>0.13 -0.22 0.023 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.025</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy><iyz>0</iyz><izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.1</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="rotor_0_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_0</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- Rotor 1 (Back Left CW) -->
|
||||
<link name="rotor_1">
|
||||
<pose>-0.13 0.22 0.023 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.025</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy><iyz>0</iyz><izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.1</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="rotor_1_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_1</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- Rotor 2 (Front Left CCW) -->
|
||||
<link name="rotor_2">
|
||||
<pose>0.13 0.22 0.023 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.025</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy><iyz>0</iyz><izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.1</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="rotor_2_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_2</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- Rotor 3 (Back Right CCW) -->
|
||||
<link name="rotor_3">
|
||||
<pose>-0.13 -0.22 0.023 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.025</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-06</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>0.000166704</iyy><iyz>0</iyz><izz>0.000167604</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry><cylinder><radius>0.1</radius><length>0.01</length></cylinder></geometry>
|
||||
<material><ambient>0.1 0.1 0.1 1</ambient></material>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name="rotor_3_joint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>rotor_3</child>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit><lower>-1e16</lower><upper>1e16</upper></limit>
|
||||
</axis>
|
||||
</joint>
|
||||
|
||||
<!-- Forward Camera for Visual Odometry -->
|
||||
<link name="forward_camera_link">
|
||||
<pose>0.1 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>1e-5</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>1e-5</iyy><iyz>0</iyz><izz>1e-5</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<geometry><box><size>0.02 0.02 0.02</size></box></geometry>
|
||||
<material><ambient>0 0 0 1</ambient></material>
|
||||
</visual>
|
||||
<sensor name="forward_camera" type="camera">
|
||||
<camera>
|
||||
<horizontal_fov>1.57</horizontal_fov>
|
||||
<image>
|
||||
<width>640</width>
|
||||
<height>480</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip><near>0.1</near><far>100</far></clip>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
<namespace>/uav</namespace>
|
||||
<remapping>image_raw:=camera/forward/image_raw</remapping>
|
||||
<remapping>camera_info:=camera/forward/camera_info</remapping>
|
||||
</ros>
|
||||
<camera_name>forward_camera</camera_name>
|
||||
<frame_name>forward_camera_link</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name="forward_camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>forward_camera_link</child>
|
||||
</joint>
|
||||
|
||||
<!-- Downward Camera for Optical Flow -->
|
||||
<link name="downward_camera_link">
|
||||
<pose>0 0 -0.05 0 1.5708 0</pose>
|
||||
<inertial>
|
||||
<mass>0.01</mass>
|
||||
<inertia>
|
||||
<ixx>1e-5</ixx><ixy>0</ixy><ixz>0</ixz>
|
||||
<iyy>1e-5</iyy><iyz>0</iyz><izz>1e-5</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<sensor name="downward_camera" type="camera">
|
||||
<camera>
|
||||
<horizontal_fov>1.2</horizontal_fov>
|
||||
<image>
|
||||
<width>320</width>
|
||||
<height>240</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip><near>0.1</near><far>50</far></clip>
|
||||
</camera>
|
||||
<always_on>1</always_on>
|
||||
<update_rate>60</update_rate>
|
||||
<visualize>false</visualize>
|
||||
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
|
||||
<ros>
|
||||
<namespace>/uav</namespace>
|
||||
<remapping>image_raw:=camera/downward/image_raw</remapping>
|
||||
<remapping>camera_info:=camera/downward/camera_info</remapping>
|
||||
</ros>
|
||||
<camera_name>downward_camera</camera_name>
|
||||
<frame_name>downward_camera_link</frame_name>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</link>
|
||||
<joint name="downward_camera_joint" type="fixed">
|
||||
<parent>base_link</parent>
|
||||
<child>downward_camera_link</child>
|
||||
</joint>
|
||||
|
||||
<!-- ArduPilot Plugin -->
|
||||
<plugin name="ardupilot_plugin" filename="libArduPilotPlugin.so">
|
||||
<fdm_addr>127.0.0.1</fdm_addr>
|
||||
<fdm_port_in>9002</fdm_port_in>
|
||||
<fdm_port_out>9003</fdm_port_out>
|
||||
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
|
||||
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
|
||||
<imuName>iris_with_ardupilot::base_link::imu_sensor</imuName>
|
||||
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
|
||||
|
||||
<!-- Motor configuration -->
|
||||
<control channel="0">
|
||||
<jointName>rotor_0_joint</jointName>
|
||||
<type>VELOCITY</type>
|
||||
<offset>0</offset>
|
||||
<p_gain>0.20</p_gain>
|
||||
<i_gain>0</i_gain>
|
||||
<d_gain>0</d_gain>
|
||||
<i_max>0</i_max>
|
||||
<i_min>0</i_min>
|
||||
<cmd_max>2.5</cmd_max>
|
||||
<cmd_min>-2.5</cmd_min>
|
||||
<multiplier>838</multiplier>
|
||||
</control>
|
||||
<control channel="1">
|
||||
<jointName>rotor_1_joint</jointName>
|
||||
<type>VELOCITY</type>
|
||||
<offset>0</offset>
|
||||
<p_gain>0.20</p_gain>
|
||||
<i_gain>0</i_gain>
|
||||
<d_gain>0</d_gain>
|
||||
<i_max>0</i_max>
|
||||
<i_min>0</i_min>
|
||||
<cmd_max>2.5</cmd_max>
|
||||
<cmd_min>-2.5</cmd_min>
|
||||
<multiplier>838</multiplier>
|
||||
</control>
|
||||
<control channel="2">
|
||||
<jointName>rotor_2_joint</jointName>
|
||||
<type>VELOCITY</type>
|
||||
<offset>0</offset>
|
||||
<p_gain>0.20</p_gain>
|
||||
<i_gain>0</i_gain>
|
||||
<d_gain>0</d_gain>
|
||||
<i_max>0</i_max>
|
||||
<i_min>0</i_min>
|
||||
<cmd_max>2.5</cmd_max>
|
||||
<cmd_min>-2.5</cmd_min>
|
||||
<multiplier>-838</multiplier>
|
||||
</control>
|
||||
<control channel="3">
|
||||
<jointName>rotor_3_joint</jointName>
|
||||
<type>VELOCITY</type>
|
||||
<offset>0</offset>
|
||||
<p_gain>0.20</p_gain>
|
||||
<i_gain>0</i_gain>
|
||||
<d_gain>0</d_gain>
|
||||
<i_max>0</i_max>
|
||||
<i_min>0</i_min>
|
||||
<cmd_max>2.5</cmd_max>
|
||||
<cmd_min>-2.5</cmd_min>
|
||||
<multiplier>-838</multiplier>
|
||||
</control>
|
||||
</plugin>
|
||||
</model>
|
||||
</sdf>
|
||||
Reference in New Issue
Block a user