123

qgroundcontrol.pro

@@ -28,11 +28,12 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
+    DEFINES += ROS_BRIDGE_DEBUG
     #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    DEFINES += DEBUG
+    DEFINES += ROS_BRIDGE_DEBUG
     DEFINES += NDEBUG
 }
 

src/Wima/Geometry/WimaPolygonArray.h

@@ -13,6 +13,9 @@ public:
     WimaPolygonArray(const WimaPolygonArray &other) :
         _polygons(other._polygons), _dirty(true)
     {}
+    ~WimaPolygonArray(){
+        _objs.clearAndDeleteContents();
+    }
 
     class QmlObjectListModel *QmlObjectListModel(){
         if (_dirty)
@@ -34,9 +37,9 @@ public:
 
 private:
     void _updateObjects(void){
-        _objs.clear();
+        _objs.clearAndDeleteContents();
         for (long i=0; i<_polygons.size(); ++i){
-            _objs.append(&_polygons[i]);
+            _objs.append(new PolygonType(_polygons[i]));
         }
     }
 

src/Wima/WimaController.h

@@ -337,7 +337,7 @@ private slots:
     void _snakeStoreWorkerResults            ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
-    void _setupTopicService                     ();
+    bool _doTopicServiceSetup();
     // Periodic tasks.
     void _eventTimerHandler                  (void);
     // Waypoint manager handling.
@@ -407,6 +407,7 @@ private:
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
     static StatusMap        _nemoStatusMap;
+    bool                    _topicServiceSetupDone;
 
     // Periodic tasks.
     QTimer      _eventTimer;

src/comm/ros_bridge/include/com_private.cpp

@@ -20,18 +20,10 @@ bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &d
 {
     if ( doc.HasMember("topic") && doc["topic"].IsString() ){
 
-
-        rapidjson::StringBuffer sb1;
-        rapidjson::Writer<rapidjson::StringBuffer> writer1(sb1);
-        doc.Accept(writer1);
-        std::cout << "getTopic doc: " << sb1.GetString() << std::endl;
-
         rapidjson::StringBuffer sb;
         rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
         doc["topic"].Accept(writer);
         topic = sb.GetString();
-
-        std::cout << "getTopic topic: " << sb.GetString() << std::endl;
         return true;
     } else
         return false;

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp

 #include "geopoint.h"
 
 std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
-    return "geometry_msgs/GeoPoint";
+    return "geographic_msgs/GeoPoint";
 }

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h

@@ -109,9 +109,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
 
     value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/point32.h

@@ -103,9 +103,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
 
     value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h

@@ -57,9 +57,6 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen
     }
 
     value.AddMember("points", points, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h

@@ -95,9 +95,6 @@ bool _toJson(const PolygonType &poly,
     }
     value.AddMember("header", header, allocator);
     value.AddMember("polygon", polygon, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h

@@ -183,10 +183,6 @@ namespace detail {
         detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
         value.AddMember("likelihood", jLikelihood, allocator);
 
-        rapidjson::Value jType;
-        jType.SetString(messageType().c_str(), allocator);
-        value.AddMember("type", jType, allocator);
-
         return true;
     }
 

src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h

@@ -32,9 +32,6 @@ template <class HeartbeatType>
 bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
 {
     value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/nemo_msgs/progress.h

@@ -41,9 +41,6 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document:
         jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
     }
     value.AddMember("progress", jProgress, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/std_msgs/header.h

@@ -62,9 +62,6 @@ bool toJson(const HeaderType &header,
                                                header.frameId().length(),
                                                allocator),
                   allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/messages/std_msgs/time.h

@@ -37,9 +37,6 @@ bool toJson(const TimeType &time,
 {
     value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
     value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/ros_bridge.h

@@ -23,13 +23,16 @@ public:
     void advertiseService(const char* service,
                           const char* type,
                           const std::function<JsonDocUPtr(JsonDocUPtr)> &callback);
+    void advertiseTopic(const char* topic,
+                        const char* type);
 
     //! @brief Start ROS bridge.
     void start();
     //! @brief Stops ROS bridge.
     void reset();
+
     //! @return Returns true if connected to the rosbridge_server, false either.
-    //! @note This function can block up to 100ms. However normal execution time is smaller.
+    //! @note \fn calls start().
     bool connected();
     bool isRunning();
 

src/comm/ros_bridge/include/topic_publisher.cpp

@@ -20,8 +20,6 @@ void ros_bridge::com_private::TopicPublisher::start()
         return;
     _stopped->store(false);
     _pThread = std::make_unique<std::thread>([this]{
-        // Init.
-        std::unordered_map<std::string, std::string> topicMap;
         // Main Loop.
         while( !this->_stopped->load() ){
             std::unique_lock<std::mutex> lk(this->_mutex);
@@ -39,38 +37,25 @@ void ros_bridge::com_private::TopicPublisher::start()
 
             // Get topic and type from Json message and remove it.
             std::string topic;
-            assert(com_private::getTopic(*pJsonDoc, topic));
-            assert(com_private::removeTopic(*pJsonDoc));
-            std::string type;
-            assert(com_private::getType(*pJsonDoc, type));
-            assert(com_private::removeType(*pJsonDoc));
-
-            // Check if topic must be advertised.
-            std::string clientName =
-                    ros_bridge::com_private::_topicAdvertiserKey
-                    + topic;
-            auto it =  topicMap.find(clientName);
-            if ( it ==  topicMap.end()) { // Need to advertise topic?
-                 topicMap.insert(std::make_pair(clientName, topic));
-                 this->_rbc.addClient(clientName);
-                 this->_rbc.advertise(clientName, topic, type);
-                 this->_rbc.waitForTopic(topic, [this]{
-                     return this->_stopped->load();
-                 }); // Wait until topic is advertised.
-            }
+            bool ret = com_private::getTopic(*pJsonDoc, topic);
+            assert(ret);
+            (void)ret;
+            ret = com_private::removeTopic(*pJsonDoc);
+            assert(ret);
+            (void)ret;
+
+            // Wait for topic (Rosbridge needs some time to process a advertise() request).
+            this->_rbc.waitForTopic(topic, [this]{
+                return this->_stopped->load();
+            });
 
             // Publish Json message.
             if ( !this->_stopped->load() )
                 this->_rbc.publish(topic, *pJsonDoc);
         } // while loop
-
-        // Tidy up.
-        for (auto& it :  topicMap){
-            this->_rbc.unadvertise(it.second);
-            this->_rbc.removeClient(it.first);
-        }
-
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
+#endif
     });
 }
 
@@ -87,7 +72,13 @@ void ros_bridge::com_private::TopicPublisher::reset()
         return;
     _pThread->join();
 
-    lk.lock();
+    lk.lock();    
+    // Tidy up.
+    for (auto& it :  this->_topicMap){
+        this->_rbc.unadvertise(it.first);
+        this->_rbc.removeClient(it.second);
+    }
+    this->_topicMap.clear();
     _queue.clear();
 }
 
@@ -95,14 +86,52 @@ void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
         const char *topic)
 {
-    std::cout << "TopicPublisher \"topic\": " << topic << std::endl;
+    // Check if topic was advertised.
+    std::string t(topic);
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Not yet advertised?
+        lk.unlock();
+#ifdef ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic "
+                  << t << " not yet advertised" << std::endl;
+#endif
+        return;
+    }
+    lk.unlock();
+
+    // Add topic information to json doc.
     auto &allocator = docPtr->GetAllocator();
     rapidjson::Value key("topic", allocator);
     rapidjson::Value value(topic, allocator);
     docPtr->AddMember(key, value, allocator);
-    std::unique_lock<std::mutex> lock(_mutex);
+
+    lk.lock();
     _queue.push_back(std::move(docPtr));
-    lock.unlock();
+    lk.unlock();
     _cv.notify_one();  // Wake publisher thread.
 }
 
+bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
+{
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    std::string t(topic);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Need to advertise topic?
+        std::string clientName =
+                std::string(ros_bridge::com_private::_topicAdvertiserKey)
+                + t;
+        this->_topicMap.insert(std::make_pair(t, clientName));
+        lk.unlock();
+        this->_rbc.addClient(clientName);
+        this->_rbc.advertise(clientName, t, type);
+        return true;
+    } else {
+        lk.unlock();
+#if ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
+#endif
+        return false;
+    }
+}
+

