Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add TrackletConverter + Example Publisher #313

Open
wants to merge 9 commits into
base: noetic
Choose a base branch
from
Open

add TrackletConverter + Example Publisher #313

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
aeeb493
Add TrackletConverter
ninoweg May 13, 2023
4809bca
Add TrackletConverter
ninoweg May 13, 2023
798d42c
Add msg for Tracklets
ninoweg May 13, 2023
bf548e0
remove toRosVisionMsg
ninoweg May 14, 2023
3341e69
add example tracker publisher
ninoweg May 14, 2023
6e91b44
replace ImgDetections with vision_msgs
ninoweg May 15, 2023
84c176e
update denormalization
ninoweg May 15, 2023
d181250
update denormalization
ninoweg May 15, 2023
bba2b8c
update pipeline
ninoweg May 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions depthai_bridge/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ FILE(GLOB LIB_SRC
"src/ImageConverter.cpp"
"src/ImgDetectionConverter.cpp"
"src/SpatialDetectionConverter.cpp"
"src/TrackletConverter.cpp"
"src/ImuConverter.cpp"
)

Expand Down
42 changes: 42 additions & 0 deletions depthai_bridge/include/depthai_bridge/TrackletConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
#pragma once

#include <deque>
#include <memory>
#include <string>

#include "depthai/pipeline/datatype/Tracklets.hpp"
#include "depthai_ros_msgs/TrackletArray.h"
#include "ros/ros.h"

namespace dai {

namespace ros {
namespace TrackletMessages = depthai_ros_msgs;
using TrackletArrayPtr = TrackletMessages::TrackletArray::Ptr;
class TrackletConverter {
public:
TrackletConverter(std::string frameName,
int width_srcImgDetection,
int height_srcImgDetection,
int width_srcImgTracker,
int height_srcImgTracker,
bool normalized_srcImgDetection = false,
bool normalized_srcImgTracker = false,
bool getBaseDeviceTimestamp = false);
void toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<TrackletMessages::TrackletArray>& trackletsMsg);
TrackletArrayPtr toRosMsgPtr(std::shared_ptr<dai::Tracklets> trackData);

private:
int _width_srcImgDetection, _height_srcImgDetection, _width_srcImgTracker, _height_srcImgTracker;
const std::string _frameName;
bool _normalized_srcImgDetection, _normalized_srcImgTracker;
std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
::ros::Time _rosBaseTime;
bool _getBaseDeviceTimestamp;
};

} // namespace ros

namespace rosBridge = ros;

} // namespace dai
108 changes: 108 additions & 0 deletions depthai_bridge/src/TrackletConverter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
#include "depthai_bridge/TrackletConverter.hpp"
#include "depthai_bridge/depthaiUtility.hpp"

