Path tracer

submitted_models/ctu_cras_norlab_absolem_sensor_config_1/CMakeLists.txt

@@ -3,18 +3,18 @@ project(ctu_cras_norlab_absolem_sensor_config_1)
 
 set(CMAKE_CXX_STANDARD 17)
 
-find_package(ignition-gazebo2 REQUIRED)
+find_package(ignition-gazebo3 REQUIRED)
 find_package(ignition-common3 REQUIRED)
 
 find_package(catkin REQUIRED)
 
 catkin_package()
 
 add_library(laser_rotate_plugin src/laser_rotate_plugin.cpp)
-target_link_libraries(laser_rotate_plugin PRIVATE ignition-gazebo2::core ignition-common3::ignition-common3)
+target_link_libraries(laser_rotate_plugin PRIVATE ignition-gazebo3::core ignition-common3::ignition-common3)
 
 add_library(flipper_control_plugin src/flipper_control_plugin.cpp)
-target_link_libraries(flipper_control_plugin PRIVATE ignition-gazebo2::core ignition-common3::ignition-common3)
+target_link_libraries(flipper_control_plugin PRIVATE ignition-gazebo3::core ignition-common3::ignition-common3)
 
 install(TARGETS laser_rotate_plugin flipper_control_plugin
         ARCHIVE DESTINATION lib

subt_ign/CMakeLists.txt

@@ -178,6 +178,19 @@ install(TARGETS validate_connections
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
+add_executable(path_tracer
+  src/path_tracer.cc)
+target_link_libraries(path_tracer
+  PRIVATE
+    ignition-gazebo${IGN_GAZEBO_VER}::core
+    ${catkin_LIBRARIES} 
+    ${YAML_CPP_LIBRARIES}
+)
+install(TARGETS path_tracer
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
 
 # Create the libCommsBrokerPlugin.so library.
 set(comms_broker_plugin_name CommsBrokerPlugin)

subt_ign/launch/path_tracer.ign

@@ -0,0 +1,197 @@
+<?xml version="1.0"?>
+<!-- This launch file is used by CloudSim to run Ignition Gazebo and the
+     plugins needed by SubT. It does not run ROS. A separete launch file,
+     cloudsim_bridge.ign, starts a ROS master and the ROS1-Ignition bridge.
+
+     For more information, please refer to
+         https://github.com/osrf/subt/wiki/tutorials/cloudsim
+
+    Usage: ign launch cloudsim_sim.ign
+            [worldName:=<worldName>
+             robotName1:=<robotName> robotConfig1:=<robotConfig>
+             robotName2:=<robotName> robotConfig2:=<robotConfig>
+             ... ]
+
+    The [worldName] command line argument is optional,
+          defaults to tunnel_circuit_practice_01 if not specified
+    The [robotNameX] command line argument is optional, where X can be 1 to 20
+
+    Example that loads tunnel circuit practice world with an X2 robot with configuration 3
+
+      ign launch cloudsim_sim.ign worldName:=tunnel_circuit_practice_01
+          robotName1:=X2_3 robotConfig1:=X2_SENSOR_CONFIG_3
+-->
+
+<%
+  # Check if worldName is not defined or is empty/nil
+  if !defined?(worldName) || worldName == nil || worldName.empty?
+    $worldName = 'tunnel_circuit_practice_01'
+  else
+    $worldName = worldName
+  end
+
+  worldNumber = $worldName.split('_').last
+%>
+
+
+<ignition version='1.0'>
+  <env>
+    <name>IGN_GAZEBO_SYSTEM_PLUGIN_PATH</name>
+    <value>$LD_LIBRARY_PATH</value>
+  </env>
+
+  <plugin name="ignition::launch::GazeboServer"
+          filename="libignition-launch-gazebo.so">
+    <% if $worldName.include?('tunnel_circuit_') &&
+          !$worldName.include?('practice') %>
+      <world_file>tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
+    <% elsif $worldName.include?('urban_circuit_') &&
+          !$worldName.include?('practice') %>
+      <world_file>urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf</world_file>
+    <% else %>
+      <world_file><%= $worldName %>.sdf</world_file>
+    <% end %>
+   <%if defined?(updateRate) && updateRate != nil && !updateRate.empty?%>
+    <update_rate><%= updateRate %></update_rate>
+    <%end%>
+    <run>true</run>
+    <levels>false</levels>
+    <record>
+      <enabled>true</enabled>
+      <!-- This path is used by cloudsim, please do not change -->
+      <path>/tmp/ign/logs</path>
+    </record>
+    <%if defined?(seed) && seed != nil && !seed.empty?%>
+    <seed><%= seed %></seed>
+    <%end%>
+
+    <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>
+
+
+  <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>1.0 1.0 1.0</ambient_light>
+        <background_color>1.0 1.0 1.0</background_color>
+
+        <!-- Perspective -->
+        <!-- <camera_pose>-120 70 70 0 0.568 -1.0</camera_pose> -->
+
+        <!-- Top down -->
+        <camera_pose>-40 -80 190 0 1.5708 1.5708</camera_pose>
+      </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>
+      <!-- Entity tree -->
+      <!-- <plugin filename="EntityTree" name="Entity tree">
+        <ignition-gui>
+          <title>Entity tree</title>
+        </ignition-gui>
+      </plugin> -->
+
+      <!-- Transform Control -->
+      <!-- <plugin filename="TransformControl" name="Transform Control">
+        <service>/world/<%= $worldName %>/gui/transform_mode</service>
+      </plugin> -->
+    </plugin>
+  </executable_wrapper>
+
+</ignition>

subt_ign/src/GameLogicPlugin.cc

@@ -1193,14 +1193,6 @@ bool GameLogicPluginPrivate::OnNewArtifact(const subt::msgs::Artifact &_req,
     _resp.set_report_status("scored");
     this->totalScore += scoreDiff;
 
-    std::ostringstream stream;
-    stream
-      << "- event:\n"
-      << "  type: artifact_report_scored\n"
-      << "  time_sec: " << this->simTime.sec() << "\n"
-      << "  total_score: " << this->totalScore << std::endl;
-    this->LogEvent(stream.str());
-
     ignmsg << "Total score: " << this->totalScore << std::endl;
     this->Log() << "new_total_score " << this->totalScore << std::endl;
   }
@@ -1370,6 +1362,18 @@ double GameLogicPluginPrivate::ScoreArtifact(const ArtifactType &_type,
     }
   }
 
+  std::ostringstream stream;
+  stream
+    << "- event:\n"
+    << "  type: artifact_report_attempt\n"
+    << "  time_sec: " << this->simTime.sec() << "\n"
+    << "  reported_pose: " << observedObjectPose << "\n"
+    << "  artifact: " << std::get<0>(minDistance) << "\n"
+    << "  distance: " << std::get<2>(minDistance) << "\n"
+    << "  points_scored: " << score << "\n"
+    << "  total_score: " << this->totalScore + score << std::endl;
+  this->LogEvent(stream.str());
+
   this->Log() << "calculated_dist[" << std::get<2>(minDistance)
     << "] for artifact[" << std::get<0>(minDistance) << "] reported_pos["
     << observedObjectPose << "]" << std::endl;
@@ -1617,8 +1621,8 @@ void GameLogicPluginPrivate::Finish()
       << "- event:\n"
       << "  type: finished\n"
       << "  time_sec: " << this->simTime.sec() << "\n"
-      << "  elapsed_real_time " << realElapsed << "\n"
-      << "  elapsed_sim_time " << simElapsed << "\n"
+      << "  elapsed_real_time: " << realElapsed << "\n"
+      << "  elapsed_sim_time: " << simElapsed << "\n"
       << "  total_score: " << this->totalScore << std::endl;
     this->LogEvent(stream.str());
 

subt_ign/src/VisibilityPlugin.cc

@@ -86,7 +86,7 @@ void VisibilityPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/,
 //////////////////////////////////////////////////
 void VisibilityPlugin::PreUpdate(
     const ignition::gazebo::UpdateInfo &/*_info*/,
-    ignition::gazebo::EntityComponentManager &_ecm)
+    ignition::gazebo::EntityComponentManager &/*_ecm*/)
 {
   if (this->dataPtr->worldName.empty())
     return;
@@ -121,7 +121,7 @@ void VisibilityPlugin::PreUpdate(
 //////////////////////////////////////////////////
 void VisibilityPlugin::PostUpdate(
     const ignition::gazebo::UpdateInfo &_info,
-    const ignition::gazebo::EntityComponentManager &_ecm)
+    const ignition::gazebo::EntityComponentManager &/*_ecm*/)
 {
   if (this->dataPtr->worldName.empty())
     return;