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.cc

@@ -89,6 +89,7 @@ WimaController::WimaController(QObject *parent)
     , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
     , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
     , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
+    , _topicServiceSetupDone    (false)
 {    
 
     // ROS Bridge.
@@ -552,22 +553,28 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 
     if ( _enableSnake.rawValue().toBool()
          && _pRosBridge->isRunning()
-            && _pRosBridge->connected() ){
-        if ( _snakeTilesLocal.polygons().size() > 0 ){
+         && _pRosBridge->connected()
+         && _topicServiceSetupDone
+         && _snakeTilesLocal.polygons().size() > 0
+         )
+    {
         using namespace ros_bridge::messages;
         // Publish snake origin.
         JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            Q_ASSERT(geographic_msgs::geo_point::toJson(
-                         _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+        bool ret = geographic_msgs::geo_point::toJson(
+                     _snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+        Q_ASSERT(ret);
         _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
         // Publish snake tiles.
         JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                         _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+        ret = jsk_recognition_msgs::polygon_array::toJson(
+                     _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+        Q_ASSERT(ret);
         _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-        }
+
     }
 
+
     _localPlanDataValid = true;
     return true;
 }
@@ -758,21 +765,24 @@ void WimaController::_eventTimerHandler()
     }
 
     if ( _snakeTicker.ready() ) {
-        static bool setupDone = false;
         if ( _enableSnake.rawValue().toBool() ) {
             if ( !_pRosBridge->isRunning()) {
                 _pRosBridge->start();
-            } else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) {
+            } else if ( _pRosBridge->isRunning()
+                        && _pRosBridge->connected()
+                        && !_topicServiceSetupDone) {
+                if ( _doTopicServiceSetup() )
+                    _topicServiceSetupDone = true;
+            } else if ( _pRosBridge->isRunning()
+                        && !_pRosBridge->connected()
+                        && _topicServiceSetupDone){
                 _pRosBridge->reset();
                 _pRosBridge->start();
-                _setupTopicService();
-                setupDone = true;
-            } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){
-                setupDone = false;
+                _topicServiceSetupDone = false;
             }
         } else  if ( _pRosBridge->isRunning() ) {
                 _pRosBridge->reset();
-                setupDone = false;
+                _topicServiceSetupDone = false;
         }
     }
 }
@@ -948,21 +958,35 @@ void WimaController::_switchSnakeManager(QVariant variant)
     }
 }
 
-void WimaController::_setupTopicService()
+bool WimaController::_doTopicServiceSetup()
 {
     using namespace ros_bridge::messages;
-    if ( _snakeTilesLocal.polygons().size() > 0){
+
+    if ( _snakeTilesLocal.polygons().size() == 0)
+        return false;
+
     // Publish snake origin.
+    _pRosBridge->advertiseTopic("/snake/origin",
+                                geographic_msgs::geo_point::messageType().c_str());
     JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        Q_ASSERT(geographic_msgs::geo_point::toJson(
-                     _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+    bool ret = geographic_msgs::geo_point::toJson(
+                _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
     _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+
     // Publish snake tiles.
+    _pRosBridge->advertiseTopic("/snake/tiles",
+                                jsk_recognition_msgs::polygon_array::messageType().c_str());
     JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                     _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+    ret = jsk_recognition_msgs::polygon_array::toJson(
+                _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
     _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-    }
+
+
     // Subscribe nemo progress.
     _pRosBridge->subscribe("/nemo/progress",
                            /* callback */ [this](JsonDocUPtr pDoc){
@@ -972,23 +996,29 @@ void WimaController::_setupTopicService()
              || progress_msg.progress().size() != requiredSize ) { // Some error occured.
             // Set progress to default.
             progress_msg.progress().fill(0, requiredSize);            
-            // Publish origin and tiles again, if valid.
-            if ( this->_snakeTilesLocal.polygons().size() > 0){
+
             // Publish snake origin.
             JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-                Q_ASSERT(geographic_msgs::geo_point::toJson(
-                             this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+            bool ret = geographic_msgs::geo_point::toJson(
+                         this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
             this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
             // Publish snake tiles.
             JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-                Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                             this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+            ret = jsk_recognition_msgs::polygon_array::toJson(
+                         this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
             this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
         }
-        }
 
         emit WimaController::nemoProgressChanged();
     });
+
+
+    // Subscribe /nemo/heartbeat.
     _pRosBridge->subscribe("/nemo/heartbeat",
                            /* callback */ [this](JsonDocUPtr pDoc){
         auto &heartbeat_msg = this->_nemoHeartbeat;
@@ -1004,31 +1034,38 @@ void WimaController::_setupTopicService()
         emit WimaController::nemoStatusStringChanged();
     });
 
+
+    // Advertise /snake/get_origin.
     _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
         using namespace ros_bridge::messages;
+
         JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        ::GeoPoint3D origin = this->_snakeOrigin;
         rapidjson::Value jOrigin(rapidjson::kObjectType);
-        if ( this->_snakeTilesLocal.polygons().size() > 0){
-            geographic_msgs::geo_point::toJson(
-                        this->_snakeOrigin, jOrigin, pDoc->GetAllocator());
-        } else {
-            geographic_msgs::geo_point::toJson(
-                        ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator());
-        }
+        bool ret = geographic_msgs::geo_point::toJson(
+                    origin, jOrigin, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
         pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
         return pDoc;
     });
 
+
+    // Advertise /snake/get_tiles.
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
         JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-        jsk_recognition_msgs::polygon_array::toJson(
+        bool ret = jsk_recognition_msgs::polygon_array::toJson(
                     this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
         pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
         return pDoc;
     });
+
+    return true;
 }

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));
+            bool ret = com_private::getTopic(*pJsonDoc, topic);
+            assert(ret);
+            (void)ret;
+            ret = com_private::removeTopic(*pJsonDoc);
+            assert(ret);
+            (void)ret;
 
-            // 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);
+            // Wait for topic (Rosbridge needs some time to process a advertise() request).
             this->_rbc.waitForTopic(topic, [this]{
                 return this->_stopped->load();
-                 }); // Wait until topic is advertised.
-            }
+            });
 
             // 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
     });
 }
 
@@ -88,6 +73,12 @@ void ros_bridge::com_private::TopicPublisher::reset()
     _pThread->join();
 
     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();
 }