Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
201 changes: 201 additions & 0 deletions submitted_models/explorer_ds1_sensor_config_1/urdf/model.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,201 @@
<?xml version='1.0' encoding='utf-8'?>
<robot name="ds1" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:arg name="name" default="ds1"/>
<xacro:property name="name" value="$(arg name)"/>

<link name='${name}/base_link'>
<inertial>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<mass value='1.5'/>
<inertia ixx='0.025' ixy='0' ixz='0' iyy='0.009' iyz='0' izz='0.033'/>
</inertial>
<collision name='base_link_inertia_collision'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/DS1_COLLISION.STL' scale='1 1 1'/>
</geometry>
</collision>

<visual name='base_link_inertia_visual'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/DS1.dae' scale='1 1 1'/>
</geometry>
</visual>
<!-- camera_visual -->
<visual name='rs_up_visual'>
<origin xyz='0.305 0 0.11' rpy='-1.57079632679 0 1.57079632679'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/realsense.dae' scale='1 1 1'/>
</geometry>
</visual>
<visual name='rs_down_visual'>
<origin xyz='0.305 0 0.03' rpy='1.57079632679 0 1.57079632679'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/realsense.dae' scale='1 1 1'/>
</geometry>
</visual>
<visual name='rs_front_visual'>
<origin xyz='0.325 0 0.07' rpy='0 -0 1.57079632679'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/realsense.dae' scale='1 1 1'/>
</geometry>
</visual>
<!-- Lidar visual -->
<visual name='laser_visual_base1'>
<origin xyz='0.18 0 0.19' rpy='0 -0 0'/>
<geometry>
<mesh filename="package://explorer_ds1_sensor_config_1/meshes/VLP16_base_1.dae" />
</geometry>
</visual>
<visual name='laser_visual_base2'>
<origin xyz='0.18 0 0.19' rpy='0 -0 0'/>
<geometry>
<mesh filename="package://explorer_ds1_sensor_config_1/meshes/VLP16_base_2.dae" />
</geometry>
</visual>
<visual name='laser_visual_scan'>
<origin xyz='0.18 0 0.19' rpy='0 -0 0'/>
<geometry>
<mesh filename="package://explorer_ds1_sensor_config_1/meshes/VLP16_scan.dae" />
</geometry>
</visual>
</link>

<sensor name='imu_sensor' update_rate='250'>
<parent link='${name}/base_link'/>
<origin xyz='0 0 0' rpy='0 -0 0'/>
</sensor>

<sensor name='air_pressure' update_rate='20'>
<parent link='${name}/base_link'/>
<origin xyz='0 0 0' rpy='0 -0 0'/>
</sensor>

<sensor name='magnetometer' update_rate='20'>
<parent link='${name}/base_link'/>
<origin xyz='0 0 0' rpy='0 -0 0'/>
</sensor>

<sensor name='rs_up' update_rate='20'>
<parent link='${name}/base_link'/>
<origin xyz='0.305 0 0.11' rpy='0 -1.57079632679 0'/>
<camera>
<image width='640' height='480' format='R8G8B8' hfov='1.0472' near='0.01' far='300' />
</camera>
</sensor>

<sensor name='rs_down' update_rate='20'>
<parent link='${name}/base_link'/>
<origin xyz='0.305 0 0.03' rpy='0 1.57079632679 0'/>
<camera>
<image width='640' height='480' format='R8G8B8' hfov='1.0472' near='0.01' far='300' />
</camera>
</sensor>

<sensor name='rs_front' update_rate='20'>
<parent link='${name}/base_link'/>
<origin xyz='0.325 0 0.07' rpy='0 -0 0'/>
<camera>
<image width='640' height='480' format='R8G8B8' hfov='1.0472' near='0.01' far='300' />
</camera>
</sensor>

<sensor name='front_laser' update_rate='15'>
<parent link='${name}/base_link'/>
<origin xyz='0.18 0 0.284' rpy='0 -0 0'/>
</sensor>

<link name='${name}/rotor_0'>
<inertial>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<mass value='0.005'/>
<inertia ixx='9.75e-07' ixy='0' ixz='0' iyy='4.17041e-05' iyz='0' izz='4.26041e-05'/>
</inertial>
<visual name='rotor_0_visual'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/propeller_ccw.dae' scale='0.12 0.12 0.12'/>
</geometry>
</visual>
</link>

