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" diff --git a/uwrt_mars_rover_vision/CMakeLists.txt b/uwrt_mars_rover_vision/CMakeLists.txt new file mode 100644 index 00000000..7e59a791 --- /dev/null +++ b/uwrt_mars_rover_vision/CMakeLists.txt @@ -0,0 +1,89 @@ +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 new file mode 100644 index 00000000..4c58a226 --- /dev/null +++ b/uwrt_mars_rover_vision/include/uwrt_mars_rover_vision/aruco_target_tracker.hpp @@ -0,0 +1,68 @@ +#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 new file mode 100644 index 00000000..af0dad74 --- /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 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 new file mode 100644 index 00000000..92b51288 --- /dev/null +++ b/uwrt_mars_rover_vision/launch/zed_vision.launch.py @@ -0,0 +1,51 @@ +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 new file mode 100644 index 00000000..83b1e45c --- /dev/null +++ b/uwrt_mars_rover_vision/package.xml @@ -0,0 +1,37 @@ + + + + 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 new file mode 100644 index 00000000..b5e6c2e6 --- /dev/null +++ b/uwrt_mars_rover_vision/src/aruco_target_tracker.cpp @@ -0,0 +1,130 @@ + +#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)