From 0d9cdb4e185242ee99a9d3a8d189919a8b5afa10 Mon Sep 17 00:00:00 2001 From: nico-palmar <66019522+nico-palmar@users.noreply.github.com> Date: Thu, 21 Sep 2023 22:05:01 -0700 Subject: [PATCH] Revert "User/nick palmar/207/aruco pose detection (#228)" (#255) Not building on the jetson --- .github/workflows/ci.yaml | 1 - uwrt_mars_rover_vision/CMakeLists.txt | 89 ------------ .../aruco_target_tracker.hpp | 68 --------- .../uwrt_mars_rover_vision/visibility.h | 65 --------- .../launch/zed_vision.launch.py | 51 ------- uwrt_mars_rover_vision/package.xml | 37 ----- .../src/aruco_target_tracker.cpp | 130 ------------------ 7 files changed, 441 deletions(-) delete mode 100644 uwrt_mars_rover_vision/CMakeLists.txt delete mode 100644 uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp delete mode 100644 uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h delete mode 100644 uwrt_mars_rover_vision/launch/zed_vision.launch.py delete mode 100644 uwrt_mars_rover_vision/package.xml delete mode 100644 uwrt_mars_rover_vision/src/aruco_target_tracker.cpp diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index da22375b..50437d6c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,7 +16,6 @@ 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" diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt deleted file mode 100644 index 7e59a791..00000000 --- a/uwrt_mars_rover_vision/CMakeLists.txt +++ /dev/null @@ -1,89 +0,0 @@ -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) -find_package(geometry_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -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(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" - ) -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 - aruco_tracker - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -# Install launch files. -install(DIRECTORY - launch - 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 deleted file mode 100644 index 4c58a226..00000000 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef ARUCO_TARGET_TRACKER_H -#define ARUCO_TARGET_TRACKER_H - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace uwrt_autonomy { -class TargetTracker : public rclcpp::Node { - public: - 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); - - /** - * @brief Reads intrinstic calibration data from the camera info topic - * @param info Camera info message - */ - 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); - - // 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 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_; -}; - -} // 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 deleted file mode 100644 index af0dad74..00000000 --- a/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/visibility.h +++ /dev/null @@ -1,65 +0,0 @@ -// 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 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. - -#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 deleted file mode 100644 index 92b51288..00000000 --- a/uwrt_mars_rover_vision/launch/zed_vision.launch.py +++ /dev/null @@ -1,51 +0,0 @@ -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 -""" diff --git a/uwrt_mars_rover_vision/package.xml b/uwrt_mars_rover_vision/package.xml deleted file mode 100644 index 83b1e45c..00000000 --- a/uwrt_mars_rover_vision/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - 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 - geometry_msgs - - 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 deleted file mode 100644 index b5e6c2e6..00000000 --- a/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp +++ /dev/null @@ -1,130 +0,0 @@ - -#include -#include -#include -#include - -#include -#include - -#include "cv_bridge/cv_bridge.h" - -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(); - 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); - - 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); - - params_ = cv::aruco::DetectorParameters::create(); - 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) { - // 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& image_msg) { - cv_bridge::CvImagePtr cv_ptr; - try { - 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; - } - // 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 visible aruco marker poses - 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); - 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); - } - } - 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_) { - cv::imshow("Image window", image_rgb); - cv::waitKey(5); - } -} - -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) { - 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); - } -} - -} // namespace uwrt_autonomy - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(uwrt_autonomy::TargetTracker)