VIPER has multiple applications available that deal with specific tasks and have to be ordered separately.
Note
All topics are provided without a namespace. When you are using your VIPER, please take its namespace into account.
FollowMe is a VIPER application that tracks bounding box of the given human between frames and provides distance to the target.
Here we provide a simple GUI program (using QT) that visualizes all detected humans in the left camera image. When one particular bounding box is selected by clicking on it, it is sent to the FollowMe App as a target and from that moment - being tracked. Distance to the target is also displayed. It can be used to tell the robot where the target is.
First you need to create a catkin workspace.
source /opt/rubedo/cvm/setup.bash
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
Then create a simple package using the following files.
cvm_follow_me_sample/include/FollowMeWindow.h
/*******************************************************************************
* Copyright (c) 2018 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#ifndef FOLLOW_ME_WINDOW_H
#define FOLLOW_ME_WINDOW_H
#include <QMainWindow>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PointStamped.h>
#include <cvm_msgs/BoundingBoxes.h>
#include <message_filters/subscriber.h>
#include <dynamic_reconfigure/Config.h>
#include <string>
namespace Ui
{
// Class generated from .ui file
class FollowMeWindow;
} // namespace Ui
namespace cvm_follow_me_sample
{
class FollowMeWindow : public QMainWindow
{
Q_OBJECT
public:
/**
* @brief Constructor
* @param parent Parent widget
*/
explicit FollowMeWindow(QWidget *parent = 0);
/**
* @brief Destructor
*/
virtual ~FollowMeWindow();
void init(const std::string& prefix = "DEFAULT");
private Q_SLOTS:
/**
* @brief Method that is invoked when an even occurs in the given QObject
* @param watched Object that is being watched by the filter
* @param event Even that has occurred
* @return Event handling status
*/
bool eventFilter(QObject* watched, QEvent* event);
/**
* @brief Method that is invoked when FollowMe status button is released
*/
void enableReleased();
/**
* @brief Method that is invoked when drop time slider value is changed
* @param value Slider value
*/
void dropTimeChanged(int value);
/**
* @brief Method that is invoked when detection probability slider value is changed
* @param value Slider value
*/
void detectionProbChanged(int value);
/**
* @brief Method that is invoked when similarity threshold slider value is changed
* @param value Slider value
*/
void similarityThresholdChanged(int value);
/**
* @brief Method that is invoked when history queue slider value is changed
* @param value Slider value
*/
void historyQueueChanged(int value);
private:
/**
* @brief Method that is called when left image from camera is received. This method is respondible
* for displaying images and bounding boxes.
* @param image Left image message
*/
void leftimageCb(const sensor_msgs::ImageConstPtr& image);
/**
* @brief Method that is called when FollowMe status from camera is received. This method is responsible for
* state managment of the sample app. Status and UI with it are changed based on the status received.
* @param status Status message
*/
void statusCb(const std_msgs::StringConstPtr& status);
/**
* @brief Method that is called when bounding boxes of humans detected by FollowMe are received
* @param boundingBoxes Vector of bounding boxes
*/
void boundingBoxesCb(const cvm_msgs::BoundingBoxesConstPtr& boundingBoxes);
/**
* @brief Method that is called when bounding box of target is received
* @param boundingBox Bounding box of target
*/
void targetBoxCb(const cvm_msgs::BoundingBoxConstPtr& boundingBox);
/**
* @brief Method that is called when position of the target is received
* @param point Position (x, y, z) of the target
*/
void targetPositionCb(const geometry_msgs::PointStampedConstPtr& point);
/**
* @brief Method that changes visiblity of GUI which displays distance information
* @param status Status indicating to set visibility on or off
*/
void enableInfoLabels(bool status);
/**
* @brief Method that changes value of given dynamic reconfigure double parameter
* @param name Name of dynamic reconfigure parameter that has type double
* @param value Value of the parameter
*/
void changeDoubleParameter(const std::string& name, double value);
/**
* @brief Method that changes value of given dynamic reconfigure int parameter
* @param name Name of dynamic reconfigure parameter that has type double
* @param value Value of the parameter
*/
void changeIntParameter(const std::string& name, int value);
/**
/**
* @brief Method that is called when dynamic reconfigure parameters of FollowMe are updated
* @param config Config file containing all parameters
*/
void parameterUpdates(const dynamic_reconfigure::ConfigConstPtr& config);
// Pointer to a common object that contains all objects used in the app
Ui::FollowMeWindow* ui;
// Subscribers
ros::Subscriber m_imageSub;
ros::Subscriber m_statusSub;
ros::Subscriber m_boundingBoxesSub;
ros::Subscriber m_targetBoundingBoxSub;
ros::Subscriber m_targetPositionSub;
ros::Subscriber m_dynParametersSub;
// Publishers
ros::Publisher m_enablePub;
ros::Publisher m_setTargetPub;
// Cache of bounding boxes
cvm_msgs::BoundingBoxesConstPtr m_boundingBoxes;
cvm_msgs::BoundingBoxConstPtr m_targetBoundingBox;
// Image width and height
int m_width;
int m_height;
// Members indicating status
bool m_showingBoundingBoxes;
bool m_trackingOn;
std::string m_prefix;
};
} // namespace cvm_follow_me_sample
#endif // FOLLOW_ME_WINDOW_H
cvm_follow_me_sample/src/FollowMeWindow.cpp
/*******************************************************************************
* Copyright (c) 2018 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#include "FollowMeWindow.h"
#include "ui_FollowMeWindow.h"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Bool.h>
#include <qevent.h>
#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
using cvm_follow_me_sample::FollowMeWindow;
FollowMeWindow::FollowMeWindow(QWidget* parent)
: QMainWindow(parent)
, ui(new Ui::FollowMeWindow())
, m_imageSub()
, m_statusSub()
, m_boundingBoxesSub()
, m_targetBoundingBoxSub()
, m_targetPositionSub()
, m_dynParametersSub()
, m_enablePub()
, m_setTargetPub()
, m_boundingBoxes()
, m_targetBoundingBox()
, m_width()
, m_height()
, m_showingBoundingBoxes()
, m_trackingOn()
, m_prefix()
{
ui->setupUi(this);
ui->display_image->installEventFilter(this);
enableInfoLabels(false);
// Connect event handlers
connect(ui->enableFollowMeButton, SIGNAL(released()), this, SLOT(enableReleased()));
connect(ui->dropTimeSlider, SIGNAL(sliderMoved(int)), this, SLOT(dropTimeChanged(int)));
connect(ui->detectionProbSlider, SIGNAL(sliderMoved(int)), this, SLOT(detectionProbChanged(int)));
connect(ui->similarityThSlider, SIGNAL(sliderMoved(int)), this, SLOT(similarityThresholdChanged(int)));
connect(ui->historyQueSlider, SIGNAL(sliderMoved(int)), this, SLOT(historyQueueChanged(int)));
}
void FollowMeWindow::init(const std::string& prefix)
{
m_prefix = prefix;
// Initialize subscribers
ros::NodeHandle n;
// Initialize subscribers
m_imageSub = n.subscribe<sensor_msgs::Image>(m_prefix + "/left/image_rect", 1, &FollowMeWindow::leftimageCb, this);
m_statusSub = n.subscribe<std_msgs::String>(m_prefix + "/follow_me/status", 1, &FollowMeWindow::statusCb, this);
m_boundingBoxesSub = n.subscribe<cvm_msgs::BoundingBoxes>(m_prefix + "/follow_me/bounding_boxes", 1, &FollowMeWindow::boundingBoxesCb, this);
m_targetBoundingBoxSub = n.subscribe<cvm_msgs::BoundingBox>(m_prefix + "/follow_me/target_bounding_box", 1, &FollowMeWindow::targetBoxCb, this);
m_targetPositionSub = n.subscribe<geometry_msgs::PointStamped>(m_prefix + "/follow_me/target_position", 1, &FollowMeWindow::targetPositionCb, this);
m_dynParametersSub = n.subscribe<dynamic_reconfigure::Config>(m_prefix + "/cvm_follow_me/parameter_updates", 1,&FollowMeWindow::parameterUpdates, this);
// Initialize publishers
m_enablePub = n.advertise<std_msgs::Bool>(m_prefix + "/follow_me/enable", 1);
m_setTargetPub = n.advertise<cvm_msgs::BoundingBox>(m_prefix + "/follow_me/set_target", 1);
}
FollowMeWindow::~FollowMeWindow()
{
m_imageSub.shutdown();
m_statusSub.shutdown();
m_boundingBoxesSub.shutdown();
m_targetBoundingBoxSub.shutdown();
m_targetPositionSub.shutdown();
m_dynParametersSub.shutdown();
delete ui;
}
bool FollowMeWindow::eventFilter(QObject* watched, QEvent* event)
{
if(watched != ui->display_image)
return false;
if(event->type() != QEvent::MouseButtonPress)
return false;
//
// Find the bounding box that has been clicked and send it to FollowMe as a target
//
if(m_showingBoundingBoxes && m_boundingBoxes)
{
const QMouseEvent* const mouseEvent = static_cast<const QMouseEvent*>(event);
const QPoint point = mouseEvent->pos();
// Remap coordinates
int x = (int)((float)point.x() / ui->display_image->width() * m_width);
int y = (int)((float)point.y() / ui->display_image->height() * m_height);
for(cvm_msgs::BoundingBox box : m_boundingBoxes->boundingBoxes)
{
if(x > box.xmin && x < box.xmax &&
y > box.ymin && y < box.ymax)
{
// If user has clicked inside set target
m_setTargetPub.publish(box);
return true;
}
}
}
return true;
}
void FollowMeWindow::enableReleased()
{
std_msgs::Bool msg;
if(ui->enableFollowMeButton->text().contains("Enable"))
{
msg.data = true;
}
else
{
msg.data = false;
}
m_enablePub.publish(msg);
}
void FollowMeWindow::dropTimeChanged(int value)
{
if(value == ui->dropTimeSlider->value())
return;
double finalValue = (float)value / 10.0f;
ui->dropTimeLabel->setText(QString::number(finalValue, 'f', 1) + QString(" s"));
// Update UI
update();
changeDoubleParameter("drop_time", finalValue);
}
void FollowMeWindow::detectionProbChanged(int value)
{
if(value == ui->detectionProbSlider->value())
return;
double finalValue = (float)value / 100.0f;
ui->detectionProbLabel->setText(QString::number(finalValue, 'f', 2));
// Update UI
update();
changeDoubleParameter("detection_prob", finalValue);
}
void FollowMeWindow::similarityThresholdChanged(int value)
{
if(value == ui->similarityThSlider->value())
return;
double finalValue = (float)value / 100.0f;
ui->similarityThLabel->setText(QString::number(finalValue, 'f', 2));
// Update UI
update();
changeDoubleParameter("similarity_threshold", finalValue);
}
void FollowMeWindow::historyQueueChanged(int value)
{
if(value == ui->historyQueSlider->value())
return;
double finalValue = value;
ui->historyQueLabel->setText(QString::number(finalValue));
// Update UI
update();
changeIntParameter("history_queue", finalValue);
}
void FollowMeWindow::leftimageCb(const sensor_msgs::ImageConstPtr& image)
{
const cv::Mat img = cv_bridge::toCvShare(image)->image;
cv::Mat result = img.clone();
m_width = image->width;
m_height = image->height;
//
// Draw bounding boxes of all detected humans in green
//
if(m_boundingBoxes)
{
if(std::abs(m_boundingBoxes->header.stamp.toSec() - image->header.stamp.toSec()) < 0.5)
{
for(cvm_msgs::BoundingBox box : m_boundingBoxes->boundingBoxes)
{
cv::Rect2d roi(box.xmin, box.ymin, box.xmax - box.xmin, box.ymax - box.ymin);
cv::rectangle(result, roi, cv::Scalar(0, 255, 0), 2, 1);
}
m_showingBoundingBoxes = true;
}
else
{
m_showingBoundingBoxes = false;
}
}
//
// Draw the bounding box of human being tracked in red
//
if(m_trackingOn && m_targetBoundingBox)
{
if(std::abs(m_targetBoundingBox->header.stamp.toSec() - image->header.stamp.toSec()) < 0.5)
{
cv::Rect2d roi(m_targetBoundingBox->xmin, m_targetBoundingBox->ymin, m_targetBoundingBox->xmax - m_targetBoundingBox->xmin, m_targetBoundingBox->ymax - m_targetBoundingBox->ymin);
cv::rectangle(result, roi, cv::Scalar(255, 0, 0), 2, 1);
m_showingBoundingBoxes = true;
}
}
//
// Resize the image to fit the label
//
cv::resize(result, result, cv::Size(ui->display_image->width(), ui->display_image->height()));
QImage imdisplay((uchar*)result.data, result.cols, result.rows, result.step, QImage::Format_RGB888);
ui->display_image->setPixmap(QPixmap::fromImage(imdisplay));
}
void FollowMeWindow::statusCb(const std_msgs::StringConstPtr& status)
{
ui->statusLabel->setText(QString::fromStdString(std::string(status->data)));
if(ui->statusLabel->text().contains("disabled")) // If FollowMe is disabled
{
if(!ui->enableFollowMeButton->text().contains("Enable FollowMe"))
ui->enableFollowMeButton->setText(QString("Enable FollowMe"));
m_trackingOn = false;
}
else // FollowMe is enabled
{
if(!ui->enableFollowMeButton->text().contains("Disable FollowMe"))
ui->enableFollowMeButton->setText(QString("Disable FollowMe"));
if(ui->statusLabel->text().contains("Waiting for another target")) // Enabled and waiting for target to track
{
m_trackingOn = false;
}
else if(ui->statusLabel->text().contains("Tracking target")) // Target is being tracked
{
m_trackingOn = true;
}
}
//
// Change visibility of info labels based on tracking status
//
enableInfoLabels(m_trackingOn);
}
void FollowMeWindow::boundingBoxesCb(const cvm_msgs::BoundingBoxesConstPtr& boundingBoxes)
{
m_boundingBoxes = boundingBoxes;
}
void FollowMeWindow::targetBoxCb(const cvm_msgs::BoundingBoxConstPtr& bounding_box)
{
m_targetBoundingBox = bounding_box;
}
void FollowMeWindow::targetPositionCb(const geometry_msgs::PointStampedConstPtr& point)
{
ui->xLabel->setText(QString("x: ") + QString::number(point->point.x, 'f', 2));
ui->yLabel->setText(QString("y: ") + QString::number(point->point.y, 'f', 2));
ui->zLabel->setText(QString("z: ") + QString::number(point->point.z, 'f', 2));
}
void FollowMeWindow::enableInfoLabels(bool status)
{
// Set value only if changes are required
if(ui->label_2->isVisible() && !status ||
!ui->label_2->isVisible() && status)
{
ui->label_2->setVisible(status);
ui->xLabel->setVisible(status);
ui->yLabel->setVisible(status);
ui->zLabel->setVisible(status);
// Update UI
update();
}
}
void FollowMeWindow::changeDoubleParameter(const std::string& name, double value)
{
dynamic_reconfigure::DoubleParameter parameter;
parameter.name = name;
parameter.value = value;
dynamic_reconfigure::Config config;
config.doubles.push_back(parameter);
dynamic_reconfigure::ReconfigureRequest serverRequest;
serverRequest.config = config;
dynamic_reconfigure::ReconfigureResponse serverResponse;
ros::service::call(m_prefix + "/cvm_follow_me/set_parameters", serverRequest, serverResponse);
}
void FollowMeWindow::changeIntParameter(const std::string& name, int value)
{
dynamic_reconfigure::IntParameter parameter;
parameter.name = name;
parameter.value = value;
dynamic_reconfigure::Config config;
config.ints.push_back(parameter);
dynamic_reconfigure::ReconfigureRequest serverRequest;
serverRequest.config = config;
dynamic_reconfigure::ReconfigureResponse serverResponse;
ros::service::call(m_prefix + "/cvm_follow_me/set_parameters", serverRequest, serverResponse);
}
void FollowMeWindow::parameterUpdates(const dynamic_reconfigure::ConfigConstPtr& config)
{
for(auto param : config->doubles)
{
if(param.name == "drop_time")
{
int value = param.value * 10;
ui->dropTimeLabel->setText(QString::number(param.value, 'f', 1) + QString(" s"));
ui->dropTimeSlider->setValue(value);
// Update UI
update();
}
else if(param.name == "detection_prob")
{
int value = param.value * 100;
ui->detectionProbLabel->setText(QString::number(param.value, 'f', 2));
ui->detectionProbSlider->setValue(value);
// Update UI
update();
}
else if(param.name == "similarity_threshold")
{
int value = param.value * 100;
ui->similarityThLabel->setText(QString::number(param.value, 'f', 2));
ui->similarityThSlider->setValue(value);
// Update UI
update();
}
}
for(auto param : config->ints)
{
if(param.name == "history_queue")
{
int value = param.value;
ui->historyQueLabel->setText(QString::number(param.value));
ui->historyQueSlider->setValue(value);
// Update UI
update();
}
}
}
cvm_follow_me_sample/src/main.cpp
/*******************************************************************************
* Copyright (c) 2018 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#include "FollowMeWindow.h"
#include <ros/spinner.h>
#include <QApplication>
using cvm_follow_me_sample::FollowMeWindow;
int main(int argc, char *argv[])
{
// Initialize ROS
ros::init(argc, argv, "FollowMeSample");
// Start a spinner with 4 threads
ros::AsyncSpinner spinner(4);
spinner.start();
// Init QT application
QApplication a(argc, argv);
FollowMeWindow w;
w.init("DEFAULT"); // VIPER prefix
w.show();
return a.exec();
}
cvm_follow_me_sample/package.xml
<?xml version="1.0"?>
<package format="2">
<name>cvm_follow_me_sample</name>
<version>1.0.0</version>
<description>FollowMe sample application</description>
<maintainer email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</maintainer>
<author email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</author>
<license>Proprietary</license>
<url type="website">http://www.rubedos.com</url>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>qtbase5-dev</buildtool_depend>
<exec_depend>cvm_base</exec_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cvm_msgs</depend>
<depend>cv_bridge</depend>
<depend>message_filters</depend>
<depend>dynamic_reconfigure</depend>
</package>
cvm_follow_me_sample/CMakeLists.txt
cmake_minimum_required(VERSION 3.5.1)
project(cvm_follow_me_sample)
add_compile_options(-std=c++14 -O3)
# Project variables
set(PROJECT_SRC ${PROJECT_SOURCE_DIR}/src/FollowMeWindow.cpp)
set(PROJECT_HDR ${PROJECT_SOURCE_DIR}/include/FollowMeWindow.h)
set(PROJECT_UI ${PROJECT_SOURCE_DIR}/ui/FollowMeWindow.ui)
set(PROJECT_BIN_SRC ${PROJECT_SOURCE_DIR}/src/main.cpp)
# Find includes in corresponding build directories
set(CMAKE_INCLUDE_CURRENT_DIR ON)
# Find required packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
sensor_msgs
geometry_msgs
cvm_msgs
cv_bridge
message_filters
dynamic_reconfigure
)
find_package(Qt5 REQUIRED
Widgets REQUIRED
)
find_package(OpenCV REQUIRED)
# Manual MOC generation
qt5_wrap_cpp(PROJECT_HDR_MOC ${PROJECT_HDR})
qt5_wrap_ui(PROJECT_UI_MOC ${PROJECT_UI})
catkin_package(
LIBRARIES
)
include_directories(
include
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
add_library(rubedo.robotics.${PROJECT_NAME} SHARED
${PROJECT_SRC}
${PROJECT_HDR_MOC}
${PROJECT_UI_MOC}
)
target_link_libraries(rubedo.robotics.${PROJECT_NAME}
Qt5::Widgets
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
)
add_executable(${PROJECT_NAME} ${PROJECT_BIN_SRC})
target_link_libraries(${PROJECT_NAME} rubedo.robotics.${PROJECT_NAME})
set(FILE "${CMAKE_CURRENT_SOURCE_DIR}/code")
if(NOT EXISTS "${FILE}")
execute_process(COMMAND mkdir -p ${FILE})
endif()
install(DIRECTORY
code
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY
src
include
ui
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/code
)
install(FILES
CMakeLists.txt
package.xml
README.md
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/code
)
install(TARGETS
rubedo.robotics.${PROJECT_NAME}
${PROJECT_NAME}
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
cvm_follow_me_sample/ui/FollowMeWindow.ui
Build and run the sample:
cd ~/catkin_ws
catkin build cvm_follow_me_sample
source devel/setup.bash
export ROS_MASTER_URI=http://<VIPER_IP>:11311
export ROS_IP=<HOST_IP>
rosrun cvm_follow_me_sample cvm_follow_me_sample
Enable the FollowMe app and click on the person you want to track. The person who is being tracked will be marked with a red bounding box.
Note
Make sure the ROS_IP you set is the IP of the host machine that is connected to your VIPER, otherwise you will not be able to enable the FollowMe App while using the sample above. For more information about networking please refer to ROS Network Setup.
FollowAruco is a VIPER application that tracks Aruco markers and provides distance to the target with a selected ID.
Here we provide a simple GUI program (using QT) that visualizes all Aruco markers in the left rectified camera image. Selected marker (ID) will be considered as a target. Distance to the target is displayed.
First you need to create a catkin workspace.
source /opt/ros/kinetic/setup.bash
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
Then create a simple package using the following files.
cvm_follow_aruco_sample/include/FollowArucoWindow.h
/*******************************************************************************
* Copyright (c) 2019 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#ifndef FOLLOW_ARUCO_WINDOW_H
#define FOLLOW_ARUCO_WINDOW_H
#include <QMainWindow>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/PointStamped.h>
#include <message_filters/subscriber.h>
#include <dynamic_reconfigure/Config.h>
#include <string>
namespace Ui
{
// Class generated from .ui file
class FollowArucoWindow;
} // namespace Ui
namespace cvm_follow_aruco_sample
{
class FollowArucoWindow : public QMainWindow
{
Q_OBJECT
public:
/**
* @brief Constructor
* @param parent Parent widget
*/
explicit FollowArucoWindow(QWidget *parent = 0);
/**
* @brief Destructor
*/
virtual ~FollowArucoWindow();
void init(const std::string& prefix = "DEFAULT");
private Q_SLOTS:
/**
* @brief Method that is invoked when FollowAruco status button is released
*/
void enableReleased();
/**
* @brief Method that is invoked when marker size value is changed
* @param value Slider value
*/
void markerSizeChanged(int value);
/**
* @brief Method that is invoked when marker id slider value is changed
* @param value Slider value
*/
void markerIdChanged(int value);
private:
/**
* @brief Method that is called when detection image from camera is received. This method is respondible
* for displaying images and bounding boxes.
* @param image detection image message
*/
void imageCb(const sensor_msgs::ImageConstPtr& image);
/**
* @brief Method that is called when FollowAruco status from camera is received. This method is responsible for
* state managment of the sample app. Status and UI with it are changed based on the status received.
* @param status Status message
*/
void statusCb(const std_msgs::StringConstPtr& status);
/**
* @brief Method that is called when position of the target is received
* @param point Position (x, y, z) of the target
*/
void targetPositionCb(const geometry_msgs::PointStampedConstPtr& point);
/**
* @brief Method that changes visiblity of GUI which displays distance information
* @param status Status indicating to set visibility on or off
*/
void enableInfoLabels(bool status);
/**
* @brief Method that changes value of given dynamic reconfigure double parameter
* @param name Name of dynamic reconfigure parameter that has type double
* @param value Value of the parameter
*/
void changeDoubleParameter(const std::string& name, double value);
/**
* @brief Method that changes value of given dynamic reconfigure int parameter
* @param name Name of dynamic reconfigure parameter that has type double
* @param value Value of the parameter
*/
void changeIntParameter(const std::string& name, int value);
/**
/**
* @brief Method that is called when dynamic reconfigure parameters of FollowAruco are updated
* @param config Config file containing all parameters
*/
void parameterUpdates(const dynamic_reconfigure::ConfigConstPtr& config);
// Pointer to a common object that contains all objects used in the app
Ui::FollowArucoWindow* ui;
// Subscribers
ros::Subscriber m_imageSub;
ros::Subscriber m_statusSub;
ros::Subscriber m_boundingBoxesSub;
ros::Subscriber m_targetBoundingBoxSub;
ros::Subscriber m_targetPositionSub;
ros::Subscriber m_dynParametersSub;
// Publishers
ros::Publisher m_enablePub;
std::string m_prefix;
};
} // namespace cvm_follow_aruco_sample
#endif // FOLLOW_ARUCO_WINDOW_H
cvm_follow_aruco_sample/src/FollowArucoWindow.cpp
/*******************************************************************************
* Copyright (c) 2019 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#include "FollowArucoWindow.h"
#include "ui_FollowArucoWindow.h"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Bool.h>
#include <qevent.h>
#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/IntParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
using cvm_follow_aruco_sample::FollowArucoWindow;
FollowArucoWindow::FollowArucoWindow(QWidget* parent)
: QMainWindow(parent)
, ui(new Ui::FollowArucoWindow())
, m_imageSub()
, m_statusSub()
, m_boundingBoxesSub()
, m_targetBoundingBoxSub()
, m_targetPositionSub()
, m_dynParametersSub()
, m_enablePub()
, m_prefix()
{
ui->setupUi(this);
ui->display_image->installEventFilter(this);
enableInfoLabels(false);
// Connect event handlers
connect(ui->enableFollowArucoButton, SIGNAL(released()), this, SLOT(enableReleased()));
connect(ui->markerSizeSlider, SIGNAL(sliderMoved(int)), this, SLOT(markerSizeChanged(int)));
connect(ui->markerIdSlider, SIGNAL(sliderMoved(int)), this, SLOT(markerIdChanged(int)));
}
FollowArucoWindow::~FollowArucoWindow()
{
m_imageSub.shutdown();
m_statusSub.shutdown();
m_boundingBoxesSub.shutdown();
m_targetBoundingBoxSub.shutdown();
m_targetPositionSub.shutdown();
m_dynParametersSub.shutdown();
delete ui;
}
void FollowArucoWindow::init(const std::string& prefix)
{
m_prefix = prefix;
ros::NodeHandle n;
// Initialize subscribers
m_imageSub = n.subscribe<sensor_msgs::Image>(m_prefix + "/follow_aruco/detection_image", 1, &FollowArucoWindow::imageCb, this);
m_statusSub = n.subscribe<std_msgs::String>(m_prefix + "/follow_aruco/status", 1, &FollowArucoWindow::statusCb, this);
m_targetPositionSub = n.subscribe<geometry_msgs::PointStamped>(m_prefix + "/follow_aruco/target_position", 1, &FollowArucoWindow::targetPositionCb, this);
m_dynParametersSub = n.subscribe<dynamic_reconfigure::Config>(m_prefix + "/cvm_follow_aruco/parameter_updates", 1,&FollowArucoWindow::parameterUpdates, this);
// Initialize publishers
m_enablePub = n.advertise<std_msgs::Bool>(m_prefix + "/follow_aruco/enable", 1);
}
void FollowArucoWindow::enableReleased()
{
std_msgs::Bool msg;
if(ui->enableFollowArucoButton->text().contains("Enable"))
{
msg.data = true;
}
else
{
msg.data = false;
}
m_enablePub.publish(msg);
}
void FollowArucoWindow::markerSizeChanged(int value)
{
if(value == ui->markerSizeSlider->value())
return;
double finalValue = (float)value / 100.0f;
ui->markerSizeLabel->setText(QString::number(finalValue, 'f', 2) + QString(" m"));
// Update UI
update();
changeDoubleParameter("marker_size", finalValue);
}
void FollowArucoWindow::markerIdChanged(int value)
{
if(value == ui->markerIdSlider->value())
return;
double finalValue = value;
ui->markerIdLabel->setText(QString::number(finalValue));
// Update UI
update();
changeIntParameter("marker_id", finalValue);
}
void FollowArucoWindow::imageCb(const sensor_msgs::ImageConstPtr& image)
{
const cv::Mat img = cv_bridge::toCvShare(image)->image;
//
// Resize the image to fit the label
//
cv::Mat resized;
cv::resize(img, resized, cv::Size(ui->display_image->width(), ui->display_image->height()));
QImage imdisplay((uchar*)resized.data, resized.cols, resized.rows, resized.step, QImage::Format_RGB888);
ui->display_image->setPixmap(QPixmap::fromImage(imdisplay));
}
void FollowArucoWindow::statusCb(const std_msgs::StringConstPtr& status)
{
ui->statusLabel->setText(QString::fromStdString(std::string(status->data)));
if(ui->statusLabel->text().contains("disabled")) // If FollowAruco is disabled
{
if(!ui->enableFollowArucoButton->text().contains("Enable FollowAruco"))
{
ui->enableFollowArucoButton->setText(QString("Enable FollowAruco"));
enableInfoLabels(false);
}
}
else if(!ui->enableFollowArucoButton->text().contains("Disable FollowAruco"))
{
ui->enableFollowArucoButton->setText(QString("Disable FollowAruco"));
enableInfoLabels(true);
}
}
void FollowArucoWindow::targetPositionCb(const geometry_msgs::PointStampedConstPtr& point)
{
ui->xLabel->setText(QString("x: ") + QString::number(point->point.x, 'f', 2));
ui->yLabel->setText(QString("y: ") + QString::number(point->point.y, 'f', 2));
ui->zLabel->setText(QString("z: ") + QString::number(point->point.z, 'f', 2));
}
void FollowArucoWindow::enableInfoLabels(bool status)
{
// Set value only if changes are required
if(ui->label_2->isVisible() && !status ||
!ui->label_2->isVisible() && status)
{
ui->label_2->setVisible(status);
ui->xLabel->setVisible(status);
ui->yLabel->setVisible(status);
ui->zLabel->setVisible(status);
// Update UI
update();
}
}
void FollowArucoWindow::changeDoubleParameter(const std::string& name, double value)
{
dynamic_reconfigure::DoubleParameter parameter;
parameter.name = name;
parameter.value = value;
dynamic_reconfigure::Config config;
config.doubles.push_back(parameter);
dynamic_reconfigure::ReconfigureRequest serverRequest;
serverRequest.config = config;
dynamic_reconfigure::ReconfigureResponse serverResponse;
ros::service::call(m_prefix + "/cvm_follow_aruco/set_parameters", serverRequest, serverResponse);
}
void FollowArucoWindow::changeIntParameter(const std::string& name, int value)
{
dynamic_reconfigure::IntParameter parameter;
parameter.name = name;
parameter.value = value;
dynamic_reconfigure::Config config;
config.ints.push_back(parameter);
dynamic_reconfigure::ReconfigureRequest serverRequest;
serverRequest.config = config;
dynamic_reconfigure::ReconfigureResponse serverResponse;
ros::service::call(m_prefix + "/cvm_follow_aruco/set_parameters", serverRequest, serverResponse);
}
void FollowArucoWindow::parameterUpdates(const dynamic_reconfigure::ConfigConstPtr& config)
{
for(auto param : config->doubles)
{
if(param.name == "marker_size")
{
int value = param.value * 100;
ui->markerSizeLabel->setText(QString::number(param.value, 'f', 2) + QString(" m"));
ui->markerSizeSlider->setValue(value);
// Update UI
update();
}
}
for(auto param : config->ints)
{
if(param.name == "marker_id")
{
int value = param.value;
ui->markerIdLabel->setText(QString::number(param.value));
ui->markerIdSlider->setValue(value);
// Update UI
update();
}
}
}
cvm_follow_aruco_sample/src/main.cpp
/*******************************************************************************
* Copyright (c) 2019 by Rubedos
* Kaunas, Lithuania, www.rubedos.com
* All rights reserved.
*******************************************************************************/
#include "FollowArucoWindow.h"
#include <ros/spinner.h>
#include <QApplication>
using cvm_follow_aruco_sample::FollowArucoWindow;
int main(int argc, char *argv[])
{
// Initialize ROS
ros::init(argc, argv, "FollowArucoSample");
// Start a spinner with 4 threads
ros::AsyncSpinner spinner(4);
spinner.start();
// Init QT application
QApplication a(argc, argv);
FollowArucoWindow w;
w.init("DEFAULT"); // VIPER prefix
w.show();
return a.exec();
}
cvm_follow_aruco_sample/package.xml
<?xml version="1.0"?>
<package format="2">
<name>cvm_follow_aruco_sample</name>
<version>1.0.0</version>
<description>FollowAruco sample application</description>
<maintainer email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</maintainer>
<author email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</author>
<license>Proprietary</license>
<url type="website">http://www.rubedos.com</url>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>qtbase5-dev</buildtool_depend>
<exec_depend>cvm_base</exec_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>cv_bridge</depend>
<depend>message_filters</depend>
<depend>dynamic_reconfigure</depend>
</package>
cvm_follow_aruco_sample/CMakeLists.txt
cmake_minimum_required(VERSION 3.5.1)
project(cvm_follow_aruco_sample)
add_compile_options(-std=c++14 -O3)
# Project variables
set(PROJECT_SRC ${PROJECT_SOURCE_DIR}/src/FollowArucoWindow.cpp)
set(PROJECT_HDR ${PROJECT_SOURCE_DIR}/include/FollowArucoWindow.h)
set(PROJECT_UI ${PROJECT_SOURCE_DIR}/ui/FollowArucoWindow.ui)
set(PROJECT_BIN_SRC ${PROJECT_SOURCE_DIR}/src/main.cpp)
# Find includes in corresponding build directories
set(CMAKE_INCLUDE_CURRENT_DIR ON)
# Find required packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
sensor_msgs
geometry_msgs
cv_bridge
message_filters
dynamic_reconfigure
)
find_package(Qt5 REQUIRED
Widgets REQUIRED
)
find_package(OpenCV REQUIRED)
# Manual MOC generation
qt5_wrap_cpp(PROJECT_HDR_MOC ${PROJECT_HDR})
qt5_wrap_ui(PROJECT_UI_MOC ${PROJECT_UI})
catkin_package(
LIBRARIES
)
include_directories(
include
${OpenCV_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
add_library(rubedo.robotics.${PROJECT_NAME} SHARED
${PROJECT_SRC}
${PROJECT_HDR_MOC}
${PROJECT_UI_MOC}
)
target_link_libraries(rubedo.robotics.${PROJECT_NAME}
Qt5::Widgets
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
)
add_executable(${PROJECT_NAME} ${PROJECT_BIN_SRC})
target_link_libraries(${PROJECT_NAME} rubedo.robotics.${PROJECT_NAME})
set(FILE "${CMAKE_CURRENT_SOURCE_DIR}/code")
if(NOT EXISTS "${FILE}")
execute_process(COMMAND mkdir -p ${FILE})
endif()
install(DIRECTORY
code
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY
src
include
ui
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/code
)
install(FILES
CMakeLists.txt
package.xml
README.md
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/code
)
install(TARGETS
rubedo.robotics.${PROJECT_NAME}
${PROJECT_NAME}
DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
cvm_follow_aruco_sample/ui/FollowArucoWindow.ui
Build and run the sample:
cd ~/catkin_ws
catkin build cvm_follow_aruco_sample
source devel/setup.bash
export ROS_MASTER_URI=http://<VIPER_IP>:11311
export ROS_IP=<HOST_IP>
rosrun cvm_follow_aruco_sample cvm_follow_aruco_sample
Enable the FollowAruco app and select the ID of the Aruco marker you want to track. The selected marker will be marked with a red bounding box and distance will be displayed on the left panel.
Note
Make sure the ROS_IP you set is the IP of the host machine that is connected to your VIPER, otherwise you will not be able to enable the FollowAruco App while using the sample above. For more information about networking please refer to ROS Network Setup.
Obstacle Detector is a VIPER application that imitates laser scan by processing point cloud produced by the camera. Output can be used as an input for a robot path planner. If laser scan is not visible you might need to play with Obstacle Detector configuration, by providing relevant target frame and transform.
rviz and can be used to view output of the Object Detector App.
- run rviz [rosrun rviz rviz];
- Add display type in rviz [Add->LaserScan];
- Browse for the
laser_scantopic;- Make sure correct link is selected, e.g.
cvm_base_link.
First you need to create a catkin workspace.
source /opt/ros/kinetic/setup.bash
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init
Then create a simple package using the following files.
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
void laserScanCallback(const sensor_msgs::LaserScanConstPtr& msg)
{
size_t size = msg->ranges.size();
ROS_INFO("Distance at %1.2f rads angle is %4.3f meters.",
msg->angle_min, msg->ranges[0]);
ROS_INFO("Distance at %1.2f rads angle is %4.3f meters.",
(msg->angle_min + msg->angle_max) / 2.0f, msg->ranges[size / 2]);
ROS_INFO("Distance at %1.2f rads angle is %4.3f meters.",
msg->angle_max, msg->ranges[size - 1]);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserscan_streaming");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("laser_scan", 1, laserScanCallback);
ros::spin();
}
laserscan_streaming/CMakeLists.txt
cmake_minimum_required(VERSION 3.5.1)
project(laserscan_streaming)
add_compile_options(-std=c++14 -O3)
# Find required packages
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
sensor_msgs
)
find_package(OpenCV REQUIRED)
catkin_package(
LIBRARIES
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(laserscan_streaming main.cpp)
target_link_libraries(laserscan_streaming ${catkin_LIBRARIES})
laserscan_streaming/package.xml
<?xml version="1.0"?>
<package format="2">
<name>laserscan_streaming</name>
<version>1.0.0</version>
<description>Simple laserscan streaming application</description>
<maintainer email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</maintainer>
<author email="arnas.ivanavicius@rubedos.com">Arnas Ivanavicius</author>
<license>Proprietary</license>
<url type="website">http://www.rubedos.com</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
</package>
Build and run the sample:
cd ~/catkin_ws
catkin build laserscan_streaming
source devel/setup.bash
export ROS_MASTER_URI=http://<VIPER_IP>:11311
rosrun laserscan_streaming laserscan_streaming