From e397574288344a4d6a2bd3910797c39392657d4f Mon Sep 17 00:00:00 2001 From: sovadim Date: Tue, 17 Nov 2020 21:11:59 +0300 Subject: [PATCH 1/4] add zed-wrapper as submodule --- .gitmodules | 3 +++ src/zed-ros-wrapper | 1 + 2 files changed, 4 insertions(+) create mode 160000 src/zed-ros-wrapper diff --git a/.gitmodules b/.gitmodules index 42d1511..4ef259c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "src/serial"] path = src/serial url = https://github.com/wjwwood/serial.git +[submodule "src/zed-ros-wrapper"] + path = src/zed-ros-wrapper + url = https://github.com/stereolabs/zed-ros-wrapper.git diff --git a/src/zed-ros-wrapper b/src/zed-ros-wrapper new file mode 160000 index 0000000..5984f58 --- /dev/null +++ b/src/zed-ros-wrapper @@ -0,0 +1 @@ +Subproject commit 5984f580e5782cd121ac9b39b84ab231c5439e2c From 0d0c20658cc5fe342231f545a7b15744ccb8f49c Mon Sep 17 00:00:00 2001 From: sovadim Date: Wed, 18 Nov 2020 00:12:06 +0300 Subject: [PATCH 2/4] depth node sources --- src/stingray_depth_vision/CMakeLists.txt | 203 ++++++++++++++++++ src/stingray_depth_vision/README.md | 9 + src/stingray_depth_vision/msg/Object.msg | 6 + src/stingray_depth_vision/package.xml | 21 ++ .../src/distance_measure.cpp | 58 +++++ 5 files changed, 297 insertions(+) create mode 100644 src/stingray_depth_vision/CMakeLists.txt create mode 100644 src/stingray_depth_vision/README.md create mode 100644 src/stingray_depth_vision/msg/Object.msg create mode 100644 src/stingray_depth_vision/package.xml create mode 100644 src/stingray_depth_vision/src/distance_measure.cpp diff --git a/src/stingray_depth_vision/CMakeLists.txt b/src/stingray_depth_vision/CMakeLists.txt new file mode 100644 index 0000000..7a5e4ef --- /dev/null +++ b/src/stingray_depth_vision/CMakeLists.txt @@ -0,0 +1,203 @@ +cmake_minimum_required(VERSION 3.0.2) +project(stingray_depth_vision) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++17) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + Object.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages() + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES stingray_depth_vision + CATKIN_DEPENDS message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/stingray_depth_vision.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/distance_measure.cpp) +target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_stingray_depth_vision.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/stingray_depth_vision/README.md b/src/stingray_depth_vision/README.md new file mode 100644 index 0000000..2f7d5b7 --- /dev/null +++ b/src/stingray_depth_vision/README.md @@ -0,0 +1,9 @@ +# Stingray depth package + +## Run ZED nodes + +```bash +$ cd zed-ros-wrapper +$ source devel/setup.zsh +$ roslaunch zed_wrapper zedm.launch +``` diff --git a/src/stingray_depth_vision/msg/Object.msg b/src/stingray_depth_vision/msg/Object.msg new file mode 100644 index 0000000..d347bb6 --- /dev/null +++ b/src/stingray_depth_vision/msg/Object.msg @@ -0,0 +1,6 @@ +string name +float32 confidence +int16 top_left_x +int16 top_left_y +int16 bottom_right_x +int16 bottom_right_y \ No newline at end of file diff --git a/src/stingray_depth_vision/package.xml b/src/stingray_depth_vision/package.xml new file mode 100644 index 0000000..f0115f6 --- /dev/null +++ b/src/stingray_depth_vision/package.xml @@ -0,0 +1,21 @@ + + + stingray_depth_vision + 0.0.0 + The stingray_depth_vision package + + Sofin Vadim + + MIT + + Sofin Vadim + + catkin + roscpp + roscpp + roscpp + message_generation + message_runtime + + + diff --git a/src/stingray_depth_vision/src/distance_measure.cpp b/src/stingray_depth_vision/src/distance_measure.cpp new file mode 100644 index 0000000..2ba0a3e --- /dev/null +++ b/src/stingray_depth_vision/src/distance_measure.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include +#include "stingray_depth_vision/Object.h" + +using namespace boost::accumulators; + +class CameraActions { + +private: + int16_t top_left_x; + int16_t top_left_y; + int16_t bottom_right_x; + int16_t bottom_right_y; +}; + +void depthCallback(const sensor_msgs::Image::ConstPtr& msg) { + float* depths = (float*)(&msg->data[0]); + + // TODO: subscribe to these values + uint32_t left_x = 0; + uint32_t right_x = msg->width; + uint32_t upper_y = 0; + uint32_t lower_y = msg->height; + + accumulator_set> acc; + + for (uint32_t h = upper_y; h < lower_y; ++h) { + for (uint32_t w = left_x; w < right_x; ++w) { + if (!std::isnan(depths[w + (h - upper_y)*w]) && !std::isinf(depths[w + (h - upper_y)*w])) { + acc(depths[w + (h - upper_y)*w]); + } + } + } + + if (count(acc) == 0) + ROS_WARN("All data is nan or inf"); + else + ROS_INFO("Mean distance: %g, Counted: %ld", mean(acc), count(acc)); +} + +void netCallback(const distance_measure::Object::ConstPtr& msg) { + +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "zed_subscriber"); + + ros::NodeHandle n; + + ros::Subscriber subDepth = n.subscribe("/zedm/zed_node/depth/depth_registered", 10, depthCallback); + + ros::spin(); + + return 0; +} From 328552262e11dbdf2f80a42f7bbba1e10d48a48f Mon Sep 17 00:00:00 2001 From: sovadim Date: Wed, 18 Nov 2020 00:12:21 +0300 Subject: [PATCH 3/4] CUDA installation --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index c97bfad..c820c94 100644 --- a/README.md +++ b/README.md @@ -20,3 +20,7 @@ Do not forget to setup workspace before you start to work: ```bash source devel/setup.bash ``` + +## Cuda installation + +[NVIDIA CUDA Installation Guide for Linux](https://docs.nvidia.com/cuda/cuda-installation-guide-linux/index.html) From 6d224e569d489b3f2b5418ee4e422bc6a5286b9e Mon Sep 17 00:00:00 2001 From: sovadim Date: Tue, 16 Mar 2021 00:21:12 +0300 Subject: [PATCH 4/4] wip --- README.md | 17 +++++ src/stingray_depth_vision/include/depthcam.h | 64 +++++++++++++++++++ src/stingray_depth_vision/package.xml | 2 +- src/stingray_depth_vision/src/depthcam.cpp | 5 ++ .../src/distance_measure.cpp | 51 ++------------- 5 files changed, 92 insertions(+), 47 deletions(-) create mode 100644 src/stingray_depth_vision/include/depthcam.h create mode 100644 src/stingray_depth_vision/src/depthcam.cpp diff --git a/README.md b/README.md index c820c94..a5b421e 100644 --- a/README.md +++ b/README.md @@ -3,19 +3,30 @@ Autonomous underwater vehicle platform based on ROS ## Building Install dependencies: + ```bash sudo apt-get install ros-$ROS_DISTRO-rosbridge-server ros-$ROS_DISTRO-image-view ros-$ROS_DISTRO-actionlib ros-$ROS_DISTRO-smach ros-$ROS_DISTRO-smach-viewer ``` + Initialize and update git submodules used in project: + ```bash git submodule init git submodule update ``` + Use following commands to build: + ```bash source /opt/ros/$ROS_DISTRO/setup.bash catkin_make ``` + +Build debug: +```bash +catkin_make -DCMAKE_BUILD_TYPE=Debug +``` + Do not forget to setup workspace before you start to work: ```bash source devel/setup.bash @@ -24,3 +35,9 @@ source devel/setup.bash ## Cuda installation [NVIDIA CUDA Installation Guide for Linux](https://docs.nvidia.com/cuda/cuda-installation-guide-linux/index.html) + +## How to run tests + +```bash + +``` diff --git a/src/stingray_depth_vision/include/depthcam.h b/src/stingray_depth_vision/include/depthcam.h new file mode 100644 index 0000000..41463fe --- /dev/null +++ b/src/stingray_depth_vision/include/depthcam.h @@ -0,0 +1,64 @@ +#ifndef STINGRAY_DEPTH_VISION_INCLUDE_DEPTHCAM_H_ +#define STINGRAY_DEPTH_VISION_INCLUDE_DEPTHCAM_H_ + +#include +#include +#include + +namespace depth { + +typedef struct Point { + int32_t x; + int32_t y; +}; + +typedef struct ImageFrame { + Point topLeft; + Point bottomRight; +}; + +class DepthCamera { + + static void depthCallback(const sensor_msgs::Image::ConstPtr& msg) { + float* depths = std::reinterpret_cast(&msg->data[0]); + } + + static size_t getMean(const float const *image, const std::optional &frame, size_t stride=1) { +#ifdef DEBUG + boost::accumulators::accumulator_set> acc; +#else + boost::accumulators::accumulator_set> acc; +#endif + // TODO: create iterator on frame + for (size_t h = upper_y; h < lower_y; ++h) { + for (size_t w = frame.top_left_x; w < frame.bottom_right_x; ++w) { + if (depthIsValid(depths[w + (h - upper_y)*w])) { + acc(depths[w + (h - upper_y)*w]); + } + } + } + + if (count(acc) == 0) + ROS_WARN("All data is nan or inf"); + else +#ifdef DEBUG + ROS_INFO("Mean distance: %g, Counted: %ld", boost::accumulators::mean(acc), boost::accumulators::count(acc)); +#endif + } + +private: + std::unique_ptr frame; + std::unique_ptr depths; + + bool dataIsInitialized() const { + return (depths != nullptr && frame != nullptr); + } + + bool depthIsValid(float value) const { + return (!std::isnan(value) && !std::isinf(value)); + } +}; + +} // namespace depth + +#endif // STINGRAY_DEPTH_VISION_INCLUDE_DEPTHCAM_H_ diff --git a/src/stingray_depth_vision/package.xml b/src/stingray_depth_vision/package.xml index f0115f6..361036e 100644 --- a/src/stingray_depth_vision/package.xml +++ b/src/stingray_depth_vision/package.xml @@ -1,7 +1,7 @@ stingray_depth_vision - 0.0.0 + 0.0.1 The stingray_depth_vision package Sofin Vadim diff --git a/src/stingray_depth_vision/src/depthcam.cpp b/src/stingray_depth_vision/src/depthcam.cpp new file mode 100644 index 0000000..6fd79cc --- /dev/null +++ b/src/stingray_depth_vision/src/depthcam.cpp @@ -0,0 +1,5 @@ +#include "depthcam.h" + +namespace depth { + +} // namespace depth diff --git a/src/stingray_depth_vision/src/distance_measure.cpp b/src/stingray_depth_vision/src/distance_measure.cpp index 2ba0a3e..97a3a59 100644 --- a/src/stingray_depth_vision/src/distance_measure.cpp +++ b/src/stingray_depth_vision/src/distance_measure.cpp @@ -1,58 +1,17 @@ #include -#include -#include -#include #include +#include "depthcam.h" #include "stingray_depth_vision/Object.h" -using namespace boost::accumulators; - -class CameraActions { - -private: - int16_t top_left_x; - int16_t top_left_y; - int16_t bottom_right_x; - int16_t bottom_right_y; -}; - -void depthCallback(const sensor_msgs::Image::ConstPtr& msg) { - float* depths = (float*)(&msg->data[0]); - - // TODO: subscribe to these values - uint32_t left_x = 0; - uint32_t right_x = msg->width; - uint32_t upper_y = 0; - uint32_t lower_y = msg->height; - - accumulator_set> acc; - - for (uint32_t h = upper_y; h < lower_y; ++h) { - for (uint32_t w = left_x; w < right_x; ++w) { - if (!std::isnan(depths[w + (h - upper_y)*w]) && !std::isinf(depths[w + (h - upper_y)*w])) { - acc(depths[w + (h - upper_y)*w]); - } - } - } - - if (count(acc) == 0) - ROS_WARN("All data is nan or inf"); - else - ROS_INFO("Mean distance: %g, Counted: %ld", mean(acc), count(acc)); -} - -void netCallback(const distance_measure::Object::ConstPtr& msg) { +void frameCallback(const distance_measure::Object::ConstPtr& msg) { } int main(int argc, char **argv) { ros::init(argc, argv, "zed_subscriber"); - - ros::NodeHandle n; - - ros::Subscriber subDepth = n.subscribe("/zedm/zed_node/depth/depth_registered", 10, depthCallback); - + ros::NodeHandle handle; + ros::Subscriber subDepth = handle.subscribe("/zedm/zed_node/depth/depth_registered", 10, depthCallback); + // TODO: publish distance ros::spin(); - return 0; }