Added labels to the map

.github/workflows/pull-request-audit.yml

@@ -2,73 +2,70 @@ name: pull-request-audit
 
 on:
   push:
-    branches: [ main ]
-  
+    branches: [main]
+
   pull_request:
-    branches: [ main ]
-    
+    branches: [main]
+
   workflow_dispatch:
-    branches: [ main ]
+    branches: [main]
 
 jobs:
   build:
-
     runs-on: ubuntu-20.04
-  
+
     steps:
-    - uses: actions/checkout@v2
-      with:
-        submodules: recursive
-        path: catkin_ws/src/opentera-webrtc-ros
-    - uses: ros-tooling/[email protected]
-      with:
-        required-ros-distributions: noetic
-
-    - uses: actions/setup-node@v2
-      with:
-        node-version: '14'
-
-    - name: Install system dependencies
-      run: | 
-        sudo apt-get update
-        sudo apt-get install nodejs ros-noetic-turtlebot3 ros-noetic-turtlebot3-gazebo ros-noetic-dwa-local-planner ros-noetic-rtabmap-ros
-        sudo apt-get install libglib2.0-dev libgtk-3-dev libpulse-dev libasound2-dev
-        sudo apt-get install python3-pip portaudio19-dev
-        sudo apt-get install nodejs npm
-        sudo apt-get install build-essential gfortran texinfo libasound2-dev
-
-    - name: Clone audio_utils in ROS workspace
-      working-directory: catkin_ws/src
-      run: |
-        source /opt/ros/noetic/setup.bash
-        git clone https://github.com/introlab/audio_utils.git --recurse-submodules
-        ls -l
-
-    - name: Install Python requirements for OpenTera client
-      working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_client_ros
-      run: |
-        ls -l
-        python3 -m pip install -r requirements.txt 
-
-    - name: Install Python requirements for Signaling server
-      working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_ros/opentera-webrtc/signaling-server
-      run: |
-        ls -l
-        python3 -m pip install -r requirements.txt    
+      - uses: actions/checkout@v2
+        with:
+          submodules: recursive
+          path: catkin_ws/src/opentera-webrtc-ros
+      - uses: ros-tooling/[email protected]
+        with:
+          required-ros-distributions: noetic
+
+      - uses: actions/setup-node@v2
+        with:
+          node-version: "14"
+
+      - name: Install system dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install nodejs ros-noetic-turtlebot3 ros-noetic-turtlebot3-gazebo ros-noetic-dwa-local-planner ros-noetic-rtabmap-ros
+          sudo apt-get install libglib2.0-dev libgtk-3-dev libpulse-dev libasound2-dev
+          sudo apt-get install python3-pip portaudio19-dev
+          sudo apt-get install nodejs npm
+          sudo apt-get install build-essential gfortran texinfo libasound2-dev
+
+      - name: Clone audio_utils in ROS workspace
+        working-directory: catkin_ws/src
+        run: |
+          source /opt/ros/noetic/setup.bash
+          git clone https://github.com/introlab/audio_utils.git --recurse-submodules
+          ls -l
+
+      - name: Install Python requirements for OpenTera client
+        working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_client_ros
+        run: |
+          ls -l
+          python3 -m pip install -r requirements.txt
 
-    - name: Install the VUE.js frontend
-      working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_demos/opentera-webrtc-teleop-frontend/teleop-vue
-      run: |
-        ls -l
-        npm --version
-        node --version
-        npm install
-        npm run build  
-
-    - name: Compile packages in ROS workspace
-      working-directory: catkin_ws
-      run: |
-        source /opt/ros/noetic/setup.bash
-        catkin_make -j1
+      - name: Install Python requirements for Signaling server
+        working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_ros/opentera-webrtc/signaling-server
+        run: |
+          ls -l
+          python3 -m pip install -r requirements.txt
 
+      - name: Install the VUE.js frontend
+        working-directory: catkin_ws/src/opentera-webrtc-ros/opentera_webrtc_demos/opentera-webrtc-teleop-frontend/teleop-vue
+        run: |
+          ls -l
+          npm --version
+          node --version
+          npm install
+          npm run build
 
+      - name: Compile packages in ROS workspace
+        working-directory: catkin_ws
+        run: |
+          source /opt/ros/noetic/setup.bash
+          catkin_make -j1

.gitignore

@@ -30,3 +30,7 @@
 *.exe
 *.out
 *.app
+
+# Compiled Python
+__pycache__
+*.pyc

map_image_generator/CMakeLists.txt

