Model submission for Trailer sensor config 1 by CTU-CRAS-Norlab

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/.gitignore

@@ -0,0 +1,3 @@
+model.sdf.urdf
+meshes/*.blend
+meshes/*.blend1

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.10)
+project(ctu_cras_norlab_trailer_sensor_config_1)
+
+find_package(catkin REQUIRED roscpp subt_communication_broker subt_msgs subt_ros)
+
+catkin_package()
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(test_comms nodes/test_comms.cc)
+target_link_libraries(test_comms ${catkin_LIBRARIES})
+
+install(DIRECTORY launch meshes urdf worlds
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(FILES model.sdf model.config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(PROGRAMS
+  scripts/print_robot_urdf
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts)

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/README.md

@@ -0,0 +1,53 @@
+# CTU\_CRAS\_NORLAB\_TRAILER\_SENSOR\_CONFIG\_1
+
+This package contains sensor config 1 of CTU-CRAS-Norlab comms extension trailer.
+It is a passive robot that is tethered to the teambase via wired Ethernet. It is
+expected to be dragged by a UGV as a marsupial.
+
+The cable length is 100 m. When distance between the trailer and teambase is
+longer, they can only communicate via standard SubT comms model. If they are
+close enough, their communication has unlimited bandwidth and they are treated
+as if they were in the very same place.
+
+For the "wire-like" behavior, this robot's name has to be COMMS_EXTENDER and
+teambase's name has to be TEAMBASE.
+
+## Testing the communication
+
+To get better idea what's happening, open `subt_communication_broker/src/subt_communication_broker.cpp`,
+find function `DispatchMessages()` and scroll to a line reading `std::tie(sendPacket, rssi) =`.
+A little above it you'll find the logic that decides whether the communication should go
+"over the wire". And there are two debug printouts which might come handy when testing this
+feature. So uncomment them and rebuild your workspace.
+
+This package provides a program called `test_comms` that can be used to create a high amount of
+SubT comms traffic, on which it will be apparent that the comms going "over the wire" helps the
+resulting speed of transfer.
+
+Start simulator:
+
+    LANG=C ign launch -v4 competition.ign robotName1:=TEAMBASE robotConfig1:=TEAMBASE robotName2:=COMMS_EXTENDER robotConfig2:=X1_SENSOR_CONFIG_1 circuit:=cave worldName:=simple_cave_01
+
+Then open another terminal and start 
+
+    ROS_NAMESPACE=COMMS_EXTENDER ./test_comms
+
+And yet another terminal and start 
+
+    ROS_NAMESPACE=TEAMBASE ./test_comms _robot_name:=TEAMBASE _dst_name:=COMMS_EXTENDER
+
+Now you should see the nodes printing about statistics about the transferred data. And the terminal
+with simulator should show you the debug printouts. If you did not rebuild it with the debug
+printouts, you can still tell that the comms is going over the wire from the VisibilityRfTable debug
+printouts, where you'll see `Range: 0`. Now move the comms extender somewhere and watch for the
+distance. As soon as it gets 100 m from the teambase, comms start dropping in both signal quality and
+delivery success ratio.
+
+## Model files management
+
+This package follows a different workflow for managing SDF and URDF models of the robot than the suggested one. The only and main source of model data is `urdf/trailer.xacro` file and the files it includes. 
+
+To get the URDF model of the robot, call `scripts/print_robot_urdf` script which prints the robot URDF on stdout. This script is used in `launch/description.launch`.
+
+The SDF model is a regular file committed to this package, but its updates are not
+to be done manually. To change the robot model, make changes in the Xacro, and then run script `scripts/update_robot_sdf`, which updates the `model.sdf` file in this repo. The change can then be commited.

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/launch/description.launch

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="name" doc="Name of Vehicle"/>
+  <param name="$(arg name)/robot_description" command="$(dirname)/../scripts/print_robot_urdf name:=$(arg name)" />
+</launch>
+

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/launch/example.ign

@@ -0,0 +1,189 @@
+<?xml version="1.0"?>
+<!-- Usage: ign launch path/to/example.ign robotName:=<X1>
+
+   Parameters:
+    robotName: Name to be assigned to model
+-->
+
+<%
+  #require_relative 'spawner'
+  _base_dir = defined?(base_dir) ? base_dir.tr('"', '') : File.realpath(File.join(File.dirname(__FILE__), ".."))
+  load File.join(_base_dir, "launch", "spawner.rb")
+
+  # 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(_base_dir, 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>
+  <env>
+    <name>LC_ALL</name>
+    <value>C</value>
+  </env>
+
+  <!-- Start ROS first. This is a bit hacky for now. -->
+  <!-- Make sure to source /opt/ros/melodic/setup.bash -->
+<% if local_variables.include?(:ros) and ros %>
+  <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>
+<% end %>
+
+<% if local_variables.include?(:teleop) and teleop %>
+  <executable name='teleop'>
+    <command>roslaunch --wait ctu_cras_norlab_marv_sensor_config_1 teleop.launch</command>
+  </executable>
+<% end %>
+
+  <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>false</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%>
+
+<plugin name="ignition::launch::GazeboFactory"
+        filename="libignition-launch-gazebo-factory.so">
+<%= spawner(robotName, modelURI, $worldName, 0, 0, 0.2, 0, 0, 0) %>
+</plugin>
+
+<% if local_variables.include?(:ros) and ros %>
+  <%= rosExecutables(robotName, $worldName) %>
+<% end %>
+
+</ignition>
+

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/launch/spawner.rb

@@ -0,0 +1,40 @@
+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.2} #{_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>
+    </include>
+    </sdf>
+  </spawn>
+  HEREDOC
+end
+
+def rosExecutables(_name, _worldName)
+  <<-HEREDOC
+  <executable name='topics'>
+    <command>roslaunch --wait ctu_cras_norlab_trailer_sensor_config_1 vehicle_topics.launch world_name:=#{_worldName} name:=#{_name}</command>
+  </executable>
+  <executable name='description'>
+    <command>roslaunch --wait ctu_cras_norlab_trailer_sensor_config_1 description.launch name:=#{_name}</command>
+  </executable>
+  HEREDOC
+end

submitted_models/ctu_cras_norlab_trailer_sensor_config_1/launch/vehicle_topics.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<launch>
+  <env name="IGN_TRANSPORT_TOPIC_STATISTICS" value="1"/>
+
+  <arg name="world_name"/>
+  <arg name="name"/>
+
+  <!-- This is a passive vehicle without any sensors. -->
+
+  <group ns="$(arg name)">
+    <include file="$(find subt_ros)/launch/models_common/all_robots.launch">
+      <arg name="name" value="$(arg name)" />
+      <arg name="world_name" value="$(arg world_name)" />
+    </include>
+  </group>
+
+</launch>