[NRRPLT-6792] Forward error topic for sending Gazebo errors.

Merged in NRRPLT-6792-the-cle-shouldnt-crash-when- (pull request #39)

Approved-by: Sandro Weber
Approved-by: Michael Zechmair

gzbridge/GazeboInterface.cc

@@ -67,6 +67,7 @@ GazeboInterface::GazeboInterface()
   this->heightmapService = "~/heightmap_data";
   this->deleteTopic = "~/entity_delete";
   this->playbackControlTopic = "~/playback_control";
+  this->errorTopic = "~/error";
 
   // material topic
   this->materialTopic = "~/material";
@@ -125,6 +126,9 @@ GazeboInterface::GazeboInterface()
   this->roadSub = this->node->Subscribe(this->roadTopic,
       &GazeboInterface::OnRoad, this, true);
 
+  this->errorSub = this->node->Subscribe(this->errorTopic,
+      &GazeboInterface::OnErrorMsg, this, true);  
+
   // For getting scene info on connect
   this->requestPub =
       this->node->Advertise<gazebo::msgs::Request>(this->requestTopic);
@@ -191,6 +195,7 @@ GazeboInterface::~GazeboInterface()
   this->sceneMsgs.clear();
   this->physicsMsgs.clear();
   this->jointMsgs.clear();
+  this->errorMsgs.clear();
 
 #ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
   this->jointStateMsgs.clear();
@@ -230,6 +235,7 @@ GazeboInterface::~GazeboInterface()
   this->lightFactoryPub.reset();
   this->lightModifyPub.reset();
   this->responseSub.reset();
+  this->errorSub.reset();
   this->node.reset();
 
   if (this->runThread)
@@ -950,6 +956,17 @@ void GazeboInterface::ProcessMessages()
       ++pIter;
     }
     this->poseMsgs.clear();
+    
+    // Forward all the error messages.
+    for (auto errorIter = this->errorMsgs.begin();
+        errorIter != this->errorMsgs.end(); ++errorIter)
+    {
+      msg = this->PackOutgoingTopicMsg(this->errorTopic,
+          pb2json(*(*errorIter).get()));
+      this->Send(msg);
+    }
+    this->errorMsgs.clear();
+    
     this->receivedMessage = false;
   }
 }
@@ -1355,6 +1372,16 @@ void GazeboInterface::OnVisualMsg(ConstVisualPtr &_msg)
   this->receiveCondition.notify_one();
 }
 
+/////////////////////////////////////////////////
+void GazeboInterface::OnErrorMsg(ConstErrorPtr &_msg)
+{
+  std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
+  this->errorMsgs.push_back(_msg);
+  this->receivedMessage = true;
+  lock.unlock();
+  this->receiveCondition.notify_one();
+}
+
 /////////////////////////////////////////////////
 std::string GazeboInterface::PackOutgoingTopicMsg(const std::string &_topic,
     const std::string &_msg)

gzbridge/GazeboInterface.hh

@@ -189,6 +189,10 @@ namespace gzweb
     /// \param[in] _msg The message data.
     private: void OnResponse(ConstResponsePtr &_msg);
 
+    /// \brief Error message callback.
+    /// \param[in] _msg The message data.
+    private: void OnErrorMsg(ConstErrorPtr &_msg);
+
     /// \brief Block if there are no connections.
     private: void WaitForConnection();
 
@@ -273,6 +277,9 @@ namespace gzweb
     /// \brief Subscribe to road msgs.
     private: gazebo::transport::SubscriberPtr roadSub;
 
+    /// \brief Subscribe to error topic
+    private: gazebo::transport::SubscriberPtr errorSub;
+
     /// \brief Publish requests
     private: gazebo::transport::PublisherPtr requestPub;
 
@@ -445,6 +452,14 @@ namespace gzweb
     /// \brief List of road message to process.
     private: RoadMsgs_L roadMsgs;
 
+    /// \def ErrorMsgs_L
+    /// \brief List of error messages.
+    typedef std::list<boost::shared_ptr<gazebo::msgs::Error const> >
+        ErrorMsgs_L;
+
+    /// \brief List of error message to process.
+    private: ErrorMsgs_L errorMsgs;
+
     /// \def PoseMsgsFilter_M
     /// \brief Map of last pose messages used for filtering
     typedef std::map< std::string, TimedPose> PoseMsgsFilter_M;
@@ -532,6 +547,9 @@ namespace gzweb
     /// \brief Name of playback control topic.
     private: std::string playbackControlTopic;
 
+    /// \brief Name of error topic.
+    private: std::string errorTopic;
+
     /// \brief Ogre material parser.
     private: OgreMaterialParser *materialParser = nullptr;