namespace dai {
namespace ros {

TrackletConverter::TrackletConverter(std::string frameName,
int width_srcImgDetection,
int height_srcImgDetection,
int width_srcImgTracker,
int height_srcImgTracker,
bool normalized_srcImgDetection,
bool normalized_srcImgTracker,
bool getBaseDeviceTimestamp)
: _frameName(frameName),
_width_srcImgDetection(width_srcImgDetection),
_height_srcImgDetection(height_srcImgDetection),
_width_srcImgTracker(width_srcImgTracker),
_height_srcImgTracker(height_srcImgTracker),
_normalized_srcImgDetection(normalized_srcImgDetection),
_normalized_srcImgTracker(normalized_srcImgTracker),
_steadyBaseTime(std::chrono::steady_clock::now()),
_getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
_rosBaseTime = ::ros::Time::now();
}

void TrackletConverter::toRosMsg(std::shared_ptr<dai::Tracklets> trackData, std::deque<TrackletMessages::TrackletArray>& trackletMsgs) {
// setting the header
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
tstamp = trackData->getTimestampDevice();
else
tstamp = trackData->getTimestamp();

TrackletMessages::TrackletArray trackletsMsg;

trackletsMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
trackletsMsg.header.frame_id = _frameName;
trackletsMsg.tracklets.resize(trackData->tracklets.size());

// publishing
for(int i = 0; i < trackData->tracklets.size(); ++i) {
// srcImgDetection -> inputDetectionsFrame
int xMin_srcImgDetection, yMin_srcImgDetection, xMax_srcImgDetection, yMax_srcImgDetection;
if(_normalized_srcImgDetection) {
xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin;
yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin;
xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax;
yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax;
} else {
xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin * _width_srcImgDetection;
yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin * _height_srcImgDetection;
xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax * _width_srcImgDetection;
yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax * _height_srcImgDetection;
}
auto xSize_srcImgDetection = xMax_srcImgDetection - xMin_srcImgDetection;
auto ySize_srcImgDetection = yMax_srcImgDetection - yMin_srcImgDetection;
auto xCenter_srcImgDetection = xMin_srcImgDetection + xSize_srcImgDetection / 2;
auto yCenter_srcImgDetection = yMin_srcImgDetection + ySize_srcImgDetection / 2;

// roi -> inputTrackerFrame
dai::Rect roi;
if(_normalized_srcImgTracker) {
roi = trackData->tracklets[i].roi;
} else {
roi = trackData->tracklets[i].roi.denormalize(_width_srcImgTracker, _height_srcImgTracker);
}
auto xSize_srcImgTracker = roi.size().width;
auto ySize_srcImgTracker = roi.size().height;
auto xCenter_srcImgTracker = roi.topLeft().x + roi.width / 2;
auto yCenter_srcImgTracker = roi.topLeft().y + roi.height / 2;

trackletsMsg.tracklets[i].roi.center.x = xCenter_srcImgTracker;
trackletsMsg.tracklets[i].roi.center.y = yCenter_srcImgTracker;
trackletsMsg.tracklets[i].roi.size_x = xSize_srcImgTracker;
trackletsMsg.tracklets[i].roi.size_y = ySize_srcImgTracker;

trackletsMsg.tracklets[i].id = trackData->tracklets[i].id;
trackletsMsg.tracklets[i].label = trackData->tracklets[i].label;
trackletsMsg.tracklets[i].age = trackData->tracklets[i].age;
trackletsMsg.tracklets[i].status = static_cast<std::underlying_type<dai::Tracklet::TrackingStatus>::type>(trackData->tracklets[i].status);

trackletsMsg.tracklets[i].srcImgDetectionHypothesis.score = trackData->tracklets[i].srcImgDetection.confidence;
trackletsMsg.tracklets[i].srcImgDetectionHypothesis.id = trackData->tracklets[i].srcImgDetection.label;
trackletsMsg.tracklets[i].srcImgDetectionBBox.center.x = xCenter_srcImgDetection;
trackletsMsg.tracklets[i].srcImgDetectionBBox.center.y = yCenter_srcImgDetection;
trackletsMsg.tracklets[i].srcImgDetectionBBox.size_x = xSize_srcImgDetection;
trackletsMsg.tracklets[i].srcImgDetectionBBox.size_y = ySize_srcImgDetection;

// converting mm to meters since per ros rep-103 lenght should always be in meters
trackletsMsg.tracklets[i].spatialCoordinates.x = trackData->tracklets[i].spatialCoordinates.x / 1000;
trackletsMsg.tracklets[i].spatialCoordinates.y = trackData->tracklets[i].spatialCoordinates.y / 1000;
trackletsMsg.tracklets[i].spatialCoordinates.z = trackData->tracklets[i].spatialCoordinates.z / 1000;
}

trackletMsgs.push_back(trackletsMsg);
}

TrackletArrayPtr TrackletConverter::toRosMsgPtr(std::shared_ptr<dai::Tracklets> trackData) {
std::deque<TrackletMessages::TrackletArray> msgQueue;
toRosMsg(trackData, msgQueue);
auto msg = msgQueue.front();
TrackletArrayPtr ptr = boost::make_shared<TrackletMessages::TrackletArray>(msg);
return ptr;
}

} // namespace ros
} // namespace dai
3 changes: 3 additions & 0 deletions depthai_examples/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,9 +111,11 @@ dai_add_node(stereo_inertial_node src/stereo_inertial_publisher.cpp)

dai_add_node(stereo_node src/stereo_publisher.cpp)
dai_add_node(yolov4_spatial_node src/yolov4_spatial_publisher.cpp)
dai_add_node(tracker_node src/tracker_publisher.cpp)

target_compile_definitions(mobilenet_node PRIVATE BLOB_NAME="${mobilenet_blob_name}")
target_compile_definitions(yolov4_spatial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}")
target_compile_definitions(tracker_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}")
target_compile_definitions(stereo_inertial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}")

