Distance segfault

docker/cloudsim_sim/Dockerfile

@@ -107,12 +107,6 @@ WORKDIR /home/$USERNAME
 
 RUN rosdep update
 
-# Clone all the subt models so that you don't download them every time
-# docker is run
-RUN mkdir -p subt_ws/src \
- && cd subt_ws/src \
- && git clone https://github.com/osrf/subt
-
 # Download the public models
 RUN ign fuel download -v 4 -j 8 -u "https://fuel.ignitionrobotics.org/OpenRobotics/collections/SubT Tech Repo"
 
@@ -128,6 +122,12 @@ RUN ./download_niosh_2.sh
 COPY download_niosh_3.sh ./
 RUN ./download_niosh_3.sh
 
+# Clone all the subt models so that you don't download them every time
+# docker is run
+RUN mkdir -p subt_ws/src \
+ && cd subt_ws/src \
+ && git clone https://github.com/osrf/subt -b distance_segfault
+
 WORKDIR /home/$USERNAME/subt_ws
 
 # Install Rotors

docker/cloudsim_sim/run_sim.bash

@@ -13,4 +13,5 @@ bwm-ng -o csv -c 0 -t 1000 -T rate -I eth0 >> $FILE &
 # atop process monitoring
 atop -R -w /tmp/ign/logs/atop_log &
 
+export ROS_LOG_DIR=/tmp/ign/logs/ros
 ign launch -v 4 $@

docker/json2docker.rb

@@ -209,7 +209,7 @@
   # In this example, two robots are started with the names X1 and X2.
   sim:
     image: #{options['sim_image']}
-    command: cloudsim_sim.ign headless:=#{submission['headless']} circuit:=#{submission['circuit']} worldName:=#{submission['world']} #{robotStr}
+    command: cloudsim_sim.ign headless:=#{submission['headless']} circuit:=#{submission['circuit']} ros:=true worldName:=#{submission['world']} #{robotStr}
     networks:
       sim_net:
         ipv4_address: 172.28.1.1

subt_ign/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(catkin REQUIRED
     subt_communication_model
     subt_communication_broker
     subt_ros
+    rosbag
 )
 
 find_package(ignition-gazebo2 2.1 QUIET REQUIRED)

subt_ign/package.xml

@@ -21,6 +21,7 @@
 
   <depend>libccd-dev</depend>
   <depend>libfcl-dev</depend>
+  <depend>rosbag</depend>
 
   <test_depend>rostest</test_depend>
 </package>

subt_ign/src/BaseStationPlugin.cc

@@ -62,7 +62,7 @@ void BaseStationPlugin::OnArtifact(const std::string &_srcAddress,
   subt::msgs::Artifact artifact;
   if (!artifact.ParseFromString(_data))
   {
-    ignerr << "Error parsing artifact" << std::endl;
+    ignerr << "Error parsing artifact with data[" << _data << "]" << std::endl;
     return;
   }
 

subt_ign/src/GameLogicPlugin.cc

@@ -17,6 +17,7 @@
 
 #include <yaml-cpp/yaml.h>
 #include <ros/ros.h>
+#include <rosbag/recorder.h>
 
 #include <ignition/msgs/boolean.pb.h>
 #include <ignition/msgs/float.pb.h>
@@ -162,6 +163,9 @@ class subt::GameLogicPluginPrivate
   /// \param[in] _event Unused.
   public: void PublishScore();
 
+  /// \brief Function that runs the ROS bag recorder.
+  public: void RosBag();
+
   /// \brief Log robot pos data
   public: void LogRobotPosData();
 
@@ -314,6 +318,9 @@ class subt::GameLogicPluginPrivate
   /// \brief Thread on which scores are published
   public: std::unique_ptr<std::thread> publishThread = nullptr;
 
+  /// \brief Thread on which the ROS bag recorder runs.
+  public: std::unique_ptr<std::thread> bagThread = nullptr;
+
   /// \brief Whether the task has started.
   public: bool started = false;
 
@@ -439,6 +446,9 @@ class subt::GameLogicPluginPrivate
   /// \brief A mutex.
   public: std::mutex mutex;
 
+  /// \brief Mutex to protect the poses data structure.
+  public: std::mutex posesMutex;
+
   /// \brief Log file output stream.
   public: std::ofstream logStream;
 
@@ -554,6 +564,10 @@ class subt::GameLogicPluginPrivate
   public: ros::Publisher rosRegionEventPub;
   public: std::map<std::string, ros::Publisher> rosRobotPosePubs;
   public: std::map<std::string, ros::Publisher> rosRobotKinematicPubs;
+
+  /// \brief Pointer to the ROS bag recorder.
+  public: std::unique_ptr<rosbag::Recorder> rosRecorder;
+
   public: std::string prevPhase = "";
 
   /// \brief Counter to create unique id for events
@@ -576,6 +590,13 @@ GameLogicPlugin::~GameLogicPlugin()
   this->dataPtr->finished = true;
   if (this->dataPtr->publishThread)
     this->dataPtr->publishThread->join();
+
+  if (this->dataPtr->bagThread)
+  {
+    // Shutdown ros. this makes the ROS bag recorder stop.
+    ros::shutdown();
+    this->dataPtr->bagThread->join();
+  }
 }
 
 //////////////////////////////////////////////////
