Coordinated Robotics Mark Sensor Config 1 (#1039)

* Add Coordinated Robotics Mark UAV * Removed alignment mark, adjust camera location * Updates based on reviews of other robots * Updates based on validation data and changes in other models * Added example rviz file. Co-authored-by: Kevin <[email protected]>
osrf · Nov 19, 2021 · e5b73be · e5b73be
1 parent 4acf3e9
commit e5b73be
Show file tree

Hide file tree

Showing 15 changed files with 2,799 additions and 0 deletions.
diff --git a/submitted_models/coro_mark_sensor_config_1/CMakeLists.txt b/submitted_models/coro_mark_sensor_config_1/CMakeLists.txt
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(coro_mark_sensor_config_1)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch meshes urdf 
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(FILES model.sdf model.config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
diff --git a/submitted_models/coro_mark_sensor_config_1/launch/description.launch b/submitted_models/coro_mark_sensor_config_1/launch/description.launch
@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="name" doc="Name of Vehicle"/>
+  <param name="$(arg name)/robot_description" command="$(find xacro)/xacro '$(find coro_mark_sensor_config_1)/urdf/model.xacro' name:=$(arg name)"/>
+</launch>
diff --git a/submitted_models/coro_mark_sensor_config_1/launch/example.ign b/submitted_models/coro_mark_sensor_config_1/launch/example.ign
@@ -0,0 +1,170 @@
+<?xml version="1.0"?>
+<!-- Usage: ign launch path/to/example.ign robotName:=<X1>
+
+   Parameters:
+    robotName: Name to be assigned to model
+-->
+
+<%
+  require_relative 'spawner'
+
+  # Modify these as needed
+  $enableGroundTruth = true
+  $headless =  local_variables.include?(:headless) ? :headless : false
+
+%>
+
+<%
+
+  unless local_variables.include?(:robotName)
+    raise "missing parameters. robotName is a required parameter"
+  end
+
+  # This assumes that this launch file is in a directory below the model
+  modelURI = File.expand_path("../", File.dirname(__FILE__))
+  $worldName = 'example'
+  worldFile = File.join(File.expand_path("../worlds", File.dirname(__FILE__)), "#{$worldName}.sdf")
+
+%>
+
+<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"%> robot_names:=<%=robotName%></command>
+  </executable>
+
+  <plugin name="ignition::launch::GazeboServer"
+          filename="libignition-launch-gazebo.so">
+    <world_file><%= worldFile %></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%>
+
+<%= spawner(robotName, modelURI, $worldName, 0, 0, 0, 0, 0, 0) %>
+<%= rosExecutables(robotName, $worldName) %>
+
+</ignition>
+
diff --git a/submitted_models/coro_mark_sensor_config_1/launch/spawner.rb b/submitted_models/coro_mark_sensor_config_1/launch/spawner.rb
@@ -0,0 +1,189 @@
+def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
+  <<-HEREDOC
+  <spawn name='#{_name}'>
+      <name>#{_name}</name>
+      <allow_renaming>false</allow_renaming>
+      <pose>#{_x} #{_y} #{_z + 0.111} #{_roll} #{_pitch} #{_yaw}</pose>
+      <world>#{_worldName}</world>
+      <is_performer>true</is_performer>
+      <sdf version='1.6'>
+      <include>
+      <name>#{_name}</name>
+      <uri>#{_modelURI}</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>#{$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>
+      <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>972.0</maxRotVelocity>
+        <motorConstant>3.41e-05</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>972.0</maxRotVelocity>
+        <motorConstant>3.41e-05</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>972.0</maxRotVelocity>
+        <motorConstant>3.41e-05</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>972.0</maxRotVelocity>
+        <motorConstant>3.41e-05</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>
+      <!-- MulticopterVelocityControl plugin -->
+      <plugin
+        filename="libignition-gazebo-multicopter-control-system.so"
+        name="ignition::gazebo::systems::MulticopterVelocityControl">
+        <robotNamespace>model/#{_name}</robotNamespace>
+        <commandSubTopic>cmd_vel</commandSubTopic>
+        <motorControlPubTopic>command/motor_speed</motorControlPubTopic>
+        <enableSubTopic>velocity_controller/enable</enableSubTopic>
+        <comLinkName>base_link</comLinkName>
+        <velocityGain>3.2 3.2 2.7</velocityGain>
+        <attitudeGain>2 3 0.2</attitudeGain>
+        <angularRateGain>0.4 0.52 0.2</angularRateGain>
+        <maximumLinearAcceleration>5.2 5.0 5.1</maximumLinearAcceleration>
+        <maximumLinearVelocity>7.7 5.8 5.4</maximumLinearVelocity>
+        <maximumAngularVelocity>3 3 3.2</maximumAngularVelocity>
+        <linearVelocityNoiseMean>0 0 0.05</linearVelocityNoiseMean>
+        <!-- linearVelocityNoiseStdDev based on error values reported in the paper Shen et. al., -->
+        <!-- Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor -->
+        <!-- http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.490.7958&rep=rep1&type=pdf -->
+        <linearVelocityNoiseStdDev>0.1105 0.1261 0.00947</linearVelocityNoiseStdDev>
+        <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
+        <!-- angularVelocityNoiseStdDev values based on ADIS16448's Rate Noise Density with a sample  -->
+        <!-- time of 0.004 ms. -->
+        <angularVelocityNoiseStdDev>0.004 0.004 0.004</angularVelocityNoiseStdDev>
+
+        <rotorConfiguration>
+          <rotor>
+            <jointName>rotor_0_joint</jointName>
+            <forceConstant>3.41e-05</forceConstant>
+            <momentConstant>0.016</momentConstant>
+            <direction>1</direction>
+          </rotor>
+          <rotor>
+            <jointName>rotor_1_joint</jointName>
+            <forceConstant>3.41e-05</forceConstant>
+            <momentConstant>0.016</momentConstant>
+            <direction>1</direction>
+          </rotor>
+          <rotor>
+            <jointName>rotor_2_joint</jointName>
+            <forceConstant>3.41e-05</forceConstant>
+            <momentConstant>0.016</momentConstant>
+            <direction>-1</direction>
+          </rotor>
+          <rotor>
+            <jointName>rotor_3_joint</jointName>
+            <forceConstant>3.41e-05</forceConstant>
+            <momentConstant>0.016</momentConstant>
+            <direction>-1</direction>
+          </rotor>
+        </rotorConfiguration>
+      </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>9.0</initial_charge>
+        <capacity>9.0</capacity>
+        <resistance>0.061523</resistance>
+        <smooth_current_tau>1.9499</smooth_current_tau>
+        <power_load>4.4</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)
+  <<-HEREDOC
+    <executable name='mark_description'>
+      <command>roslaunch --wait coro_mark_sensor_config_1 description.launch world_name:=#{_worldName} name:=#{_name}</command>
+    </executable>
+    <executable name='mark_ros_ign_bridge'>
+      <command>roslaunch --wait coro_mark_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name} uav:=1 laser_scan:=0 top_scan:=1 bottom_scan:=1 rgbd_cam:=1    </command>
+  </executable>
+  HEREDOC
+end