@@ -134,6 +134,7 @@ add_executable(map_image_generator
   include/map_image_generator/drawers/GlobalPathImageDrawer.h
   include/map_image_generator/drawers/GoalImageDrawer.h
   include/map_image_generator/drawers/ImageDrawer.h
+  include/map_image_generator/drawers/LabelImageDrawer.h
   include/map_image_generator/drawers/LaserScanImageDrawer.h
   include/map_image_generator/drawers/OccupancyGridImageDrawer.h
   include/map_image_generator/drawers/RobotImageDrawer.h
@@ -145,6 +146,7 @@ add_executable(map_image_generator
   src/drawers/GlobalPathImageDrawer.cpp
   src/drawers/GoalImageDrawer.cpp
   src/drawers/ImageDrawer.cpp
+  src/drawers/LabelImageDrawer.cpp
   src/drawers/LaserScanImageDrawer.cpp
   src/drawers/OccupancyGridImageDrawer.cpp
   src/drawers/RobotImageDrawer.cpp

map_image_generator/include/map_image_generator/Parameters.h

@@ -28,6 +28,7 @@ namespace map_image_generator
         bool m_drawRobot;
         bool m_drawGoals;
         bool m_drawLaserScan;
+        bool m_drawLabels;
 
         cv::Vec3b m_wallColor;
         cv::Vec3b m_freeSpaceColor;
@@ -37,11 +38,13 @@ namespace map_image_generator
         cv::Scalar m_goalColor;
         cv::Scalar m_laserScanColor;
         cv::Scalar m_textColor;
+        cv::Scalar m_labelColor;
 
         int m_globalPathThickness; // pixel
         int m_robotSize;           // pixel
         int m_goalSize;            // pixel
         int m_laserScanSize;       // pixel
+        int m_labelSize;           // pixel
 
     public:
         explicit Parameters(ros::NodeHandle& nodeHandle);
@@ -69,6 +72,7 @@ namespace map_image_generator
         bool drawRobot() const;
         bool drawGoals() const;
         bool drawLaserScan() const;
+        bool drawLabels() const;
 
         const cv::Vec3b& wallColor() const;
         const cv::Vec3b& freeSpaceColor() const;
@@ -78,11 +82,13 @@ namespace map_image_generator
         const cv::Scalar& goalColor() const;
         const cv::Scalar& laserScanColor() const;
         const cv::Scalar& textColor() const;
+        const cv::Scalar& labelColor() const;
 
         int globalPathThickness() const; // pixel
         int robotSize() const;           // pixel
         int goalSize() const;            // pixel
         int laserScanSize() const;       // pixel
+        int labelSize() const;           // pixel
 
     private:
         void validateParameters();
@@ -135,6 +141,8 @@ namespace map_image_generator
 
     inline bool Parameters::drawLaserScan() const { return m_drawLaserScan; }
 
+    inline bool Parameters::drawLabels() const { return m_drawLabels; }
+
     inline const cv::Vec3b& Parameters::wallColor() const { return m_wallColor; }
 
     inline const cv::Vec3b& Parameters::freeSpaceColor() const
@@ -163,12 +171,16 @@ namespace map_image_generator
 
     inline const cv::Scalar& Parameters::textColor() const { return m_textColor; }
 
+    inline const cv::Scalar& Parameters::labelColor() const { return m_labelColor; }
+
     inline int Parameters::globalPathThickness() const { return m_globalPathThickness; }
 
     inline int Parameters::robotSize() const { return m_robotSize; }
 
     inline int Parameters::goalSize() const { return m_goalSize; }
 
     inline int Parameters::laserScanSize() const { return m_laserScanSize; }
+
+    inline int Parameters::labelSize() const { return m_labelSize; }
 }
 #endif

map_image_generator/include/map_image_generator/drawers/LabelImageDrawer.h

@@ -0,0 +1,32 @@
+#ifndef LABEL_IMAGE_DRAWER_H
+#define LABEL_IMAGE_DRAWER_H
+
+#include "map_image_generator/drawers/ImageDrawer.h"
+
+#include <opentera_webrtc_ros_msgs/Label.h>
+#include <opentera_webrtc_ros_msgs/LabelArray.h>
+#include <ros/ros.h>
+
+namespace map_image_generator
+{
+    class LabelImageDrawer : public ImageDrawer
+    {
+        ros::Subscriber m_labelArraySubscriber;
+        opentera_webrtc_ros_msgs::LabelArray::ConstPtr m_lastLabelArray;
+
+    public:
+        LabelImageDrawer(const Parameters& parameters, ros::NodeHandle& nodeHandle,
+                         tf::TransformListener& tfListener);
+        ~LabelImageDrawer() override;
+
+        void draw(cv::Mat& image) override;
+
+    private:
+        void drawLabel(const opentera_webrtc_ros_msgs::Label& label, cv::Mat& image,
+                       tf::Transform& transform);
+
+        void labelArrayCallback(
+            const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray);
+    };
+}
+#endif

map_image_generator/src/MapImageGenerator.cpp

@@ -2,6 +2,7 @@
 
 #include "map_image_generator/drawers/GlobalPathImageDrawer.h"
 #include "map_image_generator/drawers/GoalImageDrawer.h"
+#include "map_image_generator/drawers/LabelImageDrawer.h"
 #include "map_image_generator/drawers/LaserScanImageDrawer.h"
 #include "map_image_generator/drawers/OccupancyGridImageDrawer.h"
 #include "map_image_generator/drawers/RobotImageDrawer.h"
@@ -33,6 +34,12 @@ MapImageGenerator::MapImageGenerator(Parameters& parameters, ros::NodeHandle& no
             std::make_unique<RobotImageDrawer>(m_parameters, nodeHandle, m_tfListener));
     }
 
