src/lhw_interfaces/srv_visage/FaceAnalysis.srv

+uint16 setting
+---
+FacesDetected faces
\ No newline at end of file

src/lhw_qi/activate

@@ -21,8 +21,8 @@ ldconfig
 # Search for pepper on the local network
 echo "Searching for the IP of Pepper2"
 #export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1) - pepper linköping MAC
-export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c" | cut -f1)
 
+export PEPPER_IP=$(arp-scan --localnet | grep -i "48:a9:d2:8c:6a:0c\|00:13:95:1d:4c:43" | cut -f1)
 if [[ -z "${PEPPER_IP}" ]]; then
     echo "${ORANGE}Pepper was not found on the current network, please set PEPPER_IP manually!${NC}"
 else
@@ -35,4 +35,4 @@ if [ $ENV_LAUNCH ]; then
   source install/setup.zsh && \
   ros2 launch lhw_qi qi.launch.py & \
   echo -e "${GREEN} QI started${NC}"
-fi
\ No newline at end of file
+fi

src/lhw_qi/src/animated_speech_component.cpp

@@ -17,7 +17,7 @@ namespace lhw_qi
     this->declare_parameter("speed", 0.8);
     this->declare_parameter("pitch", 1.2);
     this->declare_parameter("volume", 0.8);
-    this->declare_parameter("use_arms", true);
+    this->declare_parameter("use_arms", false);
     
     
     // Create a callback function for when messages are received.

src/lhw_qi/src/motion_component.cpp

