preview.ign

<?xml version="1.0"?>
<!-- Usage: ign launch preview.ign modelPath:=<path/to/model/dir> modelName:=<X1> modelType:=[UGV]

   Parameters:
    modelPath: Path to model directory
    modelName: Name to be assigned to model
    modelType(optional): Vehicle type: UGV or UAV
-->

<%
  # Modify these as needed
  $enableGroundTruth = true
  $headless = false
%>

<%
  # Handle command line arguments
  unless local_variables.include?(:modelPath)
    raise "missing parameters. modelPath is a required parameter"
  end

  unless local_variables.include?(:modelName)
    raise "missing parameters. modelName is a required parameter"
  end

  modelType = 'UGV' if modelType.nil?

  unless ['UGV', 'UAV'].include?(modelType)
    raise "invalid parameter. modelType can only be either 'UGV' or 'UAV'"
  end

  $worldName = 'preview'

%>

<ignition version='1.0'>
  <env>
    <name>IGN_GAZEBO_SYSTEM_PLUGIN_PATH</name>
    <value>$LD_LIBRARY_PATH</value>
  </env>

  <!-- Start ROS first. This is a bit hacky for now. -->
  <!-- Make sure to source /opt/ros/melodic/setup.bash -->
  <executable name='ros'>
    <command>roslaunch subt_ros competition_init.launch world_name:=<%=$worldName%> vehicle_topics:=0 enable_ground_truth:=<%=($enableGroundTruth)?"1":"0"%></command>
  </executable>

  <plugin name="ignition::launch::GazeboServer"
          filename="libignition-launch-gazebo.so">
    <world_file><%= $worldName %>.sdf</world_file>
    <run>true</run>
    <levels>false</levels>
    <record>
      <enabled>false</enabled>
    </record>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-physics-system.so"
            name="ignition::gazebo::systems::Physics">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-sensors-system.so"
            name="ignition::gazebo::systems::Sensors">
            <render_engine>ogre2</render_engine>
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-user-commands-system.so"
            name="ignition::gazebo::systems::UserCommands">
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-scene-broadcaster-system.so"
            name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>
    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-imu-system.so"
            name="ignition::gazebo::systems::Imu">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-magnetometer-system.so"
            name="ignition::gazebo::systems::Magnetometer">
    </plugin>

    <plugin entity_name="<%= $worldName %>"
            entity_type="world"
            filename="libignition-gazebo-air-pressure-system.so"
            name="ignition::gazebo::systems::AirPressure">
    </plugin>
  </plugin>

  <%if !$headless %>
  <executable_wrapper>
    <plugin name="ignition::launch::GazeboGui"
          filename="libignition-launch-gazebogui.so">
      <world_name><%= $worldName %></world_name>
      <window_title>SubT Simulator</window_title>
      <window_icon><%= ENV['SUBT_IMAGES_PATH'] %>/SubT_logo.svg</window_icon>
      <plugin filename="GzScene3D" name="3D View">
        <ignition-gui>
          <title>3D View</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="string" key="state">docked</property>
        </ignition-gui>

        <engine>ogre2</engine>
        <scene>scene</scene>
        <ambient_light>0.2 0.2 0.1</ambient_light>
        <background_color>0.8 0.8 0.8</background_color>
        <camera_pose>-6.3 -4.2 3.6 0 0.268 0.304</camera_pose>
        <service>/world/<%= $worldName %>/scene/info</service>
        <pose_topic>/world/<%= $worldName %>/pose/info</pose_topic>
        <scene_topic>/world/<%= $worldName %>/scene/info</scene_topic>
        <deletion_topic>/world/<%= $worldName %>/scene/deletion</deletion_topic>
      </plugin>
      <plugin filename="WorldControl" name="World control">
        <ignition-gui>
          <title>World control</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">72</property>
          <property type="double" key="width">121</property>
          <property type="double" key="z">1</property>

          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="left" target="left"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </ignition-gui>

        <play_pause>true</play_pause>
        <step>true</step>
        <start_paused>true</start_paused>
        <service>/world/<%= $worldName %>/control</service>
        <stats_topic>/world/<%= $worldName %>/stats</stats_topic>

      </plugin>

      <plugin filename="WorldStats" name="World stats">
        <ignition-gui>
          <title>World stats</title>
          <property type="bool" key="showTitleBar">false</property>
          <property type="bool" key="resizable">false</property>
          <property type="double" key="height">110</property>
          <property type="double" key="width">290</property>
          <property type="double" key="z">1</property>

          <property type="string" key="state">floating</property>
          <anchors target="3D View">
            <line own="right" target="right"/>
            <line own="bottom" target="bottom"/>
          </anchors>
        </ignition-gui>

        <sim_time>true</sim_time>
        <real_time>true</real_time>
        <real_time_factor>true</real_time_factor>
        <iterations>true</iterations>
        <topic>/world/<%= $worldName %>/stats</topic>
      </plugin>
    </plugin>
  </executable_wrapper>
  <%end%>