+    if (m_parameters.drawLabels())
+    {
+        m_drawers.push_back(
+            std::make_unique<LabelImageDrawer>(m_parameters, nodeHandle, m_tfListener));
+    }
+
     if (m_parameters.drawGoals())
     {
         m_drawers.push_back(

map_image_generator/src/Parameters.cpp

@@ -87,6 +87,7 @@ Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0}
     m_drawRobot = getParam<bool>("draw_robot", true);
     m_drawGoals = getParam<bool>("draw_goals", true);
     m_drawLaserScan = getParam<bool>("draw_laser_scan", true);
+    m_drawLabels = getParam<bool>("draw_labels", true);
 
     m_wallColor = getParam<cv::Vec3b>("wall_color", "0 0 0");
     m_freeSpaceColor = getParam<cv::Vec3b>("free_space_color", "255 255 255");
@@ -96,11 +97,13 @@ Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0}
     m_goalColor = getParam<cv::Scalar>("goal_color", "0 175 0 255");
     m_laserScanColor = getParam<cv::Scalar>("laser_scan_color", "255 0 0 255");
     m_textColor = getParam<cv::Scalar>("text_color", "255 255 255 255");
+    m_labelColor = getParam<cv::Scalar>("label_color", "255 0 255 255");
 
     m_globalPathThickness = getParam<int>("global_path_thickness", 3);
     m_robotSize = getParam<int>("robot_size", 30);
     m_goalSize = getParam<int>("goal_size", 20);
     m_laserScanSize = getParam<int>("laser_scan_size", 6);
+    m_labelSize = getParam<int>("label_size", 35);
 
     m_centeredRobot = getParam<bool>("centered_robot", true);
 

map_image_generator/src/drawers/LabelImageDrawer.cpp

@@ -0,0 +1,65 @@
+#include "map_image_generator/drawers/LabelImageDrawer.h"
+
+#include <opencv2/imgproc.hpp>
+
+using namespace map_image_generator;
+
+LabelImageDrawer::LabelImageDrawer(const Parameters& parameters,
+                                   ros::NodeHandle& nodeHandle,
+                                   tf::TransformListener& tfListener)
+    : ImageDrawer(parameters, nodeHandle, tfListener),
+      m_labelArraySubscriber{m_nodeHandle.subscribe(
+          "stored_labels", 1, &LabelImageDrawer::labelArrayCallback, this)}
+{
+}
+
+LabelImageDrawer::~LabelImageDrawer() = default;
+
+void LabelImageDrawer::labelArrayCallback(
+    const opentera_webrtc_ros_msgs::LabelArray::ConstPtr& labelArray)
+{
+    m_lastLabelArray = labelArray;
+}
+
+
+void LabelImageDrawer::draw(cv::Mat& image)
+{
+    if (!m_lastLabelArray)
+    {
+        return;
+    }
+
+    for (const auto& label : m_lastLabelArray->labels)
+    {
+        auto tf = getTransformInRef(label.pose.header.frame_id);
+        if (tf)
+        {
+            drawLabel(label, image, *tf);
+        }
+    }
+}
+
+void LabelImageDrawer::drawLabel(const opentera_webrtc_ros_msgs::Label& label,
+                                 cv::Mat& image, tf::Transform& transform)
+{
+    const cv::Scalar& color = m_parameters.labelColor();
+    int size = m_parameters.labelSize();
+
+    tf::Pose labelPose;
+    tf::poseMsgToTF(label.pose.pose, labelPose);
+    labelPose = transform * labelPose;
+    adjustTransformForRobotRef(labelPose);
+    double yaw = tf::getYaw(labelPose.getRotation());
+
+    int startX, startY;
+    convertTransformToMapCoordinates(labelPose, startX, startY);
+
+    int endX = static_cast<int>(startX + size * cos(yaw));
+    int endY = static_cast<int>(startY + size * sin(yaw));
+
+    cv::drawMarker(image, cv::Point(startX, startY), color, cv::MARKER_DIAMOND,
+                   static_cast<int>(ceil(size / 4.0)),
+                   static_cast<int>(ceil(size / 12.0)), cv::FILLED);
+    cv::putText(image, label.name, cv::Point(startX, startY), cv::FONT_HERSHEY_DUPLEX,
+                0.5, m_parameters.textColor(), 1);
+}

opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch

@@ -107,4 +107,9 @@
   <node name="goal_manager" type="goal_manager.py" pkg="opentera_webrtc_ros" output="screen">
     <remap from="waypoint_reached" to="webrtc_data_outgoing"/>
   </node>
+
+  <node name="labels_manager" type="labels_manager.py" pkg="opentera_webrtc_ros" output="screen">
+    <remap from="waypoint_reached" to="webrtc_data_outgoing"/>
+    <remap from="stored_labels_text" to="webrtc_data_outgoing"/>
+  </node>
 </launch>