Merge branch 'devel' into 'main'

Devel

See merge request !8

.gitlab-ci.yml

+workflow:
+  rules:
+    - if: '$CI_PIPELINE_SOURCE == "web"'
+    - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push"
+      when: never
+    - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+    - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+      when: never
+    - if: '$CI_COMMIT_BRANCH'
+
 stages:
   - license
   - build_and_test
@@ -11,7 +21,6 @@ stages:
   - echo $WOLF_APRILTAG_BRANCH
   - echo $WOLF_ROS_NODE_BRANCH
   - echo $CI_COMMIT_BRANCH
-  - echo $WOLF_ROS_APRILTAG_BRANCH
 
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
@@ -80,8 +89,7 @@ stages:
   - if [ -d wolf ]; then
   -   echo "directory wolf exists"
   -   cd wolf
-  -   git checkout devel
-  -   git pull
+  -   git fetch --all
   -   git checkout $WOLF_CORE_BRANCH
   -   git pull
   - else
@@ -94,29 +102,12 @@ stages:
   - make -j$(nproc)
   - make install
 
-.install_visionutils_template: &install_visionutils_definition
-  - cd ${CI_PROJECT_DIR}/ci_deps
-  - if [ -d vision_utils ]; then
-  -   echo "directory vision_utils exists"
-  -   cd vision_utils
-  -   git pull
-  - else
-  -   git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/vision_utils.git
-  -   cd vision_utils
-  - fi
-  - mkdir -pv build
-  - cd build
-  - cmake -DCMAKE_BUILD_TYPE=release ..
-  - make -j$(nproc)
-  - make install
-
 .install_wolfvision_template: &install_wolfvision_definition
   - cd ${CI_PROJECT_DIR}/ci_deps
   - if [ -d vision ]; then
   -   echo "directory vision exists"
   -   cd vision
-  -   git checkout devel
-  -   git pull
+  -   git fetch --all
   -   git checkout $WOLF_VISION_BRANCH
   -   git pull
   - else
@@ -150,8 +141,7 @@ stages:
   - if [ -d apriltag ]; then
   -   echo "directory apriltag exists"
   -   cd apriltag
-  -   git checkout devel
-  -   git pull
+  -   git fetch --all
   -   git checkout $WOLF_APRILTAG_BRANCH
   -   git pull
   - else
@@ -209,7 +199,6 @@ build_and_test:bionic:
     - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
-    - *install_visionutils_definition
     - *install_wolfvision_definition
     - *install_apriltag_definition
     - *install_wolfapriltag_definition
@@ -239,7 +228,6 @@ build_and_test:focal:
     - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
-    - *install_visionutils_definition
     - *install_wolfvision_definition
     - *install_apriltag_definition
     - *install_wolfapriltag_definition

src/publisher_apriltag_debug.cpp

@@ -75,22 +75,29 @@ void PublisherApriltagDebug::publishDerived()
     // Extract cv image
     try
     {
-        cv::Mat cv_img = cap_img->getImage().clone();
+        // Transform the image to RGB if grayscale so that we can draw detections in color for debugging
+        cv::Mat cv_img; 
+        if (cap_img->getImage().channels() == 1){
+            cv::cvtColor(cap_img->getImage(), cv_img, cv::COLOR_GRAY2BGR);
+        }        
+        else {
+            cv_img = cap_img->getImage().clone();
+        }
 
+        std::vector<cv::Point2d> tag_corners(8);
+        int tag_id;
         // Draw tags detected in the last capture
         for (auto feat : cap_img->getFeatureList())
         {
-            auto feat_april = std::dynamic_pointer_cast<const FeatureApriltag>(feat);
-            
-            // skip other features
-            if (not feat_april)
-                continue;
+            auto feat_april_pose = std::dynamic_pointer_cast<const FeatureApriltag>(feat);
+            std::vector<cv::Point2d> tag_corners = feat_april_pose->getTagCorners();
+            int tag_id = feat_april_pose->getTagId();
             
-            cv::line(cv_img, feat_april->getTagCorners()[0], feat_april->getTagCorners()[1], CV_RGB(255, 0, 0), 2);
-            cv::line(cv_img, feat_april->getTagCorners()[1], feat_april->getTagCorners()[2], CV_RGB(0, 255, 0), 2);
-            cv::line(cv_img, feat_april->getTagCorners()[2], feat_april->getTagCorners()[3], CV_RGB(0, 0, 255), 2);
-            cv::line(cv_img, feat_april->getTagCorners()[3], feat_april->getTagCorners()[0], CV_RGB(255, 0, 255), 2);
-            cv::putText(cv_img, cv::String(std::to_string(feat_april->getDetection().id)), feat_april->getTagCorners()[0], cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,220,0),1.5);
+            cv::line(cv_img, tag_corners[0], tag_corners[1], CV_RGB(255, 0, 0), 2);
+            cv::line(cv_img, tag_corners[1], tag_corners[2], CV_RGB(0, 255, 0), 2);
+            cv::line(cv_img, tag_corners[2], tag_corners[3], CV_RGB(0, 0, 255), 2);
+            cv::line(cv_img, tag_corners[3], tag_corners[0], CV_RGB(255, 0, 255), 2);
+            cv::putText(cv_img, cv::String(std::to_string(tag_id)), tag_corners[0], cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,220,0),1.5);
         }
 
         // Convert to image msg