Merge branch 'devel' into 'main'

Devel

See merge request !4

.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
@@ -10,7 +20,6 @@ stages:
   - echo $WOLF_VISION_BRANCH
   - echo $WOLF_ROS_NODE_BRANCH
   - echo $CI_COMMIT_BRANCH
-  - echo $WOLF_ROS_VISION_BRANCH
 
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
@@ -79,8 +88,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
@@ -93,29 +101,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
@@ -131,7 +122,7 @@ stages:
 .clone_wolfrosnode_template: &clone_wolfrosnode_definition
   - roscd
   - cd ../src
-  - git clone -b $WOLF_ROS_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
+  - git clone -b $WOLF_ROS_NODE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git
 
 .build_and_test_template: &build_and_test_definition
   - roscd
@@ -170,7 +161,6 @@ build_and_test:bionic:
     - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
-    - *install_visionutils_definition
     - *install_wolfvision_definition
     - *clone_wolfrosnode_definition
     - ldconfig
@@ -195,7 +185,6 @@ build_and_test:focal:
     - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
-    - *install_visionutils_definition
     - *install_wolfvision_definition
     - *clone_wolfrosnode_definition
     - ldconfig

CMakeLists.txt

@@ -4,7 +4,7 @@ project(wolf_ros_vision)
 ## Compile as C++14
 add_compile_options(-std=c++14) 
 # -fPIC and -rdynamic ensure unique singleton instance across shared libraries (for factories) see: https://stackoverflow.com/a/8626922
-SET(CMAKE_CXX_FLAGS "-fPIC -rdynamic")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -rdynamic")
 
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED COMPONENTS

scripts/publisher_camera_info.py

 #!/usr/bin/env python3
-import sys
-import copy
-from pure_eval import CannotEval
 import yaml
-import numpy as np
 import rospy
-from sensor_msgs.msg import CameraInfo
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import CameraInfo, Image
 
-def kvec2mat(fx, fy, cx, cy):
-    return np.array([
-    fx, 0,  cx,
-    0,  fy, cy,
-    0,  0,  0
-]).reshape((3,3))
-    
-    
-def kmat2vec(K):
-    # fx, fy, cx, cy
-    return [K[0,0], K[1,1], K[0,2], K[1,2]]
-    
 
 class CameraInfoPublisher:
     

src/publisher_vision.cpp

@@ -69,9 +69,11 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
         last_capture_(nullptr),
         img_transport_(ros::NodeHandle())
 {
+    WOLF_INFO("\n\n\n\n\n\n\n\nYOOOOOOOOOOOOOOOOOOOO PublisherVisionDebug")
+
     // if user do not provide processor's name, first processor of type PublisherVisionDebug is taken
     auto processor_name                    = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", "");
-    topic_preprocessor_                    = getParamWithDefault<std::string>(_server, prefix_ + "/topic_preprocessor", "/debug_image_prepocessor");
+    topic_preprocessor_                    = getParamWithDefault<std::string>(_server, prefix_ + "/topic_preprocessor", "/image_vo_tracks_prepocessor");
     //Tracks      
     tracks_.show_id_                       = getParamWithDefault<bool>(_server, prefix_ + "/tracks/show_id", false);
     tracks_.size_id_                       = getParamWithDefault<double>(_server, prefix_ + "/tracks/size_id", 0.5);
@@ -142,6 +144,7 @@ bool PublisherVisionDebug::ready()
 
 void PublisherVisionDebug::publishDerived()
 {
+    WOLF_INFO("YO")
     if (last_capture_ == processor_vision_->getLast() or not processor_vision_->getLast())
         return;
 
@@ -157,9 +160,14 @@ void PublisherVisionDebug::publishDerived()
     // Draw and publish debug image
     try
     {
-        // Extract cv image
-        cv::Mat cv_img_debug;
-        cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+        // Transform the image to RGB if grayscale so that we can draw detections in color for debugging
+        cv::Mat cv_img_debug; 
+        if (cap_img->getImage().channels() == 1){
+            cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+        }        
+        else {
+            cv_img_debug = cap_img->getImage().clone();
+        }
 
 
         // Draw all tracks