catkin_install_python(PROGRAMS
Expand All @@ -131,6 +133,7 @@ install(TARGETS
stereo_inertial_node
stereo_node
yolov4_spatial_node
tracker_node
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
Expand Down
81 changes: 81 additions & 0 deletions depthai_examples/launch/tracker_publisher.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
<?xml version="1.0"?>
<launch>
<!-- <args for urdf/> -->
<arg name="camera_model" default="OAK-D" /> <!-- 'zed' or 'zedm' -->
<arg name="tf_prefix" default="oak" />
<arg name="base_frame" default="oak-d_frame" />
<arg name="parent_frame" default="oak-d-base-frame" />
<arg name="publisher_name" default="tracker_publisher" />

<arg name="cam_pos_x" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->

<arg name="camera_param_uri" default="package://depthai_examples/params/camera" />
<arg name="sync_nn" default="true"/>
<arg name="subpixel" default="true"/>
<arg name="confidence" default="200" />
<arg name="LRchecktresh" default="5" />
<arg name="lrcheck" default="true" />
<arg name="extended" default="false" />
<arg name="monoResolution" default="400p"/> <!-- '720p', '800p', 400p' for OAK-D & '480p' for OAK-D-Lite -->
<arg name="monoFPS" default="30"/>
<arg name="rgbFPS" default="30"/>
<arg name="nnName" default="x" />
<arg name="resourceBaseFolder" default="$(find depthai_examples)/resources" />
<arg name="previewWidth" default="416" />
<arg name="previewHeight" default="416" />
<arg name="rgbResolution" default="1080p"/>
<arg name="rgbScaleNumerator" default="2"/>
<arg name="rgbScaleDinominator" default="3"/>
<arg name="enableDotProjector" default="true"/>
<arg name="enableFloodLight" default="true"/>
<arg name="dotProjectormA" default="1200.0"/>
<arg name="floodLightmA" default="400.0"/>
<arg name="angularVelCovariance" default="0" />
<arg name="linearAccelCovariance" default="0" />

<include file="$(find depthai_descriptions)/launch/urdf.launch">
<arg name="base_frame" value="$(arg base_frame)" />
<arg name="parent_frame" value="$(arg parent_frame)"/>
<arg name="camera_model" value="$(arg camera_model)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)" />
<arg name="cam_pos_x" value="$(arg cam_pos_x)" />
<arg name="cam_pos_y" value="$(arg cam_pos_y)" />
<arg name="cam_pos_z" value="$(arg cam_pos_z)" />
<arg name="cam_roll" value="$(arg cam_roll)" />
<arg name="cam_pitch" value="$(arg cam_pitch)" />
<arg name="cam_yaw" value="$(arg cam_yaw)" />
</include>

<node name="$(arg publisher_name)" pkg="depthai_examples" type="tracker_node" output="screen" required="true">
<param name="tf_prefix" value="$(arg tf_prefix)" />
<param name="camera_param_uri" value="$(arg camera_param_uri)"/>
<param name="sync_nn" value="$(arg sync_nn)"/>
<param name="subpixel" value="$(arg subpixel)"/>
<param name="confidence" value="$(arg confidence)"/>
<param name="LRchecktresh" value="$(arg LRchecktresh)"/>
<param name="monoResolution" value="$(arg monoResolution)"/>
<param name="monoFPS" value="$(arg monoFPS)"/>
<param name="nnName" value="$(arg nnName)"/>
<param name="resourceBaseFolder" value="$(arg resourceBaseFolder)"/>
<param name="previewWidth" value="$(arg previewWidth)"/>
<param name="previewHeight" value="$(arg previewHeight)"/>
<param name="lrcheck" value="$(arg lrcheck)"/>
<param name="extended" value="$(arg extended)"/>
<param name="rgbFPS" value="$(arg rgbFPS)"/>
<param name="rgbResolution" value="$(arg rgbResolution)"/>
<param name="rgbScaleNumerator" value="$(arg rgbScaleNumerator)"/>
<param name="rgbScaleDinominator" value="$(arg rgbScaleDinominator)"/>
<param name="enableDotProjector" value="$(arg enableDotProjector)"/>
<param name="enableFloodLight" value="$(arg enableFloodLight)"/>
<param name="dotProjectormA" value="$(arg dotProjectormA)"/>
<param name="floodLightmA" value="$(arg floodLightmA)"/>
<param name="angularVelCovariance" value="$(arg angularVelCovariance)" />
<param name="linearAccelCovariance" value="$(arg linearAccelCovariance)" />
</node>

</launch>
Loading