<link name='${name}/rotor_1'>
<inertial>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<mass value='0.005'/>
<inertia ixx='9.75e-07' ixy='0' ixz='0' iyy='4.17041e-05' iyz='0' izz='4.26041e-05'/>
</inertial>
<visual name='rotor_1_visual'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/propeller_ccw.dae' scale='0.12 0.12 0.12'/>
</geometry>
</visual>
</link>

<link name='${name}/rotor_2'>
<inertial>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<mass value='0.005'/>
<inertia ixx='9.75e-07' ixy='0' ixz='0' iyy='4.17041e-05' iyz='0' izz='4.26041e-05'/>
</inertial>
<visual name='rotor_2_visual'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/propeller_cw.dae' scale='0.12 0.12 0.12'/>
</geometry>
</visual>
</link>

<link name='${name}/rotor_3'>
<inertial>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<mass value='0.005'/>
<inertia ixx='9.75e-07' ixy='0' ixz='0' iyy='4.17041e-05' iyz='0' izz='4.26041e-05'/>
</inertial>
<visual name='rotor_3_visual'>
<origin xyz='0 0 0' rpy='0 -0 0'/>
<geometry>
<mesh filename='package://explorer_ds1_sensor_config_1/meshes/propeller_cw.dae' scale='0.12 0.12 0.12'/>
</geometry>
</visual>
</link>

<joint name='rotor_0_joint' type='revolute'>
<origin xyz='0.1644 -0.166 0.023' rpy='0 -0 0'/>
<parent link='${name}/base_link'/>
<child link='${name}/rotor_0'/>
<dynamics damping='0' friction='0' />
<axis xyz='0 0 1'/>
<limit lower='-1e+16' upper='1e+16' effort='-1' velocity='-1'/>
</joint>

<joint name='rotor_1_joint' type='revolute'>
<origin xyz='-0.16443 0.1661 0.023' rpy='0 -0 0'/>
<parent link='${name}/base_link'/>
<child link='${name}/rotor_1'/>
<dynamics damping='0' friction='0' />
<axis xyz='0 0 1'/>
<limit lower='-1e+16' upper='1e+16' effort='-1' velocity='-1'/>
</joint>

<joint name='rotor_2_joint' type='revolute'>
<origin xyz='0.16443 0.1661 0.023' rpy='0 -0 0'/>
<parent link='${name}/base_link'/>
<child link='${name}/rotor_2'/>
<dynamics damping='0' friction='0' />
<axis xyz='0 0 1'/>
<limit lower='-1e+16' upper='1e+16' effort='-1' velocity='-1'/>
</joint>

