Commit ceb4d7a8 by Sebastian Stern

refs #1910 added pics to docu, refs #2200 added ros2 video receiver adtf plugin with src code

parent 6153755f
......@@ -21,6 +21,10 @@ Alternativ kann auch die beiliegende virtualenv genutzt werden.
### Visual Studio
* Version == 2015
## Übersicht
<img src="./documentation/pics/ADTF_ROS2_codegen.png" alt="overview"/>
## Erste Schritte
......@@ -30,35 +34,45 @@ Dazu im Configuration-Editor das gewünschte System auswählen und im Bereich Sy
* Path= path_to_ros\install\Scripts;path_to_ros\install\bin;$(Path)
Danach muss ein bereitgestellter Service hinzugefügt werden.
Die Plugindaten liegen im Unterverzeichnis files/plugins.
Die Plugindaten liegen im Unterverzeichnis `\plugins`.
Den Service im Configuration-Editor unter System-Editor >> Services einfügen mit dem Level System.
Bei Bedarf kann der Service von Source gebaut werden die Daten dazu liegen im Unterordner "files/adtf_system_service" bei.
Diesen Ordner einfach in path_to_ros\ros2_ws\src\ros2 kopieren und danach in einer VisualStudio CommandLine folgende Befehle ausführen:
Der Service kann auch von Source gebaut werden. Die Daten dazu liegen im Ordner `plugins/adtf_system_service` bei.
Diesen Ordner einfach in `path_to_ros\ros2_ws\src\ros2` kopieren und danach in einer VisualStudio CommandLine folgende Befehle ausführen:
```
cd path_to_ros
install\local_setup.bat
ament build --only-package adtf_system_service
```
Nach dem Build & Install Vorgang liegen die Plugindateien im angegebenen Pfad(Bei Bedarf in der CMakeLists.txt anpassen)
Nach dem Build&Install Vorgang liegen die Plugindateien im angegebenen Pfad (Bei Bedarf in der `CMakeLists.txt` anpassen)
Das Python-Script kann über die Konsole gestartet werden. Man landet danach zunächst im Hauptmenu.
Das Script kann über die Konsole mit python main.py gestartet werden. Bei ersten Start wird man dazu aufgefordert einige Einstellungen anzupassen.
<img src="./documentation/pics/main.gif" alt="main" width="700"/>
Bei ersten Start wird man dazu aufgefordert einige Einstellungen anzupassen.
Diese sind unter dem Menü (5) Settings zu finden. Nach dem anpassen bzw. ergänzen der Parameter kann das Script genutzt werden.
<img src="./documentation/pics/settings.png" alt="settings" width="700"/>
## Funktionen
Das Script bietet drei Grundfunktionen
Das Script bietet zwei Grundfunktionen.
### Create Subscriber
Mit Hilfe dieser Funktion kann aus einer ROS2 Message Datei (*.msg) ein ROS2 Subscriber bzw. eine ADTF-Streaming Source erstellt werden.
Die zur Verfügung stehenden Message Dateien können mit dem Menü (6) Show ROS2 msg files angezeigt werden.
Mit Hilfe dieser Funktion kann aus einer ROS2-Message-Datei (*.msg) ein ROS2 Subscriber bzw. ein ADTF-Streaming-Source-Plugin erstellt werden.
Die zur Verfügung stehenden Message Dateien können mit dem Menüpunkt (6) `Show ROS2 msg files` angezeigt werden.
<img src="./documentation/pics/show_msgs.png" alt="show_msgs" width="700"/>
**Beispiel:**
Aus einer Message Datei IntegerArray.msg mit foldendem Inhalt:
Aus einer Message Datei `IntegerArray.msg` mit foldendem Inhalt:
```
int32[100] data
```
......@@ -73,16 +87,19 @@ DDL =
"</structs>"
```
Die nun generierte Streaming Source sendet mit jedem neuen Datenpaket das aus dem ROS2 DDL Netz empfangen wird ein neues Sample mit o.g. Beschreibung in das ADTF Netz.
Die nun generierte Streaming Source sendet mit jedem neuen Datenpaket, dass aus dem
ROS2-DDS -Netz empfangen wird, ein neues Sample mit o.g. Beschreibung in die ADTF Session.
<img src="./documentation/pics/create_sub.gif" alt="create_sub" width="700"/>
### Create Publisher
Diese Funktion erstellt aus gegebener DDL Beschreibung im XML oder cString Format eine eine oder mehrere Message Dateien.
Im Anschluss daran einen oder mehrere ROS2 Publisher bzw. eine ADTF-Streaming Sinks.
Diese Funktion erstellt aus gegebener DDL Beschreibung im XML oder cString Format eine oder mehrere Message Dateien.
Im Anschluss daran wird aus diesen Dateien die benötigte Anzahl ROS2 Publisher bzw. ADTF-Streaming Sinks erstellt.
**Beispiel:**
Aus einer Message Datei DDL.xml (oder .txt) mit foldendem Inhalt:
Aus einer Message Datei DDL.xml (oder .txt) mit foldendem Inhalt
```XML
<structs>
<struct alignment="1" name="tHeaderStruct" version="1">
......@@ -105,7 +122,8 @@ Aus einer Message Datei DDL.xml (oder .txt) mit foldendem Inhalt:
</structs>
```
werdem zunächst folgende Message Dateieien erstellt und ein ROS Build&Install wird für die neuen Daten ausgeführt.
werdem zunächst folgende Message Dateien erstellt. Danach wird ein ROS2 Build&Install Vorgang für die neuen Dateien ausgeführt.
Daraus ergibt sich folgendes Resultat.
```
TSimpleStruct.msg
uint8 ui8_val
......@@ -127,12 +145,9 @@ TSimpleStruct s_simple_struct
In einem zweiten Schritt werden dann den gewünschten Publisher bzw. Streaming Sinks erstellt.
Entsprechend des jeweiligen DDL-Structs und der korrespondieren ROS2 Message.
Diese Streaming Sinks publizieren mit jedem neuen Datenpaket aus dem ADTF-Netz diese in das ROS2-Netz.
### Create Message
Diese Funktion erstellt nur eine Message Datei nach gleichem Schema wie die Funktion "Create Publisher"
Diese Streaming Sinks publizieren mit jedem neuen Datenpaket aus der ADTF-Session dieses in das ROS2-DDS-Netz.
<img src="./documentation/pics/create_pub.gif" alt="create_pub" width="700"/>
......
......@@ -104,8 +104,8 @@ def show_settings():
"""
shows all settings and values in a CLI table
"""
table = PrettyTable(["Setting", "Value"])
table.align["Setting"] = 'l'
table = PrettyTable(["Key", "Value"])
table.align["Key"] = 'l'
table.align["Value"] = 'l'
for key, value in current_settings.items():
table.add_row([key, value])
......
......@@ -319,8 +319,7 @@ class MSG_Parser:
if len(element.adtf_data_path_as_list) > 0:
# iterate over both lists at the same time
for item1, item2 in zip(element.adtf_data_path_as_list, element.ros_data_path_as_list):
print(item1, item2)
for item1, item2 in zip(element.adtf_data_path_as_list, element.ros_data_path_as_list):
# check the array size
if element.array_size > 1:
......
cmake_minimum_required(VERSION 3.5)
project(adtf_cam_capture_cpp)
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(NOT WIN32)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra")
endif()
find_package(ament_cmake REQUIRED)
find_package(example_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcutils REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(rmw REQUIRED)
find_package(OpenCV REQUIRED)
find_package(ADTF REQUIRED PATHS C:/adtf/3.3.1/)
find_package(ADTF COMPONENTS systemsdk)
find_package(ADTF COMPONENTS filtersdk)
add_executable(cam_capture
src/cam_capture.cpp
src/cam_utils.hpp
)
ament_target_dependencies(cam_capture
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV"
)
add_executable(cam_view
src/cam_view.cpp
src/cam_utils.hpp
)
ament_target_dependencies(cam_view
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV"
)
add_executable(cam_detection
src/cam_detection.cpp
src/cam_utils.hpp
)
ament_target_dependencies(cam_detection
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV"
)
adtf_add_plugin(adtf_ros2_video_receiver
src/adtf_ros2_video_receiver.cpp
src/adtf_ros2_video_receiver.h
src/cam_utils.hpp
)
ament_target_dependencies(adtf_ros2_video_receiver
"rclcpp"
"sensor_msgs"
"std_msgs"
"OpenCV"
)
target_link_libraries(adtf_ros2_video_receiver adtf::systemsdk adtf::filtersdk)
install(TARGETS
cam_capture
cam_view
cam_detection
DESTINATION lib/${PROJECT_NAME}
)
adtf_install_filter(adtf_ros2_video_receiver /adtf_plugins)
add_custom_command(
TARGET adtf_ros2_video_receiver
POST_BUILD COMMAND ${ADTF_DIR}/bin/adtf_plugin_description_generator.exe -plugin=C:/adtf_plugins/adtf_ros2_video_receiver.adtfplugin -output=C:/adtf_plugins/adtf_ros2_video_receiver.plugindescription
)
ament_package()
\ No newline at end of file
<?xml version="1.0"?>
<package format="2">
<name>adtf_cam_capture_cpp</name>
<version>0.0.1</version>
<description>Exam[ple nodes how to capture viideo</description>
<maintainer email="sebastian.stern@digitalwerk.net">Sebastian Stern</maintainer>
<license>Apache License 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<build_depend>libopencv-dev</build_depend>
<build_depend>rclcpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<exec_depend>libopencv-dev</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
\ No newline at end of file
#include "adtf_ros2_video_receiver.h"
ADTF_PLUGIN("ROS2 Video Receiver", cRos2VideoReceiver);
cRos2VideoReceiver::cRos2VideoReceiver()
{
m_sCurrentFormat.m_ui32Width = 0;
m_sCurrentFormat.m_ui32Height = 0;
m_sCurrentFormat.m_szMaxByteSize = 0;
m_sCurrentFormat.m_strFormatName = ADTF_IMAGE_FORMAT(RGB_32);
m_sCurrentFormat.m_ui8DataEndianess = PLATFORM_BYTEORDER;
m_propNodeName = cString("adtf_video_reciever");
m_propTopicName = cString("adtf_topic_video");
RegisterPropertyVariable("Node name", m_propNodeName);
RegisterPropertyVariable("Topic name", m_propTopicName);
}
tResult cRos2VideoReceiver::Construct()
{
m_strNodeName = m_propNodeName;
m_strTopicName = m_propTopicName;
RETURN_IF_FAILED(cSampleStreamingSource::Construct());
RETURN_IF_FAILED(_runtime->GetObject(m_pClock));
RETURN_IF_FAILED(CreateStreamType(tTrue));
RETURN_NOERROR;
}
tResult cRos2VideoReceiver::Destruct()
{
RETURN_IF_FAILED(cSampleStreamingSource::Destruct());
RETURN_NOERROR;
}
tResult cRos2VideoReceiver::StartStreaming()
{
RETURN_IF_FAILED(cSampleStreamingSource::StartStreaming());
if (!m_pNode)
{
m_pNode = std::make_shared<rclcpp::Node>(m_strNodeName.GetPtr());
m_pSubscription = m_pNode->create_subscription<tMsg>(
m_strTopicName.GetPtr(),
std::bind(&cRos2VideoReceiver::Callback, this, _1)
);
}
m_oThread = kernel_thread(cString("ros_subscriber_thread_").Append(m_strNodeName.GetPtr()), [this]() {rclcpp::spin(m_pNode); });
RETURN_NOERROR;
}
tResult cRos2VideoReceiver::StopStreaming()
{
RETURN_IF_FAILED(cSampleStreamingSource::StopStreaming());
RETURN_NOERROR;
}
tVoid cRos2VideoReceiver::Callback(const tMsg::SharedPtr pMsg)
{
cv::Mat oFrame = imageFromMsg(pMsg);
tResult nResult = CheckStreamType(oFrame);
if (nResult.IsFailed()) return;
object_ptr<const ISample> pSample = CreateNewSample(oFrame);
if (pSample)
{
m_oWriter << pSample << trigger;
}
else
{
m_oWriter << ERR_BAD_DEVICE;
}
}
tResult cRos2VideoReceiver::CheckStreamType(const cv::Mat &oFrame)
{
tStreamImageFormat sCurrentFormat;
sCurrentFormat.m_ui32Width = oFrame.size().width;
sCurrentFormat.m_ui32Height = oFrame.size().height;
sCurrentFormat.m_szMaxByteSize = oFrame.size().width * oFrame.size().height * oFrame.channels();
sCurrentFormat.m_strFormatName = cv_MatType_2_adtf_ImageFormat(oFrame.type());
sCurrentFormat.m_ui8DataEndianess = PLATFORM_BYTEORDER;
if (sCurrentFormat != m_sCurrentFormat)
{
m_sCurrentFormat = sCurrentFormat;
RETURN_IF_FAILED(CreateStreamType(tFalse));
}
RETURN_NOERROR;
}
object_ptr<ISample> cRos2VideoReceiver::CreateNewSample(const cv::Mat &oFrame)
{
object_ptr<ISample> pSample;
if (IS_OK(alloc_sample(pSample)))
{
object_ptr_locked<ISampleBuffer> pBuffer;
if (IS_OK(pSample->WriteLock(pBuffer, m_sCurrentFormat.m_szMaxByteSize)))
{
memcpy(pBuffer->GetPtr(), oFrame.data, m_sCurrentFormat.m_szMaxByteSize);
pSample->SetTime(m_pClock->GetStreamTime());
}
}
return pSample;
}
tResult cRos2VideoReceiver::CreateStreamType(tBool bInit)
{
object_ptr<IStreamType> pType = make_object_ptr<cStreamType>(stream_meta_type_image());
RETURN_IF_FAILED(set_stream_type_image_format(*pType, m_sCurrentFormat));
if (bInit)
{
RETURN_IF_FAILED(create_pin(*this, m_oWriter, "out", pType));
RETURN_NOERROR;
}
RETURN_IF_FAILED(m_oWriter.ChangeType(pType));
RETURN_NOERROR;
}
const tChar* cRos2VideoReceiver::cv_MatType_2_adtf_ImageFormat(tInt nMatType)
{
switch (nMatType) {
case CV_8UC1:
return ADTF_IMAGE_FORMAT(GREYSCALE_8);
case CV_8UC3:
return ADTF_IMAGE_FORMAT(BGR_24);
case CV_16SC1:
return ADTF_IMAGE_FORMAT(GREYSCALE_16);
case CV_8UC4:
return ADTF_IMAGE_FORMAT(BGRA_32);
default:
return ADTF_IMAGE_FORMAT(BGR_24);
}
}
\ No newline at end of file
/**
*
* @file
* This Example shows the implementation of ros2 image/video streams as adtf Streaming Source
*
* Copyright &copy; Audi Electronics Venture GmbH. All rights reserved.
*
* $Author$
* $Date$
* $Revision$
*
*/
#pragma once
#include <memory>
#include <map>
#include <a_utils_platform_inc.h>
#include <adtf_systemsdk.h>
#include <rclcpp/rclcpp.hpp>
#include <adtf3.h>
#include <opencv2/opencv.hpp>
#include "cam_utils.hpp"
using namespace adtf::util;
using namespace adtf::ucom;
using namespace adtf::base;
using namespace adtf::streaming;
using namespace adtf::system;
using namespace adtf::mediadescription;
using std::placeholders::_1;
#define CID_ROS2_VIDEO_RECEIVER "video_receiver.streaming_source.adtf_ros2tb.cid"
class cRos2VideoReceiver : public cSampleStreamingSource
{
public:
ADTF_CLASS_ID_NAME(cRos2VideoReceiver, CID_ROS2_VIDEO_RECEIVER, "ROS2 Video Receiver");
~cRos2VideoReceiver() = default;
cRos2VideoReceiver();
tResult Construct() override;
tResult Destruct() override;
tResult StartStreaming() override;
tResult StopStreaming() override;
private:
tVoid Callback(const tMsg::SharedPtr pMsg);
object_ptr<ISample> CreateNewSample(const cv::Mat &oFrame);
tResult CheckStreamType(const cv::Mat &oFrame);
tResult CreateStreamType(tBool bInit);
static const tChar* cv_MatType_2_adtf_ImageFormat(tInt nMatType);
property_variable<cString> m_propNodeName;
property_variable<cString> m_propTopicName;
std::shared_ptr<rclcpp::Node> m_pNode;
std::shared_ptr<rclcpp::Subscription<tMsg>> m_pSubscription;
object_ptr<adtf::services::IReferenceClock> m_pClock;
kernel_thread m_oThread;
cSampleWriter m_oWriter;
cSampleCodecFactory m_oCodecFactory;
tStreamImageFormat m_sCurrentFormat;
cString m_strNodeName;
cString m_strTopicName;
};
\ No newline at end of file
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <vector>
#include <sstream>
#include "cam_utils.hpp"
const std::string g_strArgCamInterface = "--cam_interface";
const std::string g_strArgTopic = "--topic";
const std::string g_strArgRate = "--rate";
struct sCliArgs
{
int m_nCamInterface;
std::string m_oTopic;
double m_fRate;
};
bool parse_args(int nArgc, char **aArgv, sCliArgs &parsedArgs)
{
std::vector<std::string> oArgs(aArgv, aArgv + nArgc);
// check args
if (oArgs.size() < 4) return false;
// parse args
int i = 0;
for (auto const& s : oArgs)
{
if (s.find(g_strArgCamInterface) != std::string::npos)
{
parsedArgs.m_nCamInterface = std::stoi(s.substr(s.find('=')+1));
i++;
}
else if (s.find(g_strArgTopic) != std::string::npos)
{
parsedArgs.m_oTopic = s.substr(s.find('=')+1);
i++;
}
else if (s.find(g_strArgRate) != std::string::npos)
{
parsedArgs.m_fRate = std::stod(s.substr(s.find('=')+1));
i++;
}
}
return (i==3) ? true : false;
}
int main (int argc, char **argv)
{
std::stringstream oSS;
// init ros
rclcpp::init(argc, argv);
// create node
auto pNode = rclcpp::Node::make_shared("cam_capture");
// parse needed args
sCliArgs oArgs;
if (!parse_args(argc, argv, oArgs))
{
oSS.flush();
oSS << "Incorrect args!" << std::endl;
oSS << "*****Parsed Args*****" << std::endl;
oSS << g_strArgCamInterface << ": " << oArgs.m_nCamInterface << std::endl;
oSS << g_strArgTopic << ": " << oArgs.m_oTopic << std::endl;
oSS << g_strArgRate << ": " << oArgs.m_fRate << std::endl;
oSS << "*****Expected Args*****: " << std::endl;
oSS << g_strArgCamInterface << ": " << "Integer number" << std::endl;
oSS << g_strArgTopic << ": " << "str topic"<< std::endl;
oSS << g_strArgRate << ": " << "integer, float number" << std::endl;
RCLCPP_ERROR(pNode->get_logger(), oSS.str().c_str());
return -1;
}
// create publisher, msg-ptr and timer
auto pPub = pNode->create_publisher<tMsg>(oArgs.m_oTopic);
auto pMsg = std::make_shared<tMsg>();
pMsg->is_bigendian = false;
rclcpp::WallRate oLoopRate(oArgs.m_fRate);
// init open cv
cv::VideoCapture oCap = cv::VideoCapture(oArgs.m_nCamInterface);
if (!oCap.isOpened())
{
oSS.flush();
oSS << "Couldn't find camera: " << std::to_string(oArgs.m_nCamInterface) << std::endl;
RCLCPP_ERROR(pNode->get_logger(), oSS.str().c_str());
return -2;
}
while (rclcpp::ok())
{
cv::Mat oFrame;
oCap >> oFrame;
if (!oFrame.empty())
{
cv::imshow("cam_capture", oFrame);
msgFromImage(oFrame, pMsg);
pPub->publish(pMsg);
rclcpp::spin_some(pNode);
if (cv::waitKey(10) == 27) break;
}
oLoopRate.sleep();
}
rclcpp::shutdown();
return 0;
}
#include <opencv2/opencv.hpp>
#include <opencv2/dnn.hpp>
#include <opencv2/dnn/shape_utils.hpp>
#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <sstream>
#include <fstream>
#include <experimental/filesystem>
#include "cam_utils.hpp"
namespace fs = std::experimental::filesystem;
const std::string g_strArgTopicSub = "--topic_sub";
const std::string g_strArgTopicPub = "--topic_pub";
const std::string g_strArgYoloDir = "--dnn_dir";
const std::string g_strArgThreshold = "--threshold";
const std::string g_strWeightsFile= "yolo.weights";
const std::string g_strConfigFile = "yolo.cfg";
const std::string g_strClassesFile = "yolo.txt";
const double g_fNmsThreshold = 0.4;
struct sCliArgs
{
std::string m_strTopic_sub;
std::string m_strTopic_pub;
std::string m_strDir;
float m_fConfidenceThreshold;
};
struct sDNNArgs
{
fs::path m_oWeightsFile;
fs::path m_oConfigFile;
fs::path m_oClassesFile;
std::vector<std::string> m_oClasses;
};
bool find_file(const fs::path &oDirPath, const std::string &oFileName, fs::path &oPathFound)
{
if (!fs::exists(oDirPath)) return false;
fs::directory_iterator itrEnd;
for (fs::directory_iterator itr(oDirPath); itr != itrEnd; ++itr)
{
if (fs::is_directory(itr->status()))
{
if (find_file(itr->path(), oFileName, oPathFound)) return true;
}
else if (itr->path().filename() == oFileName)
{
oPathFound = itr->path();
return true;
}
}
return false;
}
bool parse_args(int nArgc, char **aArgv, sCliArgs &oParsedArgs)
{
std::vector<std::string> oArgs(aArgv, aArgv + nArgc);
// check args
if (oArgs.size() < 5) return false;
// parse args
int i = 0;
for (auto const& s : oArgs)
{
if (s.find(g_strArgTopicSub) != std::string::npos)
{
oParsedArgs.m_strTopic_sub = s.substr(s.find('=') + 1);