<%
  def spawnPreviewUGV(_path, _name, _x, _y)
    uav=0
    # Enable all sensors even though the vehicle may not have all these sensors. 
    # This will result in some topics being listed in `rostopic list` without any
    # data being published on them.
    rgbdCam=1
    stereoCam=1
    laserScan=1
    lidar3d=1
    output = <<-SDF
    <plugin name="ignition::launch::GazeboFactory"
          filename="libignition-launch-gazebo-factory.so">
      <name>#{_name}</name>
      <allow_renaming>false</allow_renaming>
      <pose>#{_x} #{_y} .2 0 0 0</pose>
      <world>#{$worldName}</world>
      <is_performer>true</is_performer>
      <sdf version='1.6'>
        <include>
          <name>#{_name}</name>
          <uri>#{_path}</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.45649 * 1.5}</wheel_separation>
            <wheel_radius>0.1651</wheel_radius>
            <topic>/model/#{_name}/cmd_vel_relay</topic>
          </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>false</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>
          <plugin filename="libignition-gazebo-pose-publisher-system.so"
            name="ignition::gazebo::systems::PosePublisher">
            <publish_link_pose>false</publish_link_pose>
            <publish_sensor_pose>false</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>false</static_publisher>
          </plugin>
          <!-- Battery plugin -->
        </include>
      </sdf>
    </plugin>
    <executable name='x1_description'>
      <command>roslaunch --wait subt_ros x1_description.launch world_name:=#{$worldName} name:=#{_name}</command>
    </executable>
    <executable name='x1_ros_ign_bridge'>
      <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} lidar_3d:=#{lidar3d}</command>
    </executable>
    SDF
  end


  def spawnPreviewUAV(_path, _name, _x, _y)
    # Assumes 4 rotors. Modify as necessary to match the number of rotors in your model

    # Enable all sensors even though the vehicle may not have all these sensors. 
    # This will result in some topics being listed in `rostopic list` without any
    # data being published on them.
    uav=1
    rgbdCam=1
    stereoCam=1
    laserScan=1
    lidar3d=1
    output = <<-SDF
    <plugin name="ignition::launch::GazeboFactory"
            filename="libignition-launch-gazebo-factory.so">
      <name>#{_name}</name>
      <allow_renaming>false</allow_renaming>
      <pose>#{_x} #{_y} 0.2 0 0 0</pose>
      <world>#{$worldName}</world>
      <is_performer>true</is_performer>
      <sdf version='1.6'>
        <include>
          <name>#{_name}</name>
          <uri>file://#{_path}</uri>
          <!-- 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>false</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>
          <plugin filename="libignition-gazebo-pose-publisher-system.so"
            name="ignition::gazebo::systems::PosePublisher">
            <publish_link_pose>false</publish_link_pose>
            <publish_sensor_pose>false</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>false</static_publisher>
          </plugin>
          <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/#{_name}</robotNamespace>
            <jointName>rotor_0_joint</jointName>
            <linkName>rotor_0</linkName>
            <turningDirection>ccw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>0</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
          </plugin>
          <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/#{_name}</robotNamespace>
            <jointName>rotor_1_joint</jointName>
            <linkName>rotor_1</linkName>
            <turningDirection>ccw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>1</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
          </plugin>
          <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/#{_name}</robotNamespace>
            <jointName>rotor_2_joint</jointName>
            <linkName>rotor_2</linkName>
            <turningDirection>cw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>2</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
          </plugin>
          <plugin filename="libignition-gazebo-multicopter-motor-model-system.so"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/#{_name}</robotNamespace>
            <jointName>rotor_3_joint</jointName>
            <linkName>rotor_3</linkName>
            <turningDirection>cw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>3</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
          </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>18.0</initial_charge>
            <capacity>18.0</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>
        </include>
      </sdf>
    </plugin>
    <executable name='x3_description'>
      <command>roslaunch --wait subt_ros x3_description.launch world_name:=#{$worldName} name:=#{_name}</command>
    </executable>
    <executable name='X3_controller'>
      <command>roslaunch --wait x3_control control.launch world_name:=#{$worldName} x3_name:=#{_name}</command>
    </executable>
    <executable name='x3_ros_ign_bridge'>
      <command>roslaunch --wait subt_ros vehicle_topics.launch world_name:=#{$worldName} name:=#{_name} uav:=#{uav} laser_scan:=#{laserScan} stereo_cam:=#{stereoCam} rgbd_cam:=#{rgbdCam}</command>
    </executable>
    SDF
  end
%>

<%
    modelFullPath = File.expand_path(modelPath)
    if modelType == 'UGV'
      spawner = method(:spawnPreviewUGV)
    else
      spawner = method(:spawnPreviewUAV)
    end
%>
<%= spawner.call(modelFullPath, modelName, 0, 0) %>


</ignition>