Skip to content

Commit

Permalink
Added labels to the map (#37)
Browse files Browse the repository at this point in the history
* Added skeleton for label image drawer

* WIP Added a labels_manager

* Added label navigation to frontend

* Added sync between labels stored by label_manager and frontend

* Added label addition and move to frontend

* Added label editing to frontend

* Remove unrequired rependency

* Updated dependency
  • Loading branch information
philippewarren authored Jan 28, 2022
1 parent 1661561 commit 6e3e758
Show file tree
Hide file tree
Showing 28 changed files with 804 additions and 189 deletions.
117 changes: 57 additions & 60 deletions .github/workflows/pull-request-audit.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
4 changes: 4 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -30,3 +30,7 @@
*.exe
*.out
*.app

# Compiled Python
__pycache__
*.pyc
2 changes: 2 additions & 0 deletions map_image_generator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
12 changes: 12 additions & 0 deletions map_image_generator/include/map_image_generator/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Original file line number Diff line number Diff line change
@@ -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
7 changes: 7 additions & 0 deletions map_image_generator/src/MapImageGenerator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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(
Expand Down
3 changes: 3 additions & 0 deletions map_image_generator/src/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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);

Expand Down
65 changes: 65 additions & 0 deletions map_image_generator/src/drawers/LabelImageDrawer.cpp
Original file line number Diff line number Diff line change
@@ -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);
}
5 changes: 5 additions & 0 deletions opentera_webrtc_demos/launch/opentera_turtlebot_sim.launch
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Loading

0 comments on commit 6e3e758

Please sign in to comment.