@@ -147,7 +147,7 @@ namespace lhw_qi
     else if (action.type == action.POINT_ABS_STRAIGHT) {
       if (action.arm == action.ARM_RIGHT) {
         // Right arm
-        std::vector<float> position = {0.0f, -1.0f, 0.0f};
+        std::vector<float> position = {1.0f, 0.0f, 0.0f};
         tracker_service_.call<void>("pointAt", "RArm", position, 0, 0.3);
       }
       else if (action.arm == action.ARM_LEFT) {

src/lhw_visage/CMakeLists.txt

@@ -28,8 +28,9 @@ set(PYTHON_INCLUDE_DIRS ${PYTHON_INCLUDE_DIRS} /usr/local/lib/python3.8/dist-pac
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rcutils REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rcutils REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -40,28 +41,27 @@ find_package(Boost REQUIRED COMPONENTS chrono filesystem program_options regex s
 # further dependencies manually.
 
 # find_package(naoqi_bridge_msgs REQUIRED)
-find_package(qi REQUIRED)
+#find_package(qi REQUIRED)
 find_package(cv_bridge)
 find_package(OpenMP)
 find_package(ZLIB REQUIRED) # Needed for visageSDK
 find_package(OpenCV REQUIRED)
 find_package(Threads REQUIRED) # This is needed for visageSDK to find references
-find_package(LibHttpServer REQUIRED)
+#find_package(LibHttpServer REQUIRED)
 
-file(GLOB visageSDK_LIBRARIES ${visageSDK_DIR}/lib/*)
+file(GLOB visageSDK_LIBRARIES ${visageSDK_DIR}/lib/* )
 
 include_directories(include
-  SYSTEM ${QI_INCLUDE_DIRS}
-  ${OpenCV_INCLUDE_DIRS}
-  ${cv_bridge_INCLUDE_DIRS}
+  #SYSTEM ${QI_INCLUDE_DIRS}
+  #${OpenCV_INCLUDE_DIRS}
+  #${cv_bridge_INCLUDE_DIRS}
   ${visageSDK_DIR}/include
   ${ZLIB_INCLUDE_DIRS}
   ${OpenMP_INCLUDE_DIRS}
-  ${LIBHTTPSERVER_INCLUDE_DIRS}
-  "/workspace/httprequest/include"
-  "/workspace/nlohmannjson/single_include"
+  #${LIBHTTPSERVER_INCLUDE_DIRS}
+  #"/workspace/httprequest/include"
+  #"/workspace/nlohmannjson/single_include"
   )
-link_libraries(${visageSDK_LIBRARIES})
 
 # since the package installs libraries without exporting them
 # it needs to make sure that the library path is being exported
@@ -70,37 +70,63 @@ if(NOT WIN32)
     "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
 endif()
 
+# Link all libraries required for VisageSDK to build properly
+# Since this package revolves around using the VisageSDK this should be OK,
+# otherwise change this to target_link_libraries(Foo ... ) for specific executables
 
-add_executable(visage_server src/visage_server.cpp)
-target_link_libraries(visage_server 
-  ${OpenCV_LIBRARIES}
+# TODO: Find out which are actually needed, seems like it sometimes can run without some?
+link_libraries(
   ${visageSDK_LIBRARIES}
-  ${QI_LIBRARIES}
-  ${Boost_LIBRARIES}
-  ${cv_bridge_LIBRARIES}
+  #${BOOST_LIBRARIES}
   ${ZLIB_LIBRARIES}
-  ${LIBHTTPSERVER_LIBRARIES}
   OpenMP::OpenMP_CXX
-  Threads::Threads
-  ${CMAKE_DL_LIBS})
+  #Threads::Threads
+  #${CMAKE_DL_LIBS}
+  )
+  
+  # VisageSDK HTTPServer
+# add_executable(visage_server src/visage_server.cpp)
+# target_link_libraries(visage_server 
+#   ${OpenCV_LIBRARIES}
+#   ${visageSDK_LIBRARIES}
+#   ${QI_LIBRARIES}
+#   ${Boost_LIBRARIES}
+#   ${cv_bridge_LIBRARIES}
+#   ${ZLIB_LIBRARIES}
+#   ${LIBHTTPSERVER_LIBRARIES}
+#   OpenMP::OpenMP_CXX
+#   Threads::Threads
+#   ${CMAKE_DL_LIBS})
+
 
 
 add_executable(visage_analysis 
-  src/standalone_visage_face_analysis.cpp 
-  src/visage_face_analysis.cpp)
+  #src/standalone_visage_face_analysis.cpp 
+  #src/visage_face_analysis.cpp)
+  #src/standalone_visage_node.cpp
+  #src/visage_node.cpp)
+  #src/standalone_visage_service_server.cpp
+  #src/visage_service_server.cpp
+  src/visage_action.cpp
+  src/standalone_visage_action.cpp
+  )
+
+
 target_link_libraries(visage_analysis 
-  ${OpenCV_LIBRARIES}
-  ${QI_LIBRARIES}
-  ${Boost_LIBRARIES}
-  ${cv_bridge_LIBRARIES})
+  #${OpenCV_LIBRARIES}
+  #${QI_LIBRARIES}
+  #${cv_bridge_LIBRARIES}
+  )
 ament_target_dependencies(visage_analysis
   "rclcpp"
+  "rclcpp_action"
   "rclcpp_components"
   "sensor_msgs"
   "lhw_interfaces")
 
+
 install(TARGETS
-  visage_server
+  #visage_server
   visage_analysis # VISAGE
   DESTINATION lib/${PROJECT_NAME})
 

src/lhw_visage/Dockerfile

@@ -12,24 +12,25 @@ RUN apt update
 # RUN apt-get -y install libboost-all-dev
 
 # Get and build boost
-RUN cd / && wget https://deac-ams.dl.sourceforge.net/project/boost/boost/1.62.0/boost_1_62_0.tar.gz
-RUN cd / && tar zxvf boost_1_62_0.tar.gz && rm boost_1_62_0.tar.gz
-RUN cd /boost_1_62_0 \
- && ./bootstrap.sh --with-libraries=all --with-toolset=gcc \
- && ./b2 install -j 8
+RUN cd / && wget -O boost_1_80_0.tar.gz https://sourceforge.net/projects/boost/files/boost/1.80.0/boost_1_80_0.tar.gz/download
+#https://deac-ams.dl.sourceforge.net/project/boost/boost/1.62.0/boost_1_62_0.tar.gz
+RUN cd / && tar zxvf boost_1_80_0.tar.gz && rm boost_1_80_0.tar.gz
+RUN cd /boost_1_80_0 \
+&& ./bootstrap.sh --with-libraries=all --with-toolset=gcc \
+&& ./b2 install -j 8
 
 RUN apt-get install ros-foxy-diagnostic-updater ros-foxy-robot-state-publisher ros-foxy-image-geometry -y
 
 # Install naoqi c++ sdk
-RUN python3 -m pip install qibuild
-RUN mkdir -p /opt
-RUN cd /opt && git clone https://github.com/samiamlabs/libqi.git -b release-2.5
-RUN cd /opt/libqi && mkdir build && cd build && cmake .. -DQI_WITH_TESTS=OFF
-RUN cd /opt/libqi/build && make -j 8 && make install
+#RUN python3 -m pip install qibuild
+#RUN mkdir -p /opt
+#RUN cd /opt && git clone https://github.com/samiamlabs/libqi.git -b release-2.5
+#RUN cd /opt/libqi && mkdir build && cd build && cmake .. -DQI_WITH_TESTS=OFF
+#RUN cd /opt/libqi/build && make -j 8 && make install
 
-RUN cd /opt && git clone https://github.com/aldebaran/libqicore.git -b release-2.5
-RUN cd /opt/libqicore && mkdir build && cd build && cmake .. -DQI_WITH_TESTS=OFF
-RUN cd /opt/libqicore/build && make -j 8 && make install
+#RUN cd /opt && git clone https://github.com/aldebaran/libqicore.git -b release-2.5
+#RUN cd /opt/libqicore && mkdir build && cd build && cmake .. -DQI_WITH_TESTS=OFF
+#RUN cd /opt/libqicore/build && make -j 8 && make install
 
 # Install opencv
 RUN apt install unzip
@@ -60,6 +61,8 @@ RUN git clone https://github.com/etr/libhttpserver.git /workspace/libhttp
 RUN cd /workspace/libhttp && \
    ./bootstrap && mkdir build && cd build && ../configure && make && make install
 
+# Install HTTPRequest
+# TODO: Fix path to libs in workspace
 RUN git clone https://github.com/elnormous/HTTPRequest.git /workspace/httprequest
 
 # Install dependencies

src/lhw_visage/include/lhw_visage/visage_action.hpp

+#ifndef LHW_VISAGE_ACTION_HPP_
+#define LHW_VISAGE_ACTION_HPP_
+// #include <qi/applicationsession.hpp>
+// #include <qi/anyobject.hpp>
+
+
+#include "rclcpp_action/rclcpp_action.hpp"
+#include <lhw_interfaces/action/face_analysis.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <std_msgs/msg/bool.hpp>
+#include <lhw_interfaces/msg/face_detected.hpp>
+#include <lhw_interfaces/msg/faces_detected.hpp>
+#include <lhw_interfaces/msg/visage_settings.hpp>
+#include <VisageFaceRecognition.h>
+#include <VisageFeaturesDetector.h>
+#include <VisageFaceAnalyser.h>
+
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#define _USE_MATH_DEFINES
+#include <iostream>
+#include <string>
+#include <vector>
+
+#define DETECT_AND_RECOGNIZE lhw_interfaces::msg::VisageSettings::SETTING_DETECT_AND_RECOGNIZE
+#define RECOGNIZE lhw_interfaces::msg::VisageSettings::SETTING_RECOGNIZE 
+#define DETECT lhw_interfaces::msg::VisageSettings::SETTING_DETECT
+#define NOTHING lhw_interfaces::msg::VisageSettings::SETTING_NOTHING
+
+class VisageAction: public rclcpp::Node {
+   public:
+    using FaceAnalysisAction = lhw_interfaces::action::FaceAnalysis;
+    using Image = sensor_msgs::msg::Image;
+    using GoalHandleFaceAnalysis = rclcpp_action::ServerGoalHandle<FaceAnalysisAction>;
+
+    VisageAction(const rclcpp::NodeOptions & options);
+    ~VisageAction(); 
+    void initiate();
+
+   private:
+    rclcpp::Subscription<Image>::SharedPtr camera_sub_;
+    rclcpp_action::Server<FaceAnalysisAction>::SharedPtr actionFaceAnalysis_;
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const FaceAnalysisAction::Goal> goal_handle);
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle);
+    rclcpp_action::GoalResponse handle_accepted(const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle);
+    rclcpp_action::GoalResponse execute(const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle);
+
+
+
+    uint8_t setting = DETECT_AND_RECOGNIZE;
+    int MAX_FACES = 3; // TODO: Allow for this to be changed through a topic?
+    int DESCRIPTOR_SIZE; // Variable is set in initiate()
+    float RECOGNITION_THRESHOLD = 0.72f;
+    float DETECTION_THRESHOLD_BUFFER = 0.10f;
+    // Possibility to turn off age & gender estimation
+    bool estimate_age = true;
+    bool estimate_gender = true;
+    bool estimate_emotion = false;
+    // Data paths to the different Neural networks and license
+    const std::string data_path_features_detector = "/workspace/visageSDK/Samples/data";
+    const std::string data_path_face_recognition = "/workspace/visageSDK/Samples/data/bdtsdata/NN/fr.bin";
+    const std::string data_path_face_analyser = "/workspace/visageSDK/Samples/data/bdtsdata/LBF/vfadata";
+    const std::string license_key = "/workspace/visageSDK/bin/702-681-574-411-934-671-299-321-288-199-223.vlc";
+    //#"/workspace/visageSDK/bin/976-065-367-113-714-050-1response_answer 83-561-523-183-165.vlc"; // TODO: Ask Johan how to use this, I think he managed to get it to work
+    int new_face_id = 0;
+
+    // nlohmann::json j;
+    const char *image_data;
+
+    VisageSDK::VisageFaceRecognition *face_recognition_;
+    VisageSDK::VisageFeaturesDetector *features_detector_;
+    VisageSDK::VisageFaceAnalyser *face_analyser_;
+    bool answer;
+    lhw_interfaces::msg::FacesDetected faces_response;
+    lhw_interfaces::msg::FaceDetected buildFaceDetectedMessage(
+    int id,
+    int age,
+    int gender,
+    float similarity,
+    int z_index,
+    std::vector<int> face_bbox);
+    int addFace(short *descriptor); 
+    void calculateGenderAndAge(VsImage *vs_image_ptr, VisageSDK::FaceData *face_data_arr_ptr, int face_i, int &age_ref, int &gender_ref); 
+    int startDetection(
+    const typename sensor_msgs::msg::Image::SharedPtr msg,
+    VsImage *&vs_image,
+    VsRect *&faces,
+    VisageSDK::FaceData *&face_data,
+    lhw_interfaces::msg::FacesDetected &faces_msg);
+
+    void detection_callback (
+        const typename sensor_msgs::msg::Image::SharedPtr msg
+        );
+};
+
+#endif  

src/lhw_visage/include/lhw_visage/visage_face_analysis.hpp

-#ifndef LHW_VISAGE__VISAGE_FACE_ANALYSIS_HPP_
-#define LHW_VISAGE__VISAGE_FACE_ANALYSIS_HPP_
-
-#include "lhw_visage/visibility_control.h"
-
-#include <sensor_msgs/msg/image.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <lhw_interfaces/msg/face_detected.hpp>
-#include <lhw_interfaces/msg/faces_detected.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include <nlohmann/json.hpp>
-#include <opencv2/opencv.hpp>
-#include "HTTPRequest.hpp"
-#include "image_transport/image_transport.hpp"
-#include <cv_bridge/cv_bridge.h>
-
-#include <qi/applicationsession.hpp>
-#include <qi/anyobject.hpp>
-
-namespace lhw_visage
-{
-  class VisageAnalysis : public rclcpp::Node
-  {
-  public:
-    LHW_VISAGE_PUBLIC
-    explicit VisageAnalysis(const rclcpp::NodeOptions & options);
-    void initiate();
-  private:
-    lhw_interfaces::msg::FaceDetected buildFaceDetectedMessage(int id, int age, int gender, int z_index, std::pair<int,int> face_center);
-    
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_sub_;
-    rclcpp::Publisher<lhw_interfaces::msg::FacesDetected>::SharedPtr faces_pub_;
-  };
-
-} // namespace lhw_visage
-
-#endif // !LHW_VISAGE__VISAGE_FACE_ANALYSIS_HPP_

src/lhw_visage/include/lhw_visage/visage_server.hpp

-#ifndef LHW_VISAGE__VISAGE_SERVER_HPP_
-#define LHW_VISAGE__VISAGE_SERVER_HPP_
-
-#include "lhw_visage/visibility_control.h"
-
-#include <VisageFaceRecognition.h>
-#include <VisageFeaturesDetector.h>
-#include <VisageFaceAnalyser.h>
-
-#include <nlohmann/json.hpp>
-#include <opencv2/opencv.hpp>
-#include <httpserver.hpp>
-#include <cv_bridge/cv_bridge.h>
-
-
-#endif // !LHW_VISAGE__VISAGE_SERVER_HPP_

src/lhw_visage/package.xml

@@ -11,6 +11,7 @@
 
   <build_depend>rclcpp</build_depend>
   <build_depend>rclcpp_components</build_depend>
+  <build_depend>rclcpp_action</build_depend>
   <build_depend>rcutils</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>naoqi_bridge_msgs</build_depend>
@@ -19,6 +20,7 @@
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
   <exec_depend>rcutils</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 

src/lhw_visage/src/standalone_visage_face_analysis.cpp → src/lhw_visage/src/standalone_visage_action.cpp

-#include "lhw_visage/visage_face_analysis.hpp"
-
+#include "lhw_visage/visage_action.hpp"
 #include <rcutils/cmdline_parser.h>
 
 
@@ -8,18 +7,8 @@ int main(int argc, char * argv[])
   rclcpp::init(argc, argv);
 
   // Parse the command line options.
-  auto ip = std::string("10.133.5.240");
-  char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-i");
-  if (nullptr != cli_option) {
-    ip = std::string(cli_option);
-  }
-  auto port = std::string("9559");
-  cli_option = rcutils_cli_get_option(argv, argv + argc, "-p");
-  if (nullptr != cli_option) {
-    port = std::string(cli_option);
-  }
   
-  auto face_analysis = std::make_shared<lhw_visage::VisageAnalysis>(rclcpp::NodeOptions());
+  auto face_analysis = std::make_shared<VisageAction>(rclcpp::NodeOptions());
   face_analysis->initiate();
   RCUTILS_LOG_INFO("Session started");
   rclcpp::spin(face_analysis);

src/lhw_visage/src/visage_action.cpp

+ 
+#include "../include/lhw_visage/visage_action.hpp"
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+
+
+ 
+    VisageAction::VisageAction(const rclcpp::NodeOptions & options) : Node("VisageAction", options)
+{
+    this->answer = false;
+    this->actionFaceAnalysis_ = rclcpp_action::create_server<FaceAnalysisAction>(
+        this,
+        "face_analysis",
+        std::bind(&VisageAction::handle_goal, this, _1, _2),
+        std::bind(&VisageAction::handle_cancel, this, _1),
+        std::bind(&VisageAction::handle_accepted, this, _1)
+        );
+
+    
+    RCLCPP_INFO(this->get_logger(), "face action server started");
+}
+
+
+rclcpp_action::GoalResponse VisageAction::handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const FaceAnalysisAction::Goal> goal
+){
+
+    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->setting);
+    (void)uuid;
+    this->answer = false; 
+
+    // Creating a camera sub
+    camera_sub_ = create_subscription<sensor_msgs::msg::Image>("image", 10, std::bind(
+        &VisageAction::detection_callback, this, _1
+    ));
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+  rclcpp_action::CancelResponse VisageAction::handle_cancel(
+    const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle)
+  {
+
+    //this->destroy_subscription(camera_sub_);
+    //delete(camera_sub_);
+    camera_sub_.reset();
+    camera_sub_ = NULL;
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+  
+rclcpp_action::GoalResponse VisageAction::handle_accepted(const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle)
+  {
+    using namespace std::placeholders;
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&VisageAction::execute, this, _1), goal_handle}.detach();
+  }
+
+
+rclcpp_action::GoalResponse VisageAction::execute(const std::shared_ptr<GoalHandleFaceAnalysis> goal_handle) {
+    RCLCPP_INFO(this->get_logger(), "Executing goal for face analysis");
+    rclcpp::Rate loop_rate(1);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<FaceAnalysisAction::Feedback>();
+    auto & done = feedback->done;
+    done = false;
+    auto result = std::make_shared<FaceAnalysisAction::Result>();
+
+    // Check when we are done.
+    while(!this->answer) {
+        // if (this->answer) {
+        //     RCLCPP_INFO(this->get_logger(), "it is true");
+        // }
+        // else {
+        //     RCLCPP_INFO(this->get_logger(), "it is false");
+        // }
+
+    }
+    camera_sub_ = NULL;
+    done = true;
+    result->faces = faces_response;
+    goal_handle->succeed(result);
+}
+
+
+void VisageAction::detection_callback(
+    const typename sensor_msgs::msg::Image::SharedPtr msg)
+{
+    RCLCPP_INFO(this->get_logger(), "We are running detection_callback");
+    std::cout << "Executing visage" << std::endl;
+    std::cout << "-----------------" << std::endl;
+    lhw_interfaces::msg::FacesDetected detected_faces_msg;
+    VsImage *vs_image = NULL;
+    VsRect *face_rects = NULL;
+	VisageSDK::FaceData *face_data = NULL;
+    const int n_faces = startDetection(msg, vs_image, face_rects, face_data, detected_faces_msg);
+    if (n_faces == 0) {
+        this->answer =false;
+        return;
+    }
+
+    /* WORK BEING DONE */
+    for (int face_i = 0; face_i < n_faces; face_i++)
+    {
+        // Extract face descriptor (vector) from each FaceData object
+        short* descriptor = new short[DESCRIPTOR_SIZE]; // Face descriptor 'vector'
+        int status = face_recognition_->extractDescriptor(&face_data[face_i], vs_image, descriptor);
+        if (status == 0)
+        {
+            std::cout << "ERROR: Failed extracting the descriptor. Checking next face instead..." << std::endl;
+            continue;
+        }
+
+        /* ALLOCATING DATA */
+        const char **names = new const char*[MAX_FACES];
+        float *similarities = new float[MAX_FACES];
+        // Only checks the first similarity since the list is sorted
+        int n_similar_faces = face_recognition_->recognize(descriptor, MAX_FACES, names, similarities);
+
+        // TODO: If any of these are turned off during runtime, it will start printing failure messages
+        int age = -1;
+        int gender = -1;
+        calculateGenderAndAge(vs_image, face_data, face_i, age, gender);
+        std::cout << "Face index: " << face_i << std::endl;
+        
+    RCLCPP_INFO(this->get_logger(), "request for face sucess");
+        float similarity = -1.0f;
+        int id = -1;
+        /* 
+        Similarity and Name are junk pointers when no person has been detected before, 
+        this was one of the issues we had in bangkok. Now this is handled separately 
+        and should not cause any more problems
+        */
+        if (n_similar_faces == 0)
+        {
+            std::cout << "First person found" << std::endl;
+            id = addFace(descriptor);
+            similarity = 1.0f;
+        }
+        else
+        {
+            
+    RCLCPP_INFO(this->get_logger(), "request for face sucess");
+        
+            /* ERROR "HANDLING" */
+            if (similarities[0] > 1.00 || age == -1 || gender == -1) 
+            {
+                // Warning for if something goes wrong in any of the analysis steps.
+                std::cout << "***********" << std::endl;
+                std::cout << "  FAILURE " << std:: endl;
+                if (similarities[0] >  1.00) {
+                    std::cout << "Similarity FAILED" << std::endl;
+                    std::cout << similarities[0] << std::endl;
+                }
+                if (age == -1) {
+                    std::cout << "Age FAILED" << std::endl;
+                }
+                if (gender == -1) {
+                    std::cout << "Gender FAILED" << std::endl;
+                }
+                std::cout << "***********" << std::endl;
+            }
+
+            
+            bool has_recognized_face = similarities[0] >= RECOGNITION_THRESHOLD;
+            // Buffer for making it harder to get false signals of a new person being found
+            bool has_detected_new_face = similarities[0] <= RECOGNITION_THRESHOLD - DETECTION_THRESHOLD_BUFFER;
+
+            switch(setting) 
+            {
+    RCLCPP_INFO(this->get_logger(), "request for face sucess");
+                case RECOGNIZE: 
+                {
+                    if (has_recognized_face)
+                    {
+                        // We have recognized a person
+                        std::cout << "Recognized a person!" << std::endl;
+                        std::cout << "Similiarity: " << similarities[0] << std::endl;
+                        std::cout << "Recognized person: " << names[0] << std::endl;
+                        similarity = similarities[0];
+                        id = stoi(names[0]);
+                    }
+                    else
+                    {
+                        /* 
+                        Recognition is too low, none of the following similarities will be higher.
+                        So continue with next face
+                        */
+                        continue;
+                    }
+                    
+                    break;
+                }
+                case DETECT:
+                {
+                    // Below recognition threshold => consider this as new face
+                    if (has_detected_new_face)
+                    {
+                        id = addFace(descriptor);
+                        similarity = 1.0f; // Not entirely sure how to handle this case 
+                    }
+                    else {
+                        /* 
+                        Recognition too high, do not add this person as a new ID!
+                        Continue with the next face
+                        */
+                        continue;
+                    }
+                    break;
+                }
+    RCLCPP_INFO(this->get_logger(), "request for face sucess");
+                case DETECT_AND_RECOGNIZE:
+                {
+                    if (has_detected_new_face) 
+                    {
+                        id = addFace(descriptor);
+                        similarity = 1.0f; // Not entirely sure how to handle this case 
+                    }
+                    /* 
+                    This method is less accurate of recognizing a face than doing the other ones separately because
+                    handling the corner case of the gray zone in between has_detected_new_face and has_recognized_face 
+                    (see variables above and threshold values) lead to new faces being recognized above 
+                    RECOGNITION_THRESHOLD - DETECTION_THRESHOLD_BUFFER
+                    */
+                    else 
+                    {
+                        // We have recognized a person
+                        std::cout << "Recognized a person!" << std::endl;
+                        std::cout << "Similiarity: " << similarities[0] << std::endl;
+                        std::cout << "Recognized person: " << names[0] << std::endl;
+                        similarity = similarities[0];
+                        id = stoi(names[0]);
+                    }
+                    break;
+                }
+                default:
+                {
+                    std::cout << "This 'should' not happen" << std::endl;
+                }
+            }
+        }
+
+        // A person was recognized or detected.
+        if (id != -1 && similarity != -1.0f)
+        {
+            // TODO: Any way to find z-index of each face similiar to how it was with DeepFace??
+            // We need some way of finding out who is closest to the camera, but this is maybe not possible/necessary
+            // if visage deals with this itself
+            VsRect *current_face_rect = &face_rects[face_i];
+            std::vector<int> face_bbox {
+                current_face_rect->x, 
+                current_face_rect->y, 
+                current_face_rect->x + current_face_rect->width, 
+                current_face_rect->y - current_face_rect->height
+            };
+            //int face_center_x = current_face_rect->x + current_face_rect->width / 2;
+            //int face_center_y = current_face_rect->y - current_face_rect->height / 2;
+            lhw_interfaces::msg::FaceDetected detected_face_msg = buildFaceDetectedMessage(
+                id,
+                age, 
+                gender,
+                similarity,
+                face_i, 
+                face_bbox
+            );
+            detected_faces_msg.faces_detected.push_back(detected_face_msg);
+            //single_face_pub_->publish(detected_face_msg);
+
+            faces_response = detected_faces_msg;
+        }
+
+        // Memory deallocation
+        delete[] similarities;
+        delete[] names;
+        delete[] descriptor;
+    }
+
+    //multi_face_pub_->publish(detected_faces_msg);
+    faces_response = detected_faces_msg;
+    this->answer = true;
+    // Memory deallocation
+    delete[] face_data;
+    delete[] face_rects;
+    vsReleaseImageHeader(&vs_image);
+    std::cout << "-----------------" << std::endl;
+}
+
+int VisageAction::startDetection(
+    const typename sensor_msgs::msg::Image::SharedPtr msg,
+    VsImage *&vs_image,
+    VsRect *&faces,
+    VisageSDK::FaceData *&face_data,
+    lhw_interfaces::msg::FacesDetected &faces_msg) {
+    const int width = msg->width;
+    const int height = msg->height;
+    const int depth = 8;
+    const int channels = 3;
+    const int step = msg->step;
+    const char *data = (char *)(const_cast<unsigned char *>(&msg->data[0]));
+
+    // Creating the VsImage
+    vs_image = vsCreateImageHeader(vsSize(width, height), depth, channels);
+    vsSetData(vs_image, (void *)data, step);
+
+    // Converts image to RGB8 format
+    if (msg->encoding == "bgr8") {
+        vsCvtColor(vs_image, vs_image, VS_BGR2RGB);
+    }
+    // Used to send coordinates of the face in the message
+    // TODO: Needs to be checked if the index of the for loop below corresponds to the correct face rectangle
+    faces = new VsRect[MAX_FACES];
+    const int n_faces = features_detector_->detectFaces(vs_image, faces, MAX_FACES);
+
+    // Retrieve the face data for each face
+
+    face_data = new VisageSDK::FaceData[MAX_FACES];
+    const int n_face_features = features_detector_->detectFacialFeatures(vs_image, face_data, MAX_FACES);
+
+    std::cout << "Amount of faces found: " << n_faces << std::endl;
+    // Make sure that both face analysis parts find the same amount of faces
+    if (n_face_features != n_faces) {
+        // TODO: Error handling
+        std::cout << "ERROR: Number of faces found does not match number of facial features" << std::endl;
+    }
+
+    // Create msg for all faces
+    faces_msg = lhw_interfaces::msg::FacesDetected();
+    faces_msg.source_width = width;
+    faces_msg.source_height = height;
+
+    return n_faces;
+}
+
+void VisageAction::calculateGenderAndAge(VsImage *vs_image_ptr, VisageSDK::FaceData *face_data_arr_ptr, int face_i, int &age_ref, int &gender_ref) {
+    if (estimate_age) {
+        // TODO: Line below segfaults
+        age_ref = face_analyser_->estimateAge(vs_image_ptr, &face_data_arr_ptr[face_i]);
+    }
+
+    if (estimate_gender) {
+        // Gender is represented as 0: Female, 1: Male
+        gender_ref = face_analyser_->estimateGender(vs_image_ptr, &face_data_arr_ptr[face_i]);
+    }
+}
+
+int VisageAction::addFace(short *descriptor) {
+    std::cout << "Found new person with ID: " << new_face_id << std::endl;
+    // We have found a new person, add their descriptor
+    int person_id = new_face_id;
+    bool success = face_recognition_->addDescriptor(descriptor, std::to_string(person_id).c_str());
+
+    if (!success) {
+        std::cout << "ERROR: Adding new face failed!" << std::endl;
+        return -1;
+    }
+    new_face_id++;
+    return person_id;
+}
+
+lhw_interfaces::msg::FaceDetected VisageAction::buildFaceDetectedMessage(
+    int id,
+    int age,
+    int gender,
+    float similarity,
+    int z_index,
+    std::vector<int> face_bbox) {
+    auto msg = lhw_interfaces::msg::FaceDetected();
+    msg.source = lhw_interfaces::msg::FaceDetected::VISAGE;
+    msg.id = id;
+    msg.similarity = similarity;
+    msg.z_index = z_index;
+    msg.age = age;
+    // Gender: -1: Failure, 0: Female, 1: Male
+    msg.gender = gender == -1 ? "NONE" : (gender == 0 ? "Female" : "Male");
+    msg.estimate_age = estimate_age;
+    msg.estimate_gender = estimate_gender;
+    for (int i = 0; i < 4; i++) {
+        msg.face_bbox[i] = face_bbox[i];
+    }
+    // TODO: Remove this and let others handle calculation if need be?
+    msg.face_center_xy[0] = (face_bbox[2] + face_bbox[0]) / 2;
+    msg.face_center_xy[1] = (face_bbox[3] + face_bbox[1]) / 2;
+    return msg;
+}
+
+VisageAction::~VisageAction() {
+    delete features_detector_;
+    delete face_recognition_;
+    delete face_analyser_;
+}
+
+void VisageAction::initiate() {
+    // If a failure happens, the biggest risk is that a license key has expired or all available
+    // licenses have been used up and Visage Technologies should be contacted ASAP
+    VisageSDK::initializeLicenseManager(license_key.c_str());
+    std::cout << "License initialized" << std::endl;
+
+    features_detector_ = new VisageSDK::VisageFeaturesDetector();
+    if (!features_detector_->Initialize(data_path_features_detector.c_str())) {
+        // FAILURE
+        std::cout << "Features Detector FAILED" << std::endl;
+    }
+    std::cout << "Features Detector initialized" << std::endl;
+
+    face_recognition_ = new VisageSDK::VisageFaceRecognition(data_path_face_recognition.c_str());
+    if (!face_recognition_->is_initialized) {
+        // FAILURE
+        std::cout << "Face Recognition FAILED" << std::endl;
+    }
+    DESCRIPTOR_SIZE = face_recognition_->getDescriptorSize();
+    std::cout << "Face Recognition initialized" << std::endl;
+
+    face_analyser_ = new VisageSDK::VisageFaceAnalyser();
+    if (!face_analyser_->init(data_path_face_analyser.c_str())) {
+        // FAILURE
+        std::cout << "Face Analyser FAILED" << std::endl;
+    }
+    std::cout << "Face Analyser initialized" << std::endl;
+
+    std::cout << "Complete" << std::endl;
+}
+
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+
+//RCLCPP_COMPONENTS_REGISTER_NODE(VisageAnalysisServer)
\ No newline at end of file

src/lhw_visage/src/visage_face_analysis.cpp

-#include "lhw_visage/visage_face_analysis.hpp"
-
-#define _USE_MATH_DEFINES
-#include <string>
-#include <iostream>
-#include <vector>
-
-namespace lhw_visage
-{
-  VisageAnalysis::VisageAnalysis(const rclcpp::NodeOptions & options)
-    : Node("VisageAnalysis", options)
-  { 
-
-    auto camera_callback = [this](const typename sensor_msgs::msg::Image::SharedPtr msg) -> void
-      {
-        //std::cout << cv_bridge::toCvCopy(msg, "rgb8")->header.frame_id << std::endl;
-        //const cv::Mat *input_img = &cv_bridge::toCvCopy(msg, msg->encoding)->image;
-        nlohmann::json j;
-        //RCUTILS_LOG_INFO((const char*)msg->encoding);
-        j["cols"] = msg->width;
-        j["rows"] = msg->height;
-        j["depth"] = 8;
-        j["channels"] = 3;
-        j["step"] = msg->step;
-        
-        http::Request request{"http://localhost:8080/visage"};
-        
-        //TODO: This might need to be sped up a bit, not sure how though
-        RCUTILS_LOG_INFO("Send POST request");
-        const auto data_response = request.send("POST", msg->data, {
-          {"Content-Type", "Data"}
-        });
-        const auto response = request.send("POST", j.dump(), {
-            {"Content-Type", "Json"}
-        });
-        RCUTILS_LOG_INFO("Recieved POST request");
-        
-        if( response.status.code == 200){
-
-        
-          nlohmann::json json_response = nlohmann::json::parse(std::string{response.body.begin(), response.body.end()});
-
-          auto faces_msg = lhw_interfaces::msg::FacesDetected();
-          faces_msg.source_width = msg->width;
-          faces_msg.source_height = msg->height;
-          // Z-index of these are formed as "0": closest to the camera, ... , "n_faces-1": furthest away
-          // TODO: Test this more just in case so it is correct
-          for (int i = 0; i < json_response["n_faces"]; i++) {
-            nlohmann::json json_person = json_response[std::to_string(i)];
-            auto face_msg = buildFaceDetectedMessage(json_person["id"], json_person["age"], json_person["gender"], i, std::make_pair<int,int>(json_person["face_center_xy"][0], json_person["face_center_xy"][1]));
-            faces_msg.faces_detected.push_back(face_msg);
-          }
-          // std::cout << json_response["0"]["age"] << std::endl;
-          faces_pub_->publish(faces_msg);
-          RCUTILS_LOG_INFO("Published Message");
-        }
-      };
-
-    camera_sub_ = create_subscription<sensor_msgs::msg::Image>("image", 10, camera_callback);
-    faces_pub_ = create_publisher<lhw_interfaces::msg::FacesDetected>("faces_detected", 10);
-  }
-
-  void VisageAnalysis::initiate()
-  {
-    RCUTILS_LOG_INFO("JAG ÄLSKAR BYGGPROBLEM :')");
-  }
-
-  // Fills all the necessary parts of a FaceDetected message
-  // Gender: 0 = Female, 1 = Male
-  lhw_interfaces::msg::FaceDetected VisageAnalysis::buildFaceDetectedMessage(int id, int age, int gender, int z_index, std::pair<int,int> face_center) 
-  {
-    auto msg = lhw_interfaces::msg::FaceDetected();
-    msg.id = id;
-    msg.age = age;
-    msg.gender = gender == 0 ? "Female" : "Male";
-    msg.z_index = z_index;
-    msg.face_center_xy[0] = std::get<0>(face_center);
-    msg.face_center_xy[1] = std::get<1>(face_center);
-    return msg;
-  }
-  
-}  // namespace lhw_visage
-
-
-#include "rclcpp_components/register_node_macro.hpp"
-
-// Register the component with class_loader.
-// This acts as a sort of entry point, allowing the component to be discoverable when its library
-// is being loaded into a running process.
-
-RCLCPP_COMPONENTS_REGISTER_NODE(lhw_visage::VisageAnalysis)

src/lhw_visage/src/visage_server.cpp

-#include <lhw_visage/visage_server.hpp>
-#include <iostream>
-
-/*
-    HTTP webserver for taking requests for the use of VisageSDK Face Analysis.
-    --------/ IMPORTANT \--------
-        The HTTP Server needs to be started before the ros2 node is ran, 
-        since otherwise "internal errors" can be caused. 
-        Have not concluded the reason for this
-
-    --------/ Sending a HTTP Request \--------
-    To send a request 2 different POST requests need to be sent:
-        1. First a POST request containing the image data in the content of the request 
-        with the header "Data". FIXME: Would be nice if it decreased to 1, but extracting 
-        the image data from the json object proved difficult
-        2. Lastly a POST request with a json in in the content of the request in the form of:
-            {
-                "cols": <int>,
-                "depth": <int>,
-                "rows": <int>,
-                "channels": <int>,
-                "step": <int>
-            }
-
-    --------/ Recieving a HTTP Response \--------
-    Request 2. will return a HTTP Response as a json string in the form of:
-        {
-            "n_faces": <int>
-            "0":{
-                    "age": <int>,
-                    "face_center_xy": [<int>, <int>],
-                    "gender": <int>,
-                    "id": <int>
-                },
-            .
-            .
-            n_faces
-        }
-        Where n_faces is the amount of recognized faces in the json object
-*/
-class VisageResource : public httpserver::http_resource 
-{
-    private:
-        const int MAX_FACES = 3; // TODO: Allow for this to be changed through a topic?
-        const float RECOGNITION_THRESHOLD = 0.72f;
-        // Data paths to the different Neural networks and license
-        const std::string data_path_features_detector = "/workspace/visageSDK/Samples/data";
-        const std::string data_path_face_recognition = "/workspace/visageSDK/Samples/data/bdtsdata/NN/fr.bin";
-        const std::string data_path_face_analyser = "/workspace/visageSDK/Samples/data/bdtsdata/LBF/vfadata";
-        const std::string license_key = "/workspace/visageSDK/bin/139-528-498-645-520-379-216-717-461-716-217.vlc";
-        //#"/workspace/visageSDK/bin/976-065-367-113-714-050-183-561-523-183-165.vlc"; // TODO: Ask Johan how to use this, I think he managed to get it to work
-        int new_face_id = 0;
-
-        nlohmann::json j;
-        const char *image_data;
-
-        VisageSDK::VisageFaceRecognition *face_recognition_;
-        VisageSDK::VisageFeaturesDetector *features_detector_;
-        VisageSDK::VisageFaceAnalyser *face_analyser_;
-
-    public:
-        const std::shared_ptr<httpserver::http_response> render_POST(const httpserver::http_request& req) 
-        {
-            // Image data needs to be sent before doing a face recognition request
-            // Since there was trouble parsing it from the json object (probably too large)
-            if (req.get_header("Content-Type") == "Data") {
-                image_data = req.get_content().c_str();
-                return std::shared_ptr<httpserver::http_response>(new httpserver::string_response("Success"));
-            }
-            else if (face_recognition_->is_initialized) {
-                std::cout << "Executing visage" << std::endl;
-                std::cout << "-----------------" << std::endl;
-                j = nlohmann::json::parse(req.get_content());
-
-                // Creating the VsImage
-                VsImage *vs_image;
-                vs_image = vsCreateImageHeader(vsSize(j["cols"], j["rows"]), j["depth"], j["channels"]);
-                vsSetData(vs_image, (void*) image_data, j["step"]);
-                vsCvtColor(vs_image, vs_image, VS_BGR2RGB); // Converts image to RGB8 format
-                // Used to send coordinates of the face in the message
-                // TODO: Needs to be checked if the index of the for loop below corresponds to the correct face rectangle
-                VsRect *faces = new VsRect[MAX_FACES];
-                int n_faces = features_detector_->detectFaces(vs_image, faces, MAX_FACES);
-
-                // Retrieve the face data for each face
-                int DESCRIPTOR_SIZE = face_recognition_->getDescriptorSize();
-                VisageSDK::FaceData *face_data = new VisageSDK::FaceData[MAX_FACES];
-                int n_face_features = features_detector_->detectFacialFeatures(vs_image, face_data, MAX_FACES);
-                nlohmann::json json_people;
-                json_people["n_faces"] = n_faces;
-
-                std::cout << "Faces: " << n_faces << std::endl;
-                // Make sure that both face analysis parts find the same amount of faces
-                if (n_face_features == n_faces) {
-                    for (int face_i = 0; face_i < n_faces; face_i++)
-                    {
-                        // Extract face descriptor (vector) from each FaceData
-                        short* descriptor = new short[DESCRIPTOR_SIZE]; // Face descriptor 'vector'
-                        int status = face_recognition_->extractDescriptor(&face_data[face_i], vs_image, descriptor);
-                        if (status == 0) {
-                            // It has failed extracting the descriptor
-                        }
-                        // Check how similar each face is
-                        const char **names = new const char*[MAX_FACES];
-                        float *similarities = new float[MAX_FACES];
-                        int n_similar_faces = face_recognition_->recognize(descriptor, MAX_FACES, names, similarities);
-
-                        // TODO: Line below segfaults
-                        int age = face_analyser_->estimateAge(vs_image, &face_data[face_i]);
-                        // Gender is represented as 0: Female, 1: Male
-                        int gender = face_analyser_->estimateGender(vs_image, &face_data[face_i]);
-                        
-                        // TODO: Any way to find z-index of each face similiar to how it was with DeepFace??
-                        // We need some way of finding out who is closest to the camera, but this is maybe not possible/necessary
-                        // if visage deals with this itself
-                        nlohmann::json json_person;
-                        VsRect *face = &faces[face_i];
-                        int face_center_x = face->x + face->width / 2;
-                        int face_center_y = face->y - face->height / 2;
-                        //int curr_id;
-                        std::cout << "Face: " << face_i << std::endl;
-                        std::cout << "Age: " << age << std::endl; 
-                        std::cout << "Gender: " << gender << std::endl;
-                        if (similarities[0] > 1.00 || age == -1 || gender == -1) {
-                            // Failsafe for if something goes wrong in any of the analysis steps.
-                            // Prevents the json parsing from crashing, which in turn crashes the node.
-                            std::cout << "**********" << std::endl;
-                            std::cout << "  FAILURE " << std:: endl;
-                            if (similarities[0] >  1.00) {
-                                std::cout << "Similarity" << std::endl;
-                            }
-                            if (age == -1) {
-                                std::cout << "Age" << std::endl;
-                            }
-                            if (gender == -1) {
-                                std::cout << "Gender" << std::endl;
-                            }
-                            std::cout << "  Failed" << std::endl;
-                            std::cout << "**********" << std::endl;
-                            return std::shared_ptr<httpserver::http_response>(new httpserver::string_response("Error - bad values", 400));
-                            
-                        }
-                        // Do we really need to check more than the similarity of the first if the lists are sorted?
-                        if (similarities[0] > RECOGNITION_THRESHOLD) {
-                            // We have recognized a person
-                            std::cout << "Similiarity: " << similarities[0] << std::endl;
-                            std::cout << "Recognized person: " << names[0] << std::endl;
-                            
-                            json_person = {
-                                {"id", stoi(names[0])},
-                                {"age", age},
-                                {"gender", gender},
-                                {"face_center_xy", {face_center_x, face_center_y}}
-                            };
-                            //curr_id = stoi(names[0]);
-                            // TODO: Anything more to do?
-                        }
-                        else {
-                            std::cout << "Found new person with id " << new_face_id << std::endl;
-                            // We have found a new person, add their descriptor
-                            face_recognition_->addDescriptor(descriptor, std::to_string(new_face_id).c_str());
-                            json_person = {
-                                {"id", new_face_id},
-                                {"age", age},
-                                {"gender", gender},
-                                {"face_center_xy", {face_center_x, face_center_y}}
-                            };
-                            //curr_id = new_face_id;
-                            new_face_id++;
-                        }
-                        json_people[std::to_string(face_i)] = json_person;
-                    }
-                    // Send all people in a single json
-                    std::cout << json_people.dump() << std::endl;
-                    // TODO: Clear all allocated memory when finished
-                    auto response = new httpserver::string_response(json_people.dump(), 200);
-                    
-                    return std::shared_ptr<httpserver::http_response>(response);
-                }
-                else {
-                    auto response = new httpserver::string_response("Face features not matching", 400);
-                    
-                    return std::shared_ptr<httpserver::http_response>(response);
-                }
-            }
-            return std::shared_ptr<httpserver::http_response>(new httpserver::string_response("Error - Face Recognition not initialized", 400));
-        }
-
-        void initiate()
-        {
-            VisageSDK::initializeLicenseManager(license_key.c_str());
-            std::cout << "License initialized" << std::endl;
-            features_detector_ = new VisageSDK::VisageFeaturesDetector();
-            features_detector_->Initialize(data_path_features_detector.c_str());
-            std::cout << "Features Detector initialized" << std::endl;
-            face_recognition_ = new VisageSDK::VisageFaceRecognition(data_path_face_recognition.c_str());
-            std::cout << "Face Recognition initialized" << std::endl;
-            face_analyser_ = new VisageSDK::VisageFaceAnalyser();
-            face_analyser_->init(data_path_face_analyser.c_str());
-            std::cout << "Face Analyser initialized" << std::endl;
-            std::cout << "Server initialized" << std::endl;
-        }
-
-        ~VisageResource()
-        {
-            delete features_detector_;
-            delete face_recognition_;
-            delete face_analyser_;
-        }
-    
-};
-
-int main() {
-    httpserver::webserver ws = httpserver::create_webserver(8080);
-    VisageResource vsr;
-    vsr.initiate();
-    ws.register_resource("/visage", &vsr);
-    ws.start(true);
-    
-    return 0;
-}
\ No newline at end of file

src/lhw_vision/Dockerfile

@@ -36,7 +36,8 @@ RUN git clone https://github.com/Parskatt/ByteTrack /workspace/bytetrack && \
     pip3 install cython_bbox;
 
 # Install Yolo5
-RUN git clone https://github.com/ultralytics/yolov5 -b /workspace/yolov5 && \
+RUN pip3 install ipython
+RUN git clone https://github.com/ultralytics/yolov5 /workspace/yolov5 && \
     cd /workspace/yolov5 && \
     pip3 install pandas seaborn && \
     python3 -c "from models.experimental import attempt_load; attempt_load('yolov5l.pt')" ;

src/lhw_vision/lhw_vision/yolo_node.py

@@ -22,7 +22,6 @@ from yolov5.utils.torch_utils import select_device
 from yolov5.utils.augmentations import letterbox
 cudnn.benchmark = True
 
-
 class YoloDetector(Node):
     """
         YOLOv5 (You only look once) using a submoduled package written in PyTorch.
@@ -34,8 +33,8 @@ class YoloDetector(Node):
         self.log = self.get_logger()
         self.cv_bridge = CvBridge()
 
-        self.device = select_device('')
-        self.model = attempt_load('/workspace/yolov5/yolov5l.pt', map_location=self.device)  # load FP32 model
+        self.device = select_device('cuda:0')
+        self.model = attempt_load('/workspace/yolov5/yolov5l.pt').cuda() #, map_location=self.device)  # load FP32 model
         self.model.half()  # Only half precision supported on cuda
         self.original_size = None
         self.rescale_size = None
@@ -109,7 +108,7 @@ class YoloDetector(Node):
         image = torch.from_numpy(image).to(self.device)
         image = image.half()/255.0  # .half() uint8 to fp16/32
         image = image.unsqueeze(0)
-
+        
         # Do the inference
         prediction = self.model(image)[0]