src/comm/ros_bridge/include/topic_publisher.h

@@ -31,9 +31,12 @@ public:
     void reset();
 
     void publish(JsonDocUPtr docPtr, const char *topic);
+    bool advertise(const char *topic, const char *type);
 
 private:
+    using TopicMap = std::unordered_map<std::string, std::string>;
     JsonQueue           _queue;
+    TopicMap            _topicMap;
     std::mutex          _mutex;
     std::shared_ptr<std::atomic_bool>   _stopped;
     RosbridgeWsClient  &_rbc;

src/comm/ros_bridge/src/ros_bridge.cpp

@@ -32,8 +32,15 @@ void ros_bridge::ROSBridge::advertiseService(const char *service,
     _server.advertiseService(service, type, callback);
 }
 
+void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type)
+{
+    _topicPublisher.advertise(topic, type);
+}
+
 void ros_bridge::ROSBridge::start()
 {
+    if ( !_stopped->load() )
+        return;
     _stopped->store(false);
     _rbc.run();
     _topicPublisher.start();
@@ -44,6 +51,8 @@ void ros_bridge::ROSBridge::start()
 void ros_bridge::ROSBridge::reset()
 {
     auto start = std::chrono::high_resolution_clock::now();
+    if ( _stopped->load() )
+        return;
     _stopped->store(true);
     _topicPublisher.reset();
     _topicSubscriber.reset();
@@ -56,6 +65,7 @@ void ros_bridge::ROSBridge::reset()
 
 bool ros_bridge::ROSBridge::connected()
 {
+    start();
     return _rbc.connected();
 }