<joint name='rotor_3_joint' type='revolute'>
<origin xyz='-0.16443 -0.1661 0.023' rpy='0 -0 0'/>
<parent link='${name}/base_link'/>
<child link='${name}/rotor_3'/>
<dynamics damping='0' friction='0' />
<axis xyz='0 0 1'/>
<limit lower='-1e+16' upper='1e+16' effort='-1' velocity='-1'/>
</joint>
</robot>
124 changes: 63 additions & 61 deletions submitted_models/robotika_x2_sensor_config_1/launch/spawner.rb
Original file line number Diff line number Diff line change
@@ -1,65 +1,67 @@
def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
"<spawn name=\"#{_name}\">\n"\
" <name>#{_name}</name>\n"\
" <allow_renaming>false</allow_renaming>\n"\
" <pose>#{_x} #{_y} #{_z+0.063494} #{_roll} #{_pitch} #{_yaw}</pose>\n"\
" <world>#{$worldName}</world>\n"\
" <is_performer>true</is_performer>\n"\
" <sdf version='1.6'>\n"\
" <include>\n"\
" <name>#{_name}</name>\n"\
" <uri>#{_modelURI}</uri>\n"\
" <!-- Diff drive -->\n"\
" <plugin filename=\"libignition-gazebo-diff-drive-system.so\"\n"\
" name=\"ignition::gazebo::systems::DiffDrive\">\n"\
" <left_joint>front_left_wheel_joint</left_joint>\n"\
" <left_joint>rear_left_wheel_joint</left_joint>\n"\
" <right_joint>front_right_wheel_joint</right_joint>\n"\
" <right_joint>rear_right_wheel_joint</right_joint>\n"\
" <wheel_separation>#{0.33559 * 1.23}</wheel_separation>\n"\
" <wheel_radius>0.098</wheel_radius>\n"\
" <topic>/model/#{_name}/cmd_vel_relay</topic>\n"\
" <min_velocity>-2</min_velocity>
" <max_velocity>2</max_velocity>
" <min_acceleration>-6</min_acceleration>
" <max_acceleration>6</max_acceleration>
" </plugin>\n"\
" <!-- Publish robot state information -->\n"\
" <plugin filename=\"libignition-gazebo-pose-publisher-system.so\"\n"\
" name=\"ignition::gazebo::systems::PosePublisher\">\n"\
" <publish_link_pose>true</publish_link_pose>\n"\
" <publish_sensor_pose>true</publish_sensor_pose>\n"\
" <publish_collision_pose>false</publish_collision_pose>\n"\
" <publish_visual_pose>false</publish_visual_pose>\n"\
" <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>\n"\
" <use_pose_vector_msg>true</use_pose_vector_msg>\n"\
" <static_publisher>true</static_publisher>\n"\
" <static_update_frequency>1</static_update_frequency>\n"\
" </plugin>\n"\
" <!-- Battery plugin -->\n"\
" <plugin filename=\"libignition-gazebo-linearbatteryplugin-system.so\"\n"\
" name=\"ignition::gazebo::systems::LinearBatteryPlugin\">\n"\
" <battery_name>linear_battery</battery_name>\n"\
" <voltage>12.694</voltage>\n"\
" <open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>\n"\
" <open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>\n"\
" <initial_charge>78.4</initial_charge>\n"\
" <capacity>78.4</capacity>\n"\
" <resistance>0.061523</resistance>\n"\
" <smooth_current_tau>1.9499</smooth_current_tau>\n"\
" <power_load>6.6</power_load>\n"\
" <start_on_motion>true</start_on_motion>\n"\
" </plugin>\n"\
" <!-- Gas Sensor plugin -->\n"\
" <plugin filename=\"libGasEmitterDetectorPlugin.so\"\n"\
" name=\"subt::GasDetector\">\n"\
" <topic>/model/#{_name}/gas_detected</topic>\n"\
" <update_rate>10</update_rate>\n"\
" <type>gas</type>\n"\
" </plugin>\n"\
" </include>\n"\
" </sdf>\n"\
"</spawn>\n"\
<<-HEREDOC
<spawn name='#{_name}'>
<name>#{_name}</name>
<allow_renaming>false</allow_renaming>
<pose>#{_x} #{_y} #{_z+0.063494} #{_roll} #{_pitch} #{_yaw}</pose>
<world>#{_worldName}</world>
<is_performer>true</is_performer>
<sdf version='1.6'>
<include>
<name>#{_name}</name>
<uri>#{_modelURI}</uri>
<!-- Diff drive -->
<plugin filename="libignition-gazebo-diff-drive-system.so"
name="ignition::gazebo::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.33559 * 1.23}</wheel_separation>
<wheel_radius>0.098</wheel_radius>
<topic>/model/#{_name}/cmd_vel_relay</topic>
<min_velocity>-2</min_velocity>
<max_velocity>2</max_velocity>
<min_acceleration>-6</min_acceleration>
<max_acceleration>6</max_acceleration>
</plugin>
<!-- Publish robot state information -->
<plugin filename="libignition-gazebo-pose-publisher-system.so"
name="ignition::gazebo::systems::PosePublisher">
<publish_link_pose>true</publish_link_pose>
<publish_sensor_pose>true</publish_sensor_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
<publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
<use_pose_vector_msg>true</use_pose_vector_msg>
<static_publisher>true</static_publisher>
<static_update_frequency>1</static_update_frequency>
</plugin>
<!-- Battery plugin -->
<plugin filename="libignition-gazebo-linearbatteryplugin-system.so"
name="ignition::gazebo::systems::LinearBatteryPlugin">
<battery_name>linear_battery</battery_name>
<voltage>12.694</voltage>
<open_circuit_voltage_constant_coef>12.694</open_circuit_voltage_constant_coef>
<open_circuit_voltage_linear_coef>-3.1424</open_circuit_voltage_linear_coef>
<initial_charge>78.4</initial_charge>
<capacity>78.4</capacity>
<resistance>0.061523</resistance>
<smooth_current_tau>1.9499</smooth_current_tau>
<power_load>6.6</power_load>
<start_on_motion>true</start_on_motion>
</plugin>
<!-- Gas Sensor plugin -->
<plugin filename="libGasEmitterDetectorPlugin.so"
name="subt::GasDetector">
<topic>/model/#{_name}/gas_detected</topic>
<update_rate>10</update_rate>
<type>gas</type>
</plugin>
</include>
</sdf>
</spawn>
HEREDOC
end

def rosExecutables(_name, _worldName)
Expand Down