@@ -663,6 +684,17 @@ void GameLogicPlugin::Configure(const ignition::gazebo::Entity & /*_entity*/,
       this->dataPtr->rosRegionEventPub =
         this->dataPtr->rosnode->advertise<subt_ros::RegionEvent>(
             "region_event", 100);
+
+      // Setup a ros bag recorder.
+      rosbag::RecorderOptions recorderOptions;
+      recorderOptions.append_date=false;
+      recorderOptions.prefix="/tmp/ign/logs/cloudsim";
+      recorderOptions.regex=true;
+      recorderOptions.topics.push_back("/subt/.*");
+
+      this->dataPtr->rosRecorder.reset(new rosbag::Recorder(recorderOptions));
+      this->dataPtr->bagThread.reset(new std::thread(
+        &GameLogicPluginPrivate::RosBag, this->dataPtr.get()));
     }
   }
 
@@ -1099,7 +1131,8 @@ void GameLogicPlugin::PostUpdate(
               auto mPose =
                 _ecm.Component<gazebo::components::Pose>(model->Data());
               // Execute the start logic if a robot has moved into the tunnel.
-              if (baseIter->second.Pos().Distance(mPose->Data().Pos()) >
+              if (mPose &&
+                  baseIter->second.Pos().Distance(mPose->Data().Pos()) >
                   this->dataPtr->allowedDistanceFromBase)
               {
                 ignition::msgs::Boolean req, res;
@@ -1206,7 +1239,11 @@ void GameLogicPlugin::PostUpdate(
               this->dataPtr.get());
         }
 
-        this->dataPtr->poses[_nameComp->Data()] = _poseComp->Data();
+        {
+          std::lock_guard<std::mutex> lock(this->dataPtr->posesMutex);
+          this->dataPtr->poses[_nameComp->Data()] = _poseComp->Data();
+        }
+
         for (std::pair<const subt::ArtifactType,
             std::map<std::string, ignition::math::Pose3d>> &artifactPair :
             this->dataPtr->artifacts)
@@ -1297,7 +1334,8 @@ void GameLogicPlugin::PostUpdate(
             this->dataPtr->robotPrevPose[name] = pose;
             return true;
           }
-          else if (robotPoseDataIt->second.back().second.Pos().Distance(
+          else if (!robotPoseDataIt->second.empty() &&
+              robotPoseDataIt->second.back().second.Pos().Distance(
                 pose.Pos()) > 1.0)
           {
             //  time passed since last pose sample
@@ -1475,6 +1513,7 @@ void GameLogicPlugin::PostUpdate(
   {
     static bool errorSent = false;
 
+    std::lock_guard<std::mutex> lock(this->dataPtr->posesMutex);
     // Get the artifact origin's pose.
     std::map<std::string, ignition::math::Pose3d>::iterator originIter =
       this->dataPtr->poses.find(subt::kArtifactOriginName);
@@ -2108,6 +2147,12 @@ void GameLogicPluginPrivate::ParseArtifacts(
   }
 }
 
+/////////////////////////////////////////////////
+void GameLogicPluginPrivate::RosBag()
+{
+  this->rosRecorder->run();
+}
+
 /////////////////////////////////////////////////
 void GameLogicPluginPrivate::PublishScore()
 {
@@ -2273,7 +2318,24 @@ void GameLogicPluginPrivate::Finish(const ignition::msgs::Time &_simTime)
         statusMsg.timestamp.sec = _simTime.sec();
         statusMsg.timestamp.nsec = _simTime.nsec();
         this->rosStatusPub.publish(statusMsg);
+
+        if (this->bagThread)
+        {
+          // Shutdown ros. this makes the ROS bag recorder stop.
+          ros::shutdown();
+          this->bagThread->join();
+        }
+
       }
+
+      // Send the recording_complete message after ROS has shutdown, if ROS
+      // has been enabled.
+      ignition::msgs::StringMsg completeMsg;
+      completeMsg.mutable_header()->mutable_stamp()->CopyFrom(
+          this->simTime);
+      completeMsg.set_data("recording_complete");
+      this->startPub.Publish(completeMsg);
+
       this->eventCounter++;
     }
   }
@@ -2625,42 +2687,49 @@ void GameLogicPluginPrivate::LogRobotArtifactData(
 bool GameLogicPluginPrivate::PoseFromArtifactHelper(const std::string &_robot,
     ignition::math::Pose3d &_result)
 {
-  // Get an iterator to the robot's pose.
-  std::map<std::string, ignition::math::Pose3d>::iterator robotIter =
-    this->poses.find(_robot);
+  ignition::math::Pose3d robotPose, basePose;
 
-  if (robotIter == this->poses.end())
   {
-    ignerr << "[GameLogicPlugin]: Unable to find robot with name ["
-           << _robot << "]. Ignoring PoseFromArtifact request" << std::endl;
-    return false;
-  }
+    std::lock_guard<std::mutex> lock(this->posesMutex);
+    // Get an iterator to the robot's pose.
+    std::map<std::string, ignition::math::Pose3d>::iterator robotIter =
+      this->poses.find(_robot);
 
-  // Get an iterator to the base station's pose.
-  std::map<std::string, ignition::math::Pose3d>::iterator baseIter =
-    this->poses.find(subt::kBaseStationName);
+    if (robotIter == this->poses.end())
+    {
+      ignerr << "[GameLogicPlugin]: Unable to find robot with name ["
+        << _robot << "]. Ignoring PoseFromArtifact request" << std::endl;
+      return false;
+    }
+    robotPose = robotIter->second;
 
-  // Sanity check: Make sure that the robot is in the staging area, as this
-  // service is only available in that zone.
-  if (baseIter == this->poses.end())
-  {
-    ignerr << "[GameLogicPlugin]: Unable to find the staging area  ["
-      << subt::kBaseStationName
-      << "]. Ignoring PoseFromArtifact request" << std::endl;
-    return false;
+    // Get an iterator to the base station's pose.
+    std::map<std::string, ignition::math::Pose3d>::iterator baseIter =
+      this->poses.find(subt::kBaseStationName);
+
+    // Sanity check: Make sure that the robot is in the staging area, as this
+    // service is only available in that zone.
+    if (baseIter == this->poses.end())
+    {
+      ignerr << "[GameLogicPlugin]: Unable to find the staging area  ["
+        << subt::kBaseStationName
+        << "]. Ignoring PoseFromArtifact request" << std::endl;
+      return false;
+    }
+
+    basePose = baseIter->second;
   }
 
-  if (baseIter->second.Pos().Distance(robotIter->second.Pos()) >
+  if (basePose.Pos().Distance(robotPose.Pos()) >
       this->allowedDistanceFromBase)
   {
     ignerr << "[GameLogicPlugin]: Robot [" << _robot << "] is too far from the "
       << "staging area. Ignoring PoseFromArtifact request" << std::endl;
     return false;
   }
 
-
   // Pose.
-  _result = robotIter->second - this->artifactOriginPose;
+  _result = robotPose - this->artifactOriginPose;
   return true;
 }
 

subt_ros/launch/cloudsim_init.launch

@@ -15,6 +15,5 @@
     type="subt_ros_relay"
     name="subt_ros_relay"/>
 
-  <node pkg="rosbag" type="record" name="cloudsim_data"
-    args="record --split --size=1000 -O /tmp/ign/logs/cloudsim.bag -e '/subt/.*'"/>
+  <!-- A rosbag recorder is in the subt_ign/src/GameLogicPlugin. -->
 </launch>