diff --git a/.gitmodules b/.gitmodules index 42d15112..4ef259cb 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/README.md b/README.md index c97bfadd..a5b421ea 100644 --- a/README.md +++ b/README.md @@ -3,20 +3,41 @@ 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 ``` + +## 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/CMakeLists.txt b/src/stingray_depth_vision/CMakeLists.txt new file mode 100644 index 00000000..7a5e4efb --- /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 00000000..2f7d5b75 --- /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/include/depthcam.h b/src/stingray_depth_vision/include/depthcam.h new file mode 100644 index 00000000..41463fe9 --- /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/msg/Object.msg b/src/stingray_depth_vision/msg/Object.msg new file mode 100644 index 00000000..d347bb6d --- /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 00000000..361036ed --- /dev/null +++ b/src/stingray_depth_vision/package.xml @@ -0,0 +1,21 @@ + + + stingray_depth_vision + 0.0.1 + 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/depthcam.cpp b/src/stingray_depth_vision/src/depthcam.cpp new file mode 100644 index 00000000..6fd79cc4 --- /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 new file mode 100644 index 00000000..97a3a594 --- /dev/null +++ b/src/stingray_depth_vision/src/distance_measure.cpp @@ -0,0 +1,17 @@ +#include +#include +#include "depthcam.h" +#include "stingray_depth_vision/Object.h" + +void frameCallback(const distance_measure::Object::ConstPtr& msg) { + +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "zed_subscriber"); + ros::NodeHandle handle; + ros::Subscriber subDepth = handle.subscribe("/zedm/zed_node/depth/depth_registered", 10, depthCallback); + // TODO: publish distance + ros::spin(); + return 0; +} diff --git a/src/zed-ros-wrapper b/src/zed-ros-wrapper new file mode 160000 index 00000000..5984f580 --- /dev/null +++ b/src/zed-ros-wrapper @@ -0,0 +1 @@ +Subproject commit 5984f580e5782cd121ac9b39b84ab231c5439e2c