From f968871ff042c3c4c6f1ee3cdd8d8406acaffc62 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Fri, 20 Jan 2023 22:03:40 -0500 Subject: [PATCH 01/11] add ros wrapper around opencv pose detection code --- uwrt_mars_rover_vision | 1 + 1 file changed, 1 insertion(+) create mode 160000 uwrt_mars_rover_vision diff --git a/uwrt_mars_rover_vision b/uwrt_mars_rover_vision new file mode 160000 index 00000000..91d57af1 --- /dev/null +++ b/uwrt_mars_rover_vision @@ -0,0 +1 @@ +Subproject commit 91d57af1c044a46dfbeec8d849004eaca38671a0 From 721ac88e9dc021eaf7a02a30f1dd45db9d62746b Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Fri, 20 Jan 2023 22:08:49 -0500 Subject: [PATCH 02/11] fix submodule not adding files --- uwrt_mars_rover_vision | 1 - uwrt_mars_rover_vision/CMakeLists.txt | 77 +++++++++++++++ .../aruco_target_tracker.hpp | 59 ++++++++++++ .../uwrt_mars_rover_vision/visibility.h | 65 +++++++++++++ .../launch/zed_vision_launch.py | 41 ++++++++ uwrt_mars_rover_vision/package.xml | 31 ++++++ .../src/aruco_target_tracker.cpp | 94 +++++++++++++++++++ 7 files changed, 367 insertions(+), 1 deletion(-) delete mode 160000 uwrt_mars_rover_vision create mode 100644 uwrt_mars_rover_vision/CMakeLists.txt create mode 100644 uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp create mode 100644 uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h create mode 100644 uwrt_mars_rover_vision/launch/zed_vision_launch.py create mode 100644 uwrt_mars_rover_vision/package.xml create mode 100644 uwrt_mars_rover_vision/src/aruco_target_tracker.cpp diff --git a/uwrt_mars_rover_vision b/uwrt_mars_rover_vision deleted file mode 160000 index 91d57af1..00000000 --- a/uwrt_mars_rover_vision +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 91d57af1c044a46dfbeec8d849004eaca38671a0 diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt new file mode 100644 index 00000000..c0e3d663 --- /dev/null +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.5) +project(uwrt_mars_rover_vision) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS highgui imgcodecs imgproc videoio) +# add custom messages and services +find_package(rosidl_default_generators REQUIRED) +# add ros joy package +find_package(sensor_msgs REQUIRED) + +# set(msg_files +# "msg/XboxController.msg" +# ) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# ${msg_files} +# DEPENDENCIES std_msgs +# ) + +# ament_export_dependencies(rosidl_default_runtime) + +#include the 'include' directory +include_directories(include) + +#create resource which references the libraries in the binary bin +set(node_plugins "") + +# add the aruco target tracker component +# add_library(xbox_controller SHARED +# src/uwrt_mars_rover_xbox_controller.cpp) +# target_compile_definitions(xbox_controller +# PRIVATE "UWRT_MARS_ROVER_XBOX_CONTROLLER_DLL") +# ament_target_dependencies(xbox_controller +# "rclcpp" +# "rclcpp_components" +# "sensor_msgs") +# # build ROS custom messages +# rosidl_target_interfaces(xbox_controller ${PROJECT_NAME} "rosidl_typesupport_cpp") +# rclcpp_components_register_nodes(xbox_controller "uwrt_xbox::UWRTXboxController") +# set(node_plugins "${node_plugins}uwrt_xbox::UWRTXboxController;$\n") + + + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# # the following line skips the linter which checks for copyrights +# # uncomment the line when a copyright and license is not present in all source files +# #set(ament_cmake_copyright_FOUND TRUE) +# # the following line skips cpplint (only works in a git repo) +# # uncomment the line when this package is not in a git repo +# #set(ament_cmake_cpplint_FOUND TRUE) +# ament_lint_auto_find_test_dependencies() +# endif() + +# tell where to put binaries +install(TARGETS +# xbox_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp new file mode 100644 index 00000000..85f3f37b --- /dev/null +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -0,0 +1,59 @@ +#ifndef ARUCO_TARGET_TRACKER_H +#define ARUCO_TARGET_TRACKER_H + +#include +#include +#include +#include +#include "image_transport/image_transport.hpp" +#include "opencv2/highgui/highgui.hpp" + +#include +#include "opencv2/imgproc.hpp" +#include + +#include + + +namespace uwrt_autonomy { + +class TargetTracker { + +public: + TargetTracker(const rclcpp::NodeOptions &options); + +private: + // use image transport for camera subscriber + image_transport::ImageTransport it_; + // zed2 image topic subscriber + image_transport::Subscriber camera_sub_; + // camera subsciber image callback + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + + // pose publisher for aruco tags + rclcpp::Publisher::SharedPtr aruco_pose_pub_; + + // zed calibration stuff + cv::Mat dist_coefficients_; + cv::Mat intrinsic_calib_matrix_; + + // vectors for ARUCO tag poses (max of 4 aruco codes identified at a time) + std::vector rvecs_(4, 0.0), t_vecs_(4, 0.0); + + // aruco detector things + cv::Ptr params_; + cv::Ptr dictionary_; + // location of corners in object's reference frame + // NOTE: below assumes fixed size for aruco tags + cv::Mat obj_points_(4, 1, CV_32FC3); + + // marker length in meters for ARUCO code. Be sure to change it if the marker length changes + // TODO: change this into a configurable parameter + float ARUCO_MARKER_LEN = 18.4 / 100; + + +} + +} + +#endif // ARUCO_TARGET_TRACKER_H \ No newline at end of file diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h new file mode 100644 index 00000000..7d23baf5 --- /dev/null +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h @@ -0,0 +1,65 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UWRT_MARS_ROVER_VISION__VISIBILITY_H_ +#define UWRT_MARS_ROVER_VISION__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + +#ifdef __GNUC__ +#define UWRT_MARS_ROVER_VISION_EXPORT __attribute__((dllexport)) +#define UWRT_MARS_ROVER_VISION_IMPORT __attribute__((dllimport)) +#else +#define UWRT_MARS_ROVER_VISION_EXPORT __declspec(dllexport) +#define UWRT_MARS_ROVER_VISION_IMPORT __declspec(dllimport) +#endif + +#ifdef UWRT_MARS_ROVER_VISION_DLL +#define UWRT_MARS_ROVER_VISION_PUBLIC UWRT_MARS_ROVER_VISION_EXPORT +#else +#define UWRT_MARS_ROVER_VISION_PUBLIC UWRT_MARS_ROVER_VISION_IMPORT +#endif + +#define UWRT_MARS_ROVER_VISION_PUBLIC_TYPE UWRT_MARS_ROVER_VISION_PUBLIC + +#define UWRT_MARS_ROVER_VISION_LOCAL + +#else + +#define UWRT_MARS_ROVER_VISION_EXPORT __attribute__((visibility("default"))) +#define UWRT_MARS_ROVER_VISION_IMPORT + +#if __GNUC__ >= 4 +#define UWRT_MARS_ROVER_VISION_PUBLIC __attribute__((visibility("default"))) +#define UWRT_MARS_ROVER_VISION_LOCAL __attribute__((visibility("hidden"))) +#else +#define UWRT_MARS_ROVER_VISION_PUBLIC +#define UWRT_MARS_ROVER_VISION_LOCAL +#endif + +#define UWRT_MARS_ROVER_VISION_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // UWRT_MARS_ROVER_VISION__VISIBILITY_H_ \ No newline at end of file diff --git a/uwrt_mars_rover_vision/launch/zed_vision_launch.py b/uwrt_mars_rover_vision/launch/zed_vision_launch.py new file mode 100644 index 00000000..e202e49a --- /dev/null +++ b/uwrt_mars_rover_vision/launch/zed_vision_launch.py @@ -0,0 +1,41 @@ + +# from http.server import executable +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node + +# TODO: change this launch file to make it relevant +# will need to get zed2_node running to get image data upstream here + + +def generate_launch_description(): + node = Node( + package='joy', + namespace="", + executable="joy_node" + ) + + container = ComposableNodeContainer( + name='container', + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="uwrt_mars_rover_xbox_controller", + plugin="uwrt_xbox::UWRTXboxController"), + ] + ) + + return launch.LaunchDescription([node, container]) + + +""" +Process to launch the xbox control component +1. Build the uwrt_mars_rover_xbox_controller package (colcon build) +2. In a new terminal, source the overlay (source ~/ros2_ws/install/setup.bash) +3. Nagivate to the launch folder in the new terminal and run: ros2 launch xbox_launch.py + +-> To see outputted results, see the /xbox_info topic +""" \ No newline at end of file diff --git a/uwrt_mars_rover_vision/package.xml b/uwrt_mars_rover_vision/package.xml new file mode 100644 index 00000000..a3c607bd --- /dev/null +++ b/uwrt_mars_rover_vision/package.xml @@ -0,0 +1,31 @@ + + + + uwrt_mars_rover_vision + 0.0.0 + ROS2 rover vision autonomy package + nico + Apache License 2.0 + + ament_cmake + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + rclcpp + sensor_msgs + rclcpp_components + cv_bridge + image_transport + libopencv-dev + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp new file mode 100644 index 00000000..d8377e6a --- /dev/null +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -0,0 +1,94 @@ + +#include +// ZED includes +#include +#include "cv_bridge/cv_bridge.h" + +namespace uwrt_autonomy { + +TargetTracker::TargetTracker(const rclcpp::NodeOptions &options): Node("target_tracker", options), it_(this->get()) +{ + // camera_sub_ = create_subscription("/zed2/zed_node/left/image_rect_color", 10, std::bind(&TargetTracker::imageCallback, this , std::placeholders::_1)); + camera_sub_ = it.subscribe("/zed2/zed_node/left/image_rect_color", 1, imageCallback); + aruco_pose_pub_ = create_publisher("/aruco_pose", 10); + + // get zed camera params + sl::Camera zed; + sl::CalibrationParameters calibration_params = zed.getCameraInformation().camera_configuration.calibration_parameters; + int param_num = 0; + for (const double &disto_param: calibration_params.left_cam.disto) + { + dist_coefficients_.at(0, param_num) = disto_param; + param_num++; + } + intrinsic_calib_matrix_ = (cv::Mat_(3,3) << + calibration_params.left_cam.fx, 0, calibration_params.left_cam.cx, + 0, calibration_params.left_cam.fy, calibration_params.left_cam.cy, + 0, 0, 1); + + // do aruco setup stuff + obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + + // TODO: potentially play with detector params + params_ = cv::aruco::DetectorParameters::create(); + // TODO: ensure the below predefined dictionary is the correct one + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + + // make sure this line doesnt mess stuff up + zed.close(); +} + + +void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + cv_bridge::CvImagePtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // assume we are receiving a bgra8 encoded image, must reduce to bgr + cv::Mat image_rgb; + cv::cvtColor(cv_ptr->image, image_rgb, cv::COLOR_BGRA2BGR); + + std::vector marker_ids; + std::vector> marker_corners, rejection_candidates; + cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); + int num_markers_detected = marker_corners.size(); + // identify aruco codes + if (num_markers_detected > 0) { + // Calculate pose for each marker + for (int i = 0; i < num_markers_detected; i++) { + cv::solvePnP(obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i)); + } + + // TODO add in functionality for validating detected markers and axes frames (raw c++ code below) + // for(int i = 0; i < marker_ids.size(); i++) + // { + // // draw the detected poses with coordinate axis of 0.2m in length + // cv::drawFrameAxes(image_ocv_c3, intrinsicCalibMatrix, distortionCoefficients, rvecs[i], tvecs[i], 0.2); + // } + // cv::aruco::drawDetectedMarkers(image_ocv_c3, marker_corners, marker_ids); + } + else + { + // std::cout << "No ARUCO code detected" << std::endl; + } + + // transform rvecs and tvecs to quaternions + + // publish message to arcuo pose publisher + // investigate translation and rotation vector to point (translation vector) and quaternion +} + +} + +#include +// RCLCPP_COMPONENTS_REGISTER_NODE(uwrt_autonomy::) From ffb2ba6a198e68681b270c78060d0b6cf1f6539a Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Sun, 22 Jan 2023 13:17:40 -0500 Subject: [PATCH 03/11] fix build errors - still broken camera subscriber --- uwrt_mars_rover_vision/CMakeLists.txt | 85 ++++++---- .../aruco_target_tracker.hpp | 23 ++- uwrt_mars_rover_vision/package.xml | 1 + .../src/aruco_target_tracker.cpp | 157 ++++++++++-------- 4 files changed, 157 insertions(+), 109 deletions(-) diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt index c0e3d663..2d939f23 100644 --- a/uwrt_mars_rover_vision/CMakeLists.txt +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -10,6 +10,43 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() + +# find_package(ZED 3 REQUIRED) +# find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) + + +# # ZED 3 reqs ---------------------------- +# # IF(NOT WIN32) +# # add_definitions(-Wno-format-extra-args) +# # SET(SPECIAL_OS_LIBS "pthread" "X11") +# # ENDIF() + +# include_directories(${CUDA_INCLUDE_DIRS}) +# include_directories(${ZED_INCLUDE_DIRS}) +# # include_directories(${OpenCV_INCLUDE_DIRS}) +# include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +# link_directories(${ZED_LIBRARY_DIR}) +# # link_directories(${OpenCV_LIBRARY_DIRS}) +# link_directories(${CUDA_LIBRARY_DIRS}) + +# FILE(GLOB_RECURSE SRC_FILES src/*.cpp) +# FILE(GLOB_RECURSE HDR_FILES include/*.hpp) + +# ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) +# add_definitions(-std=c++14 -O3) + +# # if (LINK_SHARED_ZED) +# # SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) +# # else() +# # SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) +# # endif() + +# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS} ) +# # ${OpenCV_LIBRARIES}) +# # end zed 3 reqs + + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -18,10 +55,10 @@ find_package(rclcpp_components REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(OpenCV REQUIRED COMPONENTS highgui imgcodecs imgproc videoio) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # add custom messages and services find_package(rosidl_default_generators REQUIRED) -# add ros joy package -find_package(sensor_msgs REQUIRED) # set(msg_files # "msg/XboxController.msg" @@ -41,35 +78,27 @@ include_directories(include) set(node_plugins "") # add the aruco target tracker component -# add_library(xbox_controller SHARED -# src/uwrt_mars_rover_xbox_controller.cpp) -# target_compile_definitions(xbox_controller -# PRIVATE "UWRT_MARS_ROVER_XBOX_CONTROLLER_DLL") -# ament_target_dependencies(xbox_controller -# "rclcpp" -# "rclcpp_components" -# "sensor_msgs") -# # build ROS custom messages -# rosidl_target_interfaces(xbox_controller ${PROJECT_NAME} "rosidl_typesupport_cpp") -# rclcpp_components_register_nodes(xbox_controller "uwrt_xbox::UWRTXboxController") -# set(node_plugins "${node_plugins}uwrt_xbox::UWRTXboxController;$\n") - - - -# if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# # the following line skips the linter which checks for copyrights -# # uncomment the line when a copyright and license is not present in all source files -# #set(ament_cmake_copyright_FOUND TRUE) -# # the following line skips cpplint (only works in a git repo) -# # uncomment the line when this package is not in a git repo -# #set(ament_cmake_cpplint_FOUND TRUE) -# ament_lint_auto_find_test_dependencies() -# endif() +add_library(aruco_tracker SHARED + src/aruco_target_tracker.cpp) +target_compile_definitions(aruco_tracker + PRIVATE "UWRT_MARS_ROVER_VISION_DLL") +ament_target_dependencies(aruco_tracker + "rclcpp" + "rclcpp_components" + "sensor_msgs" + "cv_bridge" + "image_transport" + "geometry_msgs" + # "opencv_" + ) +# build ROS custom messages -> uncomment later +# rosidl_target_interfaces(aruco_tracker ${PROJECT_NAME} "rosidl_typesupport_cpp") +# rclcpp_components_register_nodes(aruco_tracker "uwrt_autonomy::TargetTracker") +# set(node_plugins "${node_plugins}uwrt_autonomy::TargetTracker;$\n") # tell where to put binaries install(TARGETS -# xbox_controller + aruco_tracker ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp index 85f3f37b..a6ad4b0c 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -3,8 +3,9 @@ #include #include -#include -#include +#include +#include +#include #include "image_transport/image_transport.hpp" #include "opencv2/highgui/highgui.hpp" @@ -17,42 +18,48 @@ namespace uwrt_autonomy { -class TargetTracker { +class TargetTracker: public rclcpp::Node { public: TargetTracker(const rclcpp::NodeOptions &options); private: + // create the node for image transfer to use + // rclcpp::Node::SharedPtr node_; // use image transport for camera subscriber - image_transport::ImageTransport it_; + // image_transport::ImageTransport it_; // zed2 image topic subscriber image_transport::Subscriber camera_sub_; // camera subsciber image callback void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); // pose publisher for aruco tags - rclcpp::Publisher::SharedPtr aruco_pose_pub_; + rclcpp::Publisher::SharedPtr aruco_pose_pub_; + + // make subscriber to get the camera info + rclcpp::Subscription::SharedPtr camera_info_sub_; + void camInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info); // zed calibration stuff cv::Mat dist_coefficients_; cv::Mat intrinsic_calib_matrix_; // vectors for ARUCO tag poses (max of 4 aruco codes identified at a time) - std::vector rvecs_(4, 0.0), t_vecs_(4, 0.0); + std::vector rvecs_{std::vector(4, 0.0)}, tvecs_{std::vector(4, 0.0)}; // aruco detector things cv::Ptr params_; cv::Ptr dictionary_; // location of corners in object's reference frame // NOTE: below assumes fixed size for aruco tags - cv::Mat obj_points_(4, 1, CV_32FC3); + cv::Mat obj_points_{cv::Mat(4, 1, CV_32FC3)}; // marker length in meters for ARUCO code. Be sure to change it if the marker length changes // TODO: change this into a configurable parameter float ARUCO_MARKER_LEN = 18.4 / 100; -} +}; } diff --git a/uwrt_mars_rover_vision/package.xml b/uwrt_mars_rover_vision/package.xml index a3c607bd..df4ec181 100644 --- a/uwrt_mars_rover_vision/package.xml +++ b/uwrt_mars_rover_vision/package.xml @@ -21,6 +21,7 @@ cv_bridge image_transport libopencv-dev + geometry_msgs ament_lint_auto ament_lint_common diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp index d8377e6a..d509f897 100644 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -1,94 +1,105 @@ #include // ZED includes -#include +// #include #include "cv_bridge/cv_bridge.h" namespace uwrt_autonomy { -TargetTracker::TargetTracker(const rclcpp::NodeOptions &options): Node("target_tracker", options), it_(this->get()) +TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_tracker", options) { - // camera_sub_ = create_subscription("/zed2/zed_node/left/image_rect_color", 10, std::bind(&TargetTracker::imageCallback, this , std::placeholders::_1)); - camera_sub_ = it.subscribe("/zed2/zed_node/left/image_rect_color", 1, imageCallback); - aruco_pose_pub_ = create_publisher("/aruco_pose", 10); - - // get zed camera params - sl::Camera zed; - sl::CalibrationParameters calibration_params = zed.getCameraInformation().camera_configuration.calibration_parameters; - int param_num = 0; - for (const double &disto_param: calibration_params.left_cam.disto) - { - dist_coefficients_.at(0, param_num) = disto_param; - param_num++; - } - intrinsic_calib_matrix_ = (cv::Mat_(3,3) << - calibration_params.left_cam.fx, 0, calibration_params.left_cam.cx, - 0, calibration_params.left_cam.fy, calibration_params.left_cam.cy, - 0, 0, 1); - - // do aruco setup stuff - obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); - - // TODO: potentially play with detector params - params_ = cv::aruco::DetectorParameters::create(); - // TODO: ensure the below predefined dictionary is the correct one - dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - - // make sure this line doesnt mess stuff up - zed.close(); -} + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + camera_sub_ = image_transport::create_subscription( + this, "/zed2/zed_node/left/image_rect_color", + std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); + + // camera_sub_ = create_subscription("/zed2/zed_node/left/image_rect_color", 10, + // std::bind(&TargetTracker::imageCallback, this , std::placeholders::_1)); + + aruco_pose_pub_ = create_publisher("/aruco_pose", 10); + camera_info_sub_ = create_subscription( + "/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + + // get zed camera params + // sl::Camera zed; + // sl::CalibrationParameters calibration_params = + // zed.getCameraInformation().camera_configuration.calibration_parameters; int param_num = 0; for (const double + // &disto_param: calibration_params.left_cam.disto) + // { + // dist_coefficients_.at(0, param_num) = disto_param; + // param_num++; + // } + // intrinsic_calib_matrix_ = (cv::Mat_(3,3) << + // calibration_params.left_cam.fx, 0, calibration_params.left_cam.cx, + // 0, calibration_params.left_cam.fy, calibration_params.left_cam.cy, + // 0, 0, 1); + // // do aruco setup stuff + // obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + // obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + // obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + // obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + + // // TODO: potentially play with detector params + // params_ = cv::aruco::DetectorParameters::create(); + // // TODO: ensure the below predefined dictionary is the correct one + // dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + + // // make sure this line doesnt mess stuff up + // zed.close(); +} void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - cv_bridge::CvImagePtr cv_ptr; - try - { - cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); - } - catch (cv_bridge::Exception& e) - { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; - } + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // assume we are receiving a bgra8 encoded image, must reduce to bgr + cv::Mat image_rgb; + cv::cvtColor(cv_ptr->image, image_rgb, cv::COLOR_BGRA2BGR); - // assume we are receiving a bgra8 encoded image, must reduce to bgr - cv::Mat image_rgb; - cv::cvtColor(cv_ptr->image, image_rgb, cv::COLOR_BGRA2BGR); - - std::vector marker_ids; - std::vector> marker_corners, rejection_candidates; - cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); - int num_markers_detected = marker_corners.size(); - // identify aruco codes - if (num_markers_detected > 0) { - // Calculate pose for each marker - for (int i = 0; i < num_markers_detected; i++) { - cv::solvePnP(obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i)); - } - - // TODO add in functionality for validating detected markers and axes frames (raw c++ code below) - // for(int i = 0; i < marker_ids.size(); i++) - // { - // // draw the detected poses with coordinate axis of 0.2m in length - // cv::drawFrameAxes(image_ocv_c3, intrinsicCalibMatrix, distortionCoefficients, rvecs[i], tvecs[i], 0.2); - // } - // cv::aruco::drawDetectedMarkers(image_ocv_c3, marker_corners, marker_ids); - } - else - { - // std::cout << "No ARUCO code detected" << std::endl; + std::vector marker_ids; + std::vector> marker_corners, rejection_candidates; + cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); + int num_markers_detected = marker_corners.size(); + // identify aruco codes + if (num_markers_detected > 0) { + // Calculate pose for each marker + for (int i = 0; i < num_markers_detected; i++) { + cv::solvePnP(obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), + tvecs_.at(i)); } - // transform rvecs and tvecs to quaternions + // log the points just to check that stuff is making sense + RCLCPP_INFO_STREAM(this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); - // publish message to arcuo pose publisher - // investigate translation and rotation vector to point (translation vector) and quaternion + // TODO add in functionality for validating detected markers and axes frames (raw c++ code below) + // for(int i = 0; i < marker_ids.size(); i++) + // { + // // draw the detected poses with coordinate axis of 0.2m in length + // cv::drawFrameAxes(image_ocv_c3, intrinsicCalibMatrix, distortionCoefficients, rvecs[i], tvecs[i], 0.2); + // } + // cv::aruco::drawDetectedMarkers(image_ocv_c3, marker_corners, marker_ids); + } else { + // std::cout << "No ARUCO code detected" << std::endl; + } + + // transform rvecs and tvecs to quaternions + + // publish message to arcuo pose publisher + // investigate translation and rotation vector to point (translation vector) and quaternion } +void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info) { + (void)info; + // RCLCPP_INFO_STREAM(this->get_logger(), "Cam Info: " << info->D << " | " << info->K); } +} // namespace uwrt_autonomy + #include // RCLCPP_COMPONENTS_REGISTER_NODE(uwrt_autonomy::) From c7e7e0bb70a072f873ac9f11a86bf00a70626e36 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 23 Jan 2023 22:47:04 -0500 Subject: [PATCH 04/11] fix compilation issues --- uwrt_mars_rover_vision/CMakeLists.txt | 6 +- .../aruco_target_tracker.hpp | 3 +- .../uwrt_mars_rover_vision/visibility.h | 2 +- .../src/aruco_target_tracker.cpp | 59 +++++++++---------- 4 files changed, 34 insertions(+), 36 deletions(-) diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt index 2d939f23..0f0151f1 100644 --- a/uwrt_mars_rover_vision/CMakeLists.txt +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -69,7 +69,7 @@ find_package(rosidl_default_generators REQUIRED) # DEPENDENCIES std_msgs # ) -# ament_export_dependencies(rosidl_default_runtime) +ament_export_dependencies(rosidl_default_runtime) #include the 'include' directory include_directories(include) @@ -93,8 +93,8 @@ ament_target_dependencies(aruco_tracker ) # build ROS custom messages -> uncomment later # rosidl_target_interfaces(aruco_tracker ${PROJECT_NAME} "rosidl_typesupport_cpp") -# rclcpp_components_register_nodes(aruco_tracker "uwrt_autonomy::TargetTracker") -# set(node_plugins "${node_plugins}uwrt_autonomy::TargetTracker;$\n") +rclcpp_components_register_nodes(aruco_tracker "uwrt_autonomy::TargetTracker") +set(node_plugins "${node_plugins}uwrt_autonomy::TargetTracker;$\n") # tell where to put binaries install(TARGETS diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp index a6ad4b0c..b7bfd056 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -38,7 +38,8 @@ class TargetTracker: public rclcpp::Node { // make subscriber to get the camera info rclcpp::Subscription::SharedPtr camera_info_sub_; - void camInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info); + UWRT_MARS_ROVER_VISION_LOCAL + void camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info); // zed calibration stuff cv::Mat dist_coefficients_; diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h index 7d23baf5..37b5c42f 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h @@ -7,7 +7,7 @@ // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, +// distributed unUWRT_MARS_ROVER_VISION_LOCALder the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp index d509f897..89df876e 100644 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -17,36 +17,22 @@ TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_ // std::bind(&TargetTracker::imageCallback, this , std::placeholders::_1)); aruco_pose_pub_ = create_publisher("/aruco_pose", 10); + // todo: figure out how to only call subscription once to not constantly read these messages camera_info_sub_ = create_subscription( "/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + + + // do aruco setup stuff + obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + + // TODO: potentially play with detector params + params_ = cv::aruco::DetectorParameters::create(); + // TODO: ensure the below predefined dictionary is the correct one + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - // get zed camera params - // sl::Camera zed; - // sl::CalibrationParameters calibration_params = - // zed.getCameraInformation().camera_configuration.calibration_parameters; int param_num = 0; for (const double - // &disto_param: calibration_params.left_cam.disto) - // { - // dist_coefficients_.at(0, param_num) = disto_param; - // param_num++; - // } - // intrinsic_calib_matrix_ = (cv::Mat_(3,3) << - // calibration_params.left_cam.fx, 0, calibration_params.left_cam.cx, - // 0, calibration_params.left_cam.fy, calibration_params.left_cam.cy, - // 0, 0, 1); - - // // do aruco setup stuff - // obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - // obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - // obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); - // obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); - - // // TODO: potentially play with detector params - // params_ = cv::aruco::DetectorParameters::create(); - // // TODO: ensure the below predefined dictionary is the correct one - // dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - - // // make sure this line doesnt mess stuff up - // zed.close(); } void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { @@ -94,12 +80,23 @@ void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& // investigate translation and rotation vector to point (translation vector) and quaternion } -void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info) { - (void)info; - // RCLCPP_INFO_STREAM(this->get_logger(), "Cam Info: " << info->D << " | " << info->K); +void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info) +{ + // (void)info; + int param_num = 0; + for (const double &disto_param: info->d) + { + dist_coefficients_.at(0, param_num) = disto_param; + param_num++; + } + intrinsic_calib_matrix_ = (cv::Mat_(3,3) << + info->k[0], 0, info->k[2], + 0, info->k[4], info->k[5], + 0, 0, 1); + RCLCPP_DEBUG_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); } } // namespace uwrt_autonomy #include -// RCLCPP_COMPONENTS_REGISTER_NODE(uwrt_autonomy::) +RCLCPP_COMPONENTS_REGISTER_NODE(uwrt_autonomy::TargetTracker) From 1a74b5f8ed064ea67773cf25cd6da715c8962b9a Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Sun, 29 Jan 2023 14:23:21 -0500 Subject: [PATCH 05/11] complete working version of aruco pose publisher --- .../aruco_target_tracker.hpp | 16 ++-- .../src/aruco_target_tracker.cpp | 86 +++++++++++-------- 2 files changed, 58 insertions(+), 44 deletions(-) diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp index b7bfd056..c667c7db 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include #include "image_transport/image_transport.hpp" @@ -24,25 +24,23 @@ class TargetTracker: public rclcpp::Node { TargetTracker(const rclcpp::NodeOptions &options); private: - // create the node for image transfer to use - // rclcpp::Node::SharedPtr node_; - // use image transport for camera subscriber - // image_transport::ImageTransport it_; // zed2 image topic subscriber image_transport::Subscriber camera_sub_; // camera subsciber image callback void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); // pose publisher for aruco tags - rclcpp::Publisher::SharedPtr aruco_pose_pub_; + rclcpp::Publisher::SharedPtr aruco_pose_pub_; // make subscriber to get the camera info rclcpp::Subscription::SharedPtr camera_info_sub_; - UWRT_MARS_ROVER_VISION_LOCAL void camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info); + // convert translation and rotation vector to pose message + void toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg); + // zed calibration stuff - cv::Mat dist_coefficients_; + cv::Mat dist_coefficients_ = cv::Mat::zeros(1, 5, CV_32FC1); cv::Mat intrinsic_calib_matrix_; // vectors for ARUCO tag poses (max of 4 aruco codes identified at a time) @@ -58,8 +56,6 @@ class TargetTracker: public rclcpp::Node { // marker length in meters for ARUCO code. Be sure to change it if the marker length changes // TODO: change this into a configurable parameter float ARUCO_MARKER_LEN = 18.4 / 100; - - }; } diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp index 89df876e..1d3f582e 100644 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -1,27 +1,29 @@ #include -// ZED includes -// #include +#include #include "cv_bridge/cv_bridge.h" +#include +#include +#include +#include + namespace uwrt_autonomy { TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_tracker", options) { + // todo: figure out how to only call subscription once to not constantly read these messages + // maybe link the camera info and camera subscription messages together? Only do if necessary + camera_info_sub_ = create_subscription( + "/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; camera_sub_ = image_transport::create_subscription( this, "/zed2/zed_node/left/image_rect_color", std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); - - // camera_sub_ = create_subscription("/zed2/zed_node/left/image_rect_color", 10, - // std::bind(&TargetTracker::imageCallback, this , std::placeholders::_1)); - aruco_pose_pub_ = create_publisher("/aruco_pose", 10); - // todo: figure out how to only call subscription once to not constantly read these messages - camera_info_sub_ = create_subscription( - "/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + aruco_pose_pub_ = create_publisher("/aruco_pose", 10); - // do aruco setup stuff obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); @@ -32,10 +34,24 @@ TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_ params_ = cv::aruco::DetectorParameters::create(); // TODO: ensure the below predefined dictionary is the correct one dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); +} +void TargetTracker::toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg) +{ + // convert the compressed rodrigues rotation vector into a rotation matrix + cv::Mat rot_matrix(3, 3, CV_64FC1); + cv::Rodrigues(rvec, rot_matrix); + tf2::Matrix3x3 tf2_rot_matrix{rot_matrix.at(0, 0), rot_matrix.at(0, 1), rot_matrix.at(0, 2), + rot_matrix.at(1, 0), rot_matrix.at(1, 1), rot_matrix.at(1, 2), + rot_matrix.at(2, 0), rot_matrix.at(2, 1), rot_matrix.at(2, 2)}; + tf2::Vector3 tf2_tvec{ tvec[0], tvec[1], tvec[2] }; + // Create a transform and convert to a Pose + tf2::Transform tf2_transform {tf2_rot_matrix, tf2_tvec }; + tf2::toMsg(tf2_transform, pose_msg); } -void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { +void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) +{ cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); @@ -43,46 +59,48 @@ void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } - - // assume we are receiving a bgra8 encoded image, must reduce to bgr + // receive a bgra8 encoded image, must convert to bgr cv::Mat image_rgb; cv::cvtColor(cv_ptr->image, image_rgb, cv::COLOR_BGRA2BGR); - std::vector marker_ids; std::vector> marker_corners, rejection_candidates; cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); int num_markers_detected = marker_corners.size(); // identify aruco codes - if (num_markers_detected > 0) { + if (num_markers_detected > 0) + { // Calculate pose for each marker for (int i = 0; i < num_markers_detected; i++) { cv::solvePnP(obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i)); + // send a pose message stamped of the aruco tag pose + geometry_msgs::msg::PoseStamped aruco_pose_msg; + aruco_pose_msg.header.frame_id = std::to_string(marker_ids[i]); + toPoseMsg(rvecs_[i], tvecs_[i], aruco_pose_msg.pose); + aruco_pose_pub_->publish(aruco_pose_msg); + // draw the detected poses with coordinate axis of 0.2m in length + cv::drawFrameAxes(image_rgb, intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i), 0.2); } // log the points just to check that stuff is making sense - RCLCPP_INFO_STREAM(this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); - - // TODO add in functionality for validating detected markers and axes frames (raw c++ code below) - // for(int i = 0; i < marker_ids.size(); i++) - // { - // // draw the detected poses with coordinate axis of 0.2m in length - // cv::drawFrameAxes(image_ocv_c3, intrinsicCalibMatrix, distortionCoefficients, rvecs[i], tvecs[i], 0.2); - // } - // cv::aruco::drawDetectedMarkers(image_ocv_c3, marker_corners, marker_ids); - } else { - // std::cout << "No ARUCO code detected" << std::endl; - } - - // transform rvecs and tvecs to quaternions - - // publish message to arcuo pose publisher - // investigate translation and rotation vector to point (translation vector) and quaternion + RCLCPP_DEBUG_STREAM(this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); + cv::aruco::drawDetectedMarkers(image_rgb, marker_corners, marker_ids); + } + + // TODO: make optional to show the position on an aruco tag + cv::imshow("Image window", image_rgb); + // handle key event -> make cv2 show a new image on the window every 5ms + cv::waitKey(5); } void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info) { - // (void)info; + // temporary hacky work around to only read cam info once + if (intrinsic_calib_matrix_.size().height == 0) + { + RCLCPP_INFO_STREAM(this->get_logger(), info->distortion_model); + RCLCPP_INFO_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); + (void)info; int param_num = 0; for (const double &disto_param: info->d) { @@ -93,7 +111,7 @@ void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPt info->k[0], 0, info->k[2], 0, info->k[4], info->k[5], 0, 0, 1); - RCLCPP_DEBUG_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); + } } } // namespace uwrt_autonomy From 9700523311391e96d1063a1a0a898177ad133880 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Wed, 1 Feb 2023 00:13:37 -0500 Subject: [PATCH 06/11] complete aruco target tracker --- uwrt_mars_rover_vision/CMakeLists.txt | 60 ++----------- .../aruco_target_tracker.hpp | 54 ++++++------ .../launch/zed_vision.launch.py | 48 +++++++++++ .../launch/zed_vision_launch.py | 41 --------- .../src/aruco_target_tracker.cpp | 86 ++++++++++++------- 5 files changed, 142 insertions(+), 147 deletions(-) create mode 100644 uwrt_mars_rover_vision/launch/zed_vision.launch.py delete mode 100644 uwrt_mars_rover_vision/launch/zed_vision_launch.py diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt index 0f0151f1..93d35216 100644 --- a/uwrt_mars_rover_vision/CMakeLists.txt +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -10,43 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - -# find_package(ZED 3 REQUIRED) -# find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) - - -# # ZED 3 reqs ---------------------------- -# # IF(NOT WIN32) -# # add_definitions(-Wno-format-extra-args) -# # SET(SPECIAL_OS_LIBS "pthread" "X11") -# # ENDIF() - -# include_directories(${CUDA_INCLUDE_DIRS}) -# include_directories(${ZED_INCLUDE_DIRS}) -# # include_directories(${OpenCV_INCLUDE_DIRS}) -# include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) - -# link_directories(${ZED_LIBRARY_DIR}) -# # link_directories(${OpenCV_LIBRARY_DIRS}) -# link_directories(${CUDA_LIBRARY_DIRS}) - -# FILE(GLOB_RECURSE SRC_FILES src/*.cpp) -# FILE(GLOB_RECURSE HDR_FILES include/*.hpp) - -# ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -# add_definitions(-std=c++14 -O3) - -# # if (LINK_SHARED_ZED) -# # SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) -# # else() -# # SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) -# # endif() - -# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS} ) -# # ${OpenCV_LIBRARIES}) -# # end zed 3 reqs - - # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -54,20 +17,9 @@ find_package(std_msgs REQUIRED) find_package(rclcpp_components REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) -find_package(OpenCV REQUIRED COMPONENTS highgui imgcodecs imgproc videoio) +find_package(OpenCV REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -# add custom messages and services -find_package(rosidl_default_generators REQUIRED) - -# set(msg_files -# "msg/XboxController.msg" -# ) - -# rosidl_generate_interfaces(${PROJECT_NAME} -# ${msg_files} -# DEPENDENCIES std_msgs -# ) ament_export_dependencies(rosidl_default_runtime) @@ -89,10 +41,8 @@ ament_target_dependencies(aruco_tracker "cv_bridge" "image_transport" "geometry_msgs" - # "opencv_" + "OpenCV" ) -# build ROS custom messages -> uncomment later -# rosidl_target_interfaces(aruco_tracker ${PROJECT_NAME} "rosidl_typesupport_cpp") rclcpp_components_register_nodes(aruco_tracker "uwrt_autonomy::TargetTracker") set(node_plugins "${node_plugins}uwrt_autonomy::TargetTracker;$\n") @@ -103,4 +53,10 @@ install(TARGETS LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_package() diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp index c667c7db..0d4fa5f4 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -6,16 +6,11 @@ #include #include #include -#include "image_transport/image_transport.hpp" -#include "opencv2/highgui/highgui.hpp" - +#include +#include #include -#include "opencv2/imgproc.hpp" -#include - #include - namespace uwrt_autonomy { class TargetTracker: public rclcpp::Node { @@ -24,38 +19,49 @@ class TargetTracker: public rclcpp::Node { TargetTracker(const rclcpp::NodeOptions &options); private: - // zed2 image topic subscriber - image_transport::Subscriber camera_sub_; - // camera subsciber image callback - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg); - - // pose publisher for aruco tags - rclcpp::Publisher::SharedPtr aruco_pose_pub_; + /** + * @brief Reads images from a zed camera subscriber topic and publishes poses for detected ARUCO tags + * @param image_msg Image data and metadata from the zed2 camera + */ + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg); - // make subscriber to get the camera info - rclcpp::Subscription::SharedPtr camera_info_sub_; + /** + * @brief Reads intrinstic calibration data from the camera info topic + * @param info Camera info message + */ void camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info); - // convert translation and rotation vector to pose message + /** + * @brief Converts a rotation vector and translation vector to pose message + * @param rvec Compressed rodrigues rotation vector (angle-axis form) which represents a rotation + * @param tvec Translation vector in 3D + * @param pose_msg Reference to a pose message + */ void toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg); + // pose publisher for aruco tags + rclcpp::Publisher::SharedPtr aruco_pose_pub_; + // zed2 image topic subscriber + image_transport::Subscriber camera_sub_; + // camera info subscriber to get intrinstic calibration params + rclcpp::Subscription::SharedPtr camera_info_sub_; + // zed calibration stuff cv::Mat dist_coefficients_ = cv::Mat::zeros(1, 5, CV_32FC1); cv::Mat intrinsic_calib_matrix_; - // vectors for ARUCO tag poses (max of 4 aruco codes identified at a time) + // vectors for ARUCO tag rotations and translations (max of 4 aruco codes identified at a time) std::vector rvecs_{std::vector(4, 0.0)}, tvecs_{std::vector(4, 0.0)}; - - // aruco detector things + // opencv aruco detector variables cv::Ptr params_; cv::Ptr dictionary_; // location of corners in object's reference frame - // NOTE: below assumes fixed size for aruco tags cv::Mat obj_points_{cv::Mat(4, 1, CV_32FC3)}; - // marker length in meters for ARUCO code. Be sure to change it if the marker length changes - // TODO: change this into a configurable parameter - float ARUCO_MARKER_LEN = 18.4 / 100; + // marker length (both length and width are the same) in meters for an ARUCO marker + float aruco_marker_len_; + // whether or not to show a visual output of the codes + bool display_marker_pose_; }; } diff --git a/uwrt_mars_rover_vision/launch/zed_vision.launch.py b/uwrt_mars_rover_vision/launch/zed_vision.launch.py new file mode 100644 index 00000000..c31ad796 --- /dev/null +++ b/uwrt_mars_rover_vision/launch/zed_vision.launch.py @@ -0,0 +1,48 @@ +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + +def generate_launch_description(): + zed_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('zed_wrapper'), + 'launch/zed2.launch.py' + ]) + ]) + ) + + container = ComposableNodeContainer( + name='container', + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="uwrt_mars_rover_vision", + plugin="uwrt_autonomy::TargetTracker", + parameters=[ + # marker dimension in meters + {"aruco_marker_len": 0.184 }, + # aruco dictionary following the urc guidlines (boolean) + {"aruco_dict": "4x4_50"}, + # whether or not to show marker poses on the screen + {"display_marker_pose": True} + ] + ) + ] + ) + + return LaunchDescription([zed_launch, container]) + + +""" +Process to launch the target tracker component +1. Build the uwrt_mars_rover_vision package and zed_wrapper (colcon build) +2. In a new terminal, source the overlay (source ~/ros2_ws/install/setup.bash) +3. Nagivate to the launch folder in the new terminal and run: ros2 launch uwrt_mars_rover_vision zed_vision.launch.py +""" \ No newline at end of file diff --git a/uwrt_mars_rover_vision/launch/zed_vision_launch.py b/uwrt_mars_rover_vision/launch/zed_vision_launch.py deleted file mode 100644 index e202e49a..00000000 --- a/uwrt_mars_rover_vision/launch/zed_vision_launch.py +++ /dev/null @@ -1,41 +0,0 @@ - -# from http.server import executable -import launch -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.actions import Node - -# TODO: change this launch file to make it relevant -# will need to get zed2_node running to get image data upstream here - - -def generate_launch_description(): - node = Node( - package='joy', - namespace="", - executable="joy_node" - ) - - container = ComposableNodeContainer( - name='container', - namespace="", - package="rclcpp_components", - executable="component_container", - composable_node_descriptions=[ - ComposableNode( - package="uwrt_mars_rover_xbox_controller", - plugin="uwrt_xbox::UWRTXboxController"), - ] - ) - - return launch.LaunchDescription([node, container]) - - -""" -Process to launch the xbox control component -1. Build the uwrt_mars_rover_xbox_controller package (colcon build) -2. In a new terminal, source the overlay (source ~/ros2_ws/install/setup.bash) -3. Nagivate to the launch folder in the new terminal and run: ros2 launch xbox_launch.py - --> To see outputted results, see the /xbox_info topic -""" \ No newline at end of file diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp index 1d3f582e..5bdd8673 100644 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -12,28 +12,47 @@ namespace uwrt_autonomy { TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_tracker", options) { - // todo: figure out how to only call subscription once to not constantly read these messages - // maybe link the camera info and camera subscription messages together? Only do if necessary - camera_info_sub_ = create_subscription( - "/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + // get parameters set in the launch file + this->declare_parameter("aruco_marker_len", 0.2); + this->declare_parameter("aruco_dict", ""); + this->declare_parameter("display_marker_pose", true); + aruco_marker_len_ = this->get_parameter("aruco_marker_len").get_parameter_value().get(); + display_marker_pose_ = this->get_parameter("display_marker_pose").get_parameter_value().get(); + std::string aruco_dict = this->get_parameter("aruco_dict").get_parameter_value().get(); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Marker Len: " << aruco_marker_len_); + + camera_info_sub_ = create_subscription("/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); rmw_qos_profile_t custom_qos = rmw_qos_profile_default; - camera_sub_ = image_transport::create_subscription( - this, "/zed2/zed_node/left/image_rect_color", - std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); + camera_sub_ = image_transport::create_subscription(this, "/zed2/zed_node/left/image_rect_color", std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); aruco_pose_pub_ = create_publisher("/aruco_pose", 10); // do aruco setup stuff - obj_points_.ptr(0)[0] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[1] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[2] = cv::Vec3f(ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); - obj_points_.ptr(0)[3] = cv::Vec3f(-ARUCO_MARKER_LEN/2.f, -ARUCO_MARKER_LEN/2.f, 0); + obj_points_.ptr(0)[0] = cv::Vec3f(-aruco_marker_len_/2.f, aruco_marker_len_/2.f, 0); + obj_points_.ptr(0)[1] = cv::Vec3f(aruco_marker_len_/2.f, aruco_marker_len_/2.f, 0); + obj_points_.ptr(0)[2] = cv::Vec3f(aruco_marker_len_/2.f, -aruco_marker_len_/2.f, 0); + obj_points_.ptr(0)[3] = cv::Vec3f(-aruco_marker_len_/2.f, -aruco_marker_len_/2.f, 0); - // TODO: potentially play with detector params params_ = cv::aruco::DetectorParameters::create(); - // TODO: ensure the below predefined dictionary is the correct one - dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + if (aruco_dict == "") + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Please set an ARUCO dictionary to use"); + } + else if (aruco_dict == "4x4_50") + { + // use this dictionary for URC + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + } + else if (aruco_dict == "4x4_100") + { + // use this dictionary for CIRC + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); + } + else + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Please choose a defined ARUCO dictionary"); + } } void TargetTracker::toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg) @@ -50,11 +69,11 @@ void TargetTracker::toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg tf2::toMsg(tf2_transform, pose_msg); } -void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) +void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg) { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + cv_ptr = cv_bridge::toCvCopy(image_msg, image_msg->encoding); } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; @@ -62,11 +81,14 @@ void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& // receive a bgra8 encoded image, must convert to bgr cv::Mat image_rgb; cv::cvtColor(cv_ptr->image, image_rgb, cv::COLOR_BGRA2BGR); + + // detect ARUCO markers in the image std::vector marker_ids; std::vector> marker_corners, rejection_candidates; cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); int num_markers_detected = marker_corners.size(); - // identify aruco codes + + // identify visible aruco marker poses if (num_markers_detected > 0) { // Calculate pose for each marker @@ -78,29 +100,33 @@ void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& aruco_pose_msg.header.frame_id = std::to_string(marker_ids[i]); toPoseMsg(rvecs_[i], tvecs_[i], aruco_pose_msg.pose); aruco_pose_pub_->publish(aruco_pose_msg); - // draw the detected poses with coordinate axis of 0.2m in length - cv::drawFrameAxes(image_rgb, intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i), 0.2); + if (display_marker_pose_) + { + // draw the detected poses with coordinate axis of 0.2m in length + cv::drawFrameAxes(image_rgb, intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i), 0.2); + } } - - // log the points just to check that stuff is making sense RCLCPP_DEBUG_STREAM(this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); - cv::aruco::drawDetectedMarkers(image_rgb, marker_corners, marker_ids); + if (display_marker_pose_) + { + cv::aruco::drawDetectedMarkers(image_rgb, marker_corners, marker_ids); + } } - // TODO: make optional to show the position on an aruco tag - cv::imshow("Image window", image_rgb); - // handle key event -> make cv2 show a new image on the window every 5ms - cv::waitKey(5); + if (display_marker_pose_) + { + cv::imshow("Image window", image_rgb); + cv::waitKey(5); + } } void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info) { - // temporary hacky work around to only read cam info once + // hacky work around to only read cam info once if (intrinsic_calib_matrix_.size().height == 0) { - RCLCPP_INFO_STREAM(this->get_logger(), info->distortion_model); - RCLCPP_INFO_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); - (void)info; + RCLCPP_DEBUG_STREAM(this->get_logger(), info->distortion_model); + RCLCPP_DEBUG_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); int param_num = 0; for (const double &disto_param: info->d) { From 0e6a1ebbdc347d5541a92f998ac6ac2af1494f40 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 6 Feb 2023 21:25:15 -0500 Subject: [PATCH 07/11] fix formatting and add test dependencies --- uwrt_mars_rover_vision/CMakeLists.txt | 27 ++++ .../aruco_target_tracker.hpp | 78 +++++------ .../uwrt_mars_rover_vision/visibility.h | 2 +- uwrt_mars_rover_vision/package.xml | 5 + .../src/aruco_target_tracker.cpp | 128 +++++++++--------- 5 files changed, 138 insertions(+), 102 deletions(-) diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt index 93d35216..7e59a791 100644 --- a/uwrt_mars_rover_vision/CMakeLists.txt +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -59,4 +59,31 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +if (BUILD_TESTING) + # Force generation of compile_commands.json for clang-tidy + set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "") + # clang-format + find_package(ament_cmake_clang_format REQUIRED) + ament_clang_format( + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-format + ) + # clang-tidy + find_package(ament_cmake_clang_tidy REQUIRED) + ament_clang_tidy( + ${CMAKE_BINARY_DIR} + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.clang-tidy + ) + # cppcheck + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + # flake8 + find_package(ament_cmake_flake8 REQUIRED) + ament_flake8( + CONFIG_FILE ${CMAKE_SOURCE_DIR}/../../.flake8 + ) + # xmllint + find_package(ament_cmake_xmllint REQUIRED) + ament_xmllint() +endif () + ament_package() diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp index 0d4fa5f4..22f31635 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -1,69 +1,71 @@ #ifndef ARUCO_TARGET_TRACKER_H #define ARUCO_TARGET_TRACKER_H -#include #include + #include -#include -#include #include -#include #include +#include +#include +#include +#include #include -namespace uwrt_autonomy { - -class TargetTracker: public rclcpp::Node { - +namespace uwrt_autonomy +{ +class TargetTracker : public rclcpp::Node +{ public: - TargetTracker(const rclcpp::NodeOptions &options); + TargetTracker(const rclcpp::NodeOptions & options); private: - /** + /** * @brief Reads images from a zed camera subscriber topic and publishes poses for detected ARUCO tags * @param image_msg Image data and metadata from the zed2 camera */ - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg); + void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg); - /** + /** * @brief Reads intrinstic calibration data from the camera info topic * @param info Camera info message */ - void camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info); + void camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info); - /** + /** * @brief Converts a rotation vector and translation vector to pose message * @param rvec Compressed rodrigues rotation vector (angle-axis form) which represents a rotation * @param tvec Translation vector in 3D * @param pose_msg Reference to a pose message */ - void toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg); + void toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose & pose_msg); - // pose publisher for aruco tags - rclcpp::Publisher::SharedPtr aruco_pose_pub_; - // zed2 image topic subscriber - image_transport::Subscriber camera_sub_; - // camera info subscriber to get intrinstic calibration params - rclcpp::Subscription::SharedPtr camera_info_sub_; + // pose publisher for aruco tags + rclcpp::Publisher::SharedPtr aruco_pose_pub_; + // zed2 image topic subscriber + image_transport::Subscriber camera_sub_; + // camera info subscriber to get intrinstic calibration params + rclcpp::Subscription::SharedPtr camera_info_sub_; - // zed calibration stuff - cv::Mat dist_coefficients_ = cv::Mat::zeros(1, 5, CV_32FC1); - cv::Mat intrinsic_calib_matrix_; + // zed calibration stuff + cv::Mat dist_coefficients_ = cv::Mat::zeros(1, 5, CV_32FC1); + cv::Mat intrinsic_calib_matrix_; - // vectors for ARUCO tag rotations and translations (max of 4 aruco codes identified at a time) - std::vector rvecs_{std::vector(4, 0.0)}, tvecs_{std::vector(4, 0.0)}; - // opencv aruco detector variables - cv::Ptr params_; - cv::Ptr dictionary_; - // location of corners in object's reference frame - cv::Mat obj_points_{cv::Mat(4, 1, CV_32FC3)}; + // vectors for ARUCO tag rotations and translations (max of 4 aruco codes identified at a time) + std::vector rvecs_{std::vector(4, 0.0)}, + tvecs_{std::vector(4, 0.0)}; + // opencv aruco detector variables + cv::Ptr params_; + cv::Ptr dictionary_; + // location of corners in object's reference frame + cv::Mat obj_points_{cv::Mat(4, 1, CV_32FC3)}; - // marker length (both length and width are the same) in meters for an ARUCO marker - float aruco_marker_len_; - // whether or not to show a visual output of the codes - bool display_marker_pose_; + // marker length (both length and width are the same) in meters for an ARUCO marker + float aruco_marker_len_; + // whether or not to show a visual output of the codes + bool display_marker_pose_; }; - -} -#endif // ARUCO_TARGET_TRACKER_H \ No newline at end of file +} // namespace uwrt_autonomy + +#endif // ARUCO_TARGET_TRACKER_H \ No newline at end of file diff --git a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h index 37b5c42f..af0dad74 100644 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h @@ -62,4 +62,4 @@ extern "C" { } #endif -#endif // UWRT_MARS_ROVER_VISION__VISIBILITY_H_ \ No newline at end of file +#endif // UWRT_MARS_ROVER_VISION__VISIBILITY_H_ \ No newline at end of file diff --git a/uwrt_mars_rover_vision/package.xml b/uwrt_mars_rover_vision/package.xml index df4ec181..83b1e45c 100644 --- a/uwrt_mars_rover_vision/package.xml +++ b/uwrt_mars_rover_vision/package.xml @@ -25,6 +25,11 @@ ament_lint_auto ament_lint_common + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_xmllint ament_cmake diff --git a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp index 5bdd8673..3e5378e9 100644 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -1,80 +1,84 @@ -#include -#include -#include "cv_bridge/cv_bridge.h" - #include #include #include #include -namespace uwrt_autonomy { +#include +#include + +#include "cv_bridge/cv_bridge.h" -TargetTracker::TargetTracker(const rclcpp::NodeOptions& options) : Node("target_tracker", options) +namespace uwrt_autonomy +{ +TargetTracker::TargetTracker(const rclcpp::NodeOptions & options) : Node("target_tracker", options) { // get parameters set in the launch file this->declare_parameter("aruco_marker_len", 0.2); this->declare_parameter("aruco_dict", ""); this->declare_parameter("display_marker_pose", true); aruco_marker_len_ = this->get_parameter("aruco_marker_len").get_parameter_value().get(); - display_marker_pose_ = this->get_parameter("display_marker_pose").get_parameter_value().get(); - std::string aruco_dict = this->get_parameter("aruco_dict").get_parameter_value().get(); + display_marker_pose_ = + this->get_parameter("display_marker_pose").get_parameter_value().get(); + std::string aruco_dict = + this->get_parameter("aruco_dict").get_parameter_value().get(); RCLCPP_DEBUG_STREAM(this->get_logger(), "Marker Len: " << aruco_marker_len_); - camera_info_sub_ = create_subscription("/zed2/zed_node/left/camera_info", 10, std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); + camera_info_sub_ = create_subscription( + "/zed2/zed_node/left/camera_info", 10, + std::bind(&TargetTracker::camInfoCallback, this, std::placeholders::_1)); rmw_qos_profile_t custom_qos = rmw_qos_profile_default; - camera_sub_ = image_transport::create_subscription(this, "/zed2/zed_node/left/image_rect_color", std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); - + camera_sub_ = image_transport::create_subscription( + this, "/zed2/zed_node/left/image_rect_color", + std::bind(&TargetTracker::imageCallback, this, std::placeholders::_1), "raw", custom_qos); + aruco_pose_pub_ = create_publisher("/aruco_pose", 10); - + // do aruco setup stuff - obj_points_.ptr(0)[0] = cv::Vec3f(-aruco_marker_len_/2.f, aruco_marker_len_/2.f, 0); - obj_points_.ptr(0)[1] = cv::Vec3f(aruco_marker_len_/2.f, aruco_marker_len_/2.f, 0); - obj_points_.ptr(0)[2] = cv::Vec3f(aruco_marker_len_/2.f, -aruco_marker_len_/2.f, 0); - obj_points_.ptr(0)[3] = cv::Vec3f(-aruco_marker_len_/2.f, -aruco_marker_len_/2.f, 0); + obj_points_.ptr(0)[0] = + cv::Vec3f(-aruco_marker_len_ / 2.f, aruco_marker_len_ / 2.f, 0); + obj_points_.ptr(0)[1] = cv::Vec3f(aruco_marker_len_ / 2.f, aruco_marker_len_ / 2.f, 0); + obj_points_.ptr(0)[2] = + cv::Vec3f(aruco_marker_len_ / 2.f, -aruco_marker_len_ / 2.f, 0); + obj_points_.ptr(0)[3] = + cv::Vec3f(-aruco_marker_len_ / 2.f, -aruco_marker_len_ / 2.f, 0); params_ = cv::aruco::DetectorParameters::create(); - if (aruco_dict == "") - { + if (aruco_dict == "") { RCLCPP_ERROR_STREAM(this->get_logger(), "Please set an ARUCO dictionary to use"); - } - else if (aruco_dict == "4x4_50") - { + } else if (aruco_dict == "4x4_50") { // use this dictionary for URC dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - } - else if (aruco_dict == "4x4_100") - { + } else if (aruco_dict == "4x4_100") { // use this dictionary for CIRC dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100); - } - else - { + } else { RCLCPP_ERROR_STREAM(this->get_logger(), "Please choose a defined ARUCO dictionary"); } } -void TargetTracker::toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose& pose_msg) +void TargetTracker::toPoseMsg(cv::Vec3d rvec, cv::Vec3d tvec, geometry_msgs::msg::Pose & pose_msg) { // convert the compressed rodrigues rotation vector into a rotation matrix cv::Mat rot_matrix(3, 3, CV_64FC1); cv::Rodrigues(rvec, rot_matrix); - tf2::Matrix3x3 tf2_rot_matrix{rot_matrix.at(0, 0), rot_matrix.at(0, 1), rot_matrix.at(0, 2), - rot_matrix.at(1, 0), rot_matrix.at(1, 1), rot_matrix.at(1, 2), - rot_matrix.at(2, 0), rot_matrix.at(2, 1), rot_matrix.at(2, 2)}; - tf2::Vector3 tf2_tvec{ tvec[0], tvec[1], tvec[2] }; + tf2::Matrix3x3 tf2_rot_matrix{ + rot_matrix.at(0, 0), rot_matrix.at(0, 1), rot_matrix.at(0, 2), + rot_matrix.at(1, 0), rot_matrix.at(1, 1), rot_matrix.at(1, 2), + rot_matrix.at(2, 0), rot_matrix.at(2, 1), rot_matrix.at(2, 2)}; + tf2::Vector3 tf2_tvec{tvec[0], tvec[1], tvec[2]}; // Create a transform and convert to a Pose - tf2::Transform tf2_transform {tf2_rot_matrix, tf2_tvec }; + tf2::Transform tf2_transform{tf2_rot_matrix, tf2_tvec}; tf2::toMsg(tf2_transform, pose_msg); } -void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg) +void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & image_msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(image_msg, image_msg->encoding); - } catch (cv_bridge::Exception& e) { + } catch (cv_bridge::Exception & e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); return; } @@ -85,58 +89,56 @@ void TargetTracker::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& // detect ARUCO markers in the image std::vector marker_ids; std::vector> marker_corners, rejection_candidates; - cv::aruco::detectMarkers(image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); + cv::aruco::detectMarkers( + image_rgb, dictionary_, marker_corners, marker_ids, params_, rejection_candidates); int num_markers_detected = marker_corners.size(); - + // identify visible aruco marker poses - if (num_markers_detected > 0) - { + if (num_markers_detected > 0) { // Calculate pose for each marker for (int i = 0; i < num_markers_detected; i++) { - cv::solvePnP(obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), - tvecs_.at(i)); + cv::solvePnP( + obj_points_, marker_corners.at(i), intrinsic_calib_matrix_, dist_coefficients_, + rvecs_.at(i), tvecs_.at(i)); // send a pose message stamped of the aruco tag pose geometry_msgs::msg::PoseStamped aruco_pose_msg; aruco_pose_msg.header.frame_id = std::to_string(marker_ids[i]); toPoseMsg(rvecs_[i], tvecs_[i], aruco_pose_msg.pose); aruco_pose_pub_->publish(aruco_pose_msg); - if (display_marker_pose_) - { + if (display_marker_pose_) { // draw the detected poses with coordinate axis of 0.2m in length - cv::drawFrameAxes(image_rgb, intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i), 0.2); + cv::drawFrameAxes( + image_rgb, intrinsic_calib_matrix_, dist_coefficients_, rvecs_.at(i), tvecs_.at(i), 0.2); } } - RCLCPP_DEBUG_STREAM(this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); - if (display_marker_pose_) - { + RCLCPP_DEBUG_STREAM( + this->get_logger(), "ARUCO at tvec: " << tvecs_.at(0) << " | rvec: " << rvecs_.at(0)); + if (display_marker_pose_) { cv::aruco::drawDetectedMarkers(image_rgb, marker_corners, marker_ids); } - } + } - if (display_marker_pose_) - { + if (display_marker_pose_) { cv::imshow("Image window", image_rgb); cv::waitKey(5); } } -void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info) +void TargetTracker::camInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr info) { // hacky work around to only read cam info once - if (intrinsic_calib_matrix_.size().height == 0) - { - RCLCPP_DEBUG_STREAM(this->get_logger(), info->distortion_model); - RCLCPP_DEBUG_STREAM(this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] << " | cy: " << info->k[5]); - int param_num = 0; - for (const double &disto_param: info->d) - { + if (intrinsic_calib_matrix_.size().height == 0) { + RCLCPP_DEBUG_STREAM(this->get_logger(), info->distortion_model); + RCLCPP_DEBUG_STREAM( + this->get_logger(), "fx: " << info->k[0] << " | cx: " << info->k[2] << " | fy: " << info->k[4] + << " | cy: " << info->k[5]); + int param_num = 0; + for (const double & disto_param : info->d) { dist_coefficients_.at(0, param_num) = disto_param; param_num++; - } - intrinsic_calib_matrix_ = (cv::Mat_(3,3) << - info->k[0], 0, info->k[2], - 0, info->k[4], info->k[5], - 0, 0, 1); + } + intrinsic_calib_matrix_ = + (cv::Mat_(3, 3) << info->k[0], 0, info->k[2], 0, info->k[4], info->k[5], 0, 0, 1); } } From 8b03673cc1e8904becfa289ad8d31f22682d7a8e Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 6 Feb 2023 21:28:56 -0500 Subject: [PATCH 08/11] add package into ci --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 50437d6c..da22375b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,6 +16,7 @@ env: uwrt_mars_rover_drivetrain uwrt_mars_rover_drivetrain_description uwrt_mars_rover_drivetrain_hw + uwrt_mars_rover_vision ROS_DISTRO: galactic IMAGE: "rostooling/setup-ros-docker:ubuntu-focal-ros-galactic-desktop-latest" From f722dd82381b367c437a62db328ff0e75c0f52a8 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 6 Feb 2023 21:42:14 -0500 Subject: [PATCH 09/11] add zed2 wrapper as a submodule --- .gitmodules | 3 +++ README.md | 2 +- zed-ros2-wrapper | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 .gitmodules create mode 160000 zed-ros2-wrapper diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..754486e7 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "zed-ros2-wrapper"] + path = zed-ros2-wrapper + url = https://github.com/stereolabs/zed-ros2-wrapper.git diff --git a/README.md b/README.md index 4874355b..5a2b5abd 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ 1. Navigate to your ROS2 workspace in the terminal (e.g: `dev_ws` or `uwrt_ws`) 2. Create a `src` directory if you haven't already: `mkdir -p dev_ws/src`. 3. Navigate inside of your `src` directory: `cd src` -4. Clone the repository into your `src` directory: `git clone git@github.com:uwrobotics/uwrt_mars_rover.git` +4. Clone the repository into your `src` directory: `git clone --recurse-submodules git@github.com:uwrobotics/uwrt_mars_rover.git` 5. Update your system before continuing: `sudo apt update -y --no-install-recommends && sudo apt dist-upgrade -y` 6. Install `rosdep`, the ROS dependency manager: `sudo apt install -y python3-rosdep` 7. Download the repository's upstream dependencies: `vcs import --input uwrt_mars_rover/common_upstream_dependencies.repos` diff --git a/zed-ros2-wrapper b/zed-ros2-wrapper new file mode 160000 index 00000000..8d377ba3 --- /dev/null +++ b/zed-ros2-wrapper @@ -0,0 +1 @@ +Subproject commit 8d377ba384528486db23201aad7deabf00b29e7b From b8b821dde582a1ef1d0fee24e0b533e34957c6e7 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 13 Feb 2023 22:15:19 -0500 Subject: [PATCH 10/11] remove submodule and add upstream dependency --- .gitmodules | 3 --- common_upstream_dependencies.repos | 5 +++++ zed-ros2-wrapper | 1 - 3 files changed, 5 insertions(+), 4 deletions(-) delete mode 100644 .gitmodules delete mode 160000 zed-ros2-wrapper diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 754486e7..00000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "zed-ros2-wrapper"] - path = zed-ros2-wrapper - url = https://github.com/stereolabs/zed-ros2-wrapper.git diff --git a/common_upstream_dependencies.repos b/common_upstream_dependencies.repos index b7ac034c..4433779f 100644 --- a/common_upstream_dependencies.repos +++ b/common_upstream_dependencies.repos @@ -12,3 +12,8 @@ repositories: type: git url: https://github.com/dawonn/vectornav version: 1ecaea8966eb04507d7a80b86a21860e4941928f # Latest known working on ros2 branch + + zed2_ros_wrapper: + type: git + url: https://github.com/stereolabs/zed-ros2-wrapper.git + version: master diff --git a/zed-ros2-wrapper b/zed-ros2-wrapper deleted file mode 160000 index 8d377ba3..00000000 --- a/zed-ros2-wrapper +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8d377ba384528486db23201aad7deabf00b29e7b From b0b92634865975bfc997699b62138700f25fac72 Mon Sep 17 00:00:00 2001 From: nick-palmar Date: Mon, 13 Feb 2023 22:16:27 -0500 Subject: [PATCH 11/11] revet readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5a2b5abd..4874355b 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ 1. Navigate to your ROS2 workspace in the terminal (e.g: `dev_ws` or `uwrt_ws`) 2. Create a `src` directory if you haven't already: `mkdir -p dev_ws/src`. 3. Navigate inside of your `src` directory: `cd src` -4. Clone the repository into your `src` directory: `git clone --recurse-submodules git@github.com:uwrobotics/uwrt_mars_rover.git` +4. Clone the repository into your `src` directory: `git clone git@github.com:uwrobotics/uwrt_mars_rover.git` 5. Update your system before continuing: `sudo apt update -y --no-install-recommends && sudo apt dist-upgrade -y` 6. Install `rosdep`, the ROS dependency manager: `sudo apt install -y python3-rosdep` 7. Download the repository's upstream dependencies: `vcs import --input uwrt_mars_rover/common_upstream_dependencies.repos`