User/nick palmar/207/aruco pose detection (#228)

aruco tag detection 1.0

.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"
 

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;$<TARGET_FILE:aruco_tracker>\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()

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 <uwrt_mars_rover_vision/visibility.h>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <image_transport/image_transport.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <vector>
+
+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<geometry_msgs::msg::PoseStamped>::SharedPtr aruco_pose_pub_;
+  // zed2 image topic subscriber
+  image_transport::Subscriber camera_sub_;
+  // camera info subscriber to get intrinstic calibration params
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::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<cv::Vec3d> rvecs_{std::vector<cv::Vec3d>(4, 0.0)}, tvecs_{std::vector<cv::Vec3d>(4, 0.0)};
+  // opencv aruco detector variables
+  cv::Ptr<cv::aruco::DetectorParameters> params_;
+  cv::Ptr<cv::aruco::Dictionary> 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

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_

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
+"""

uwrt_mars_rover_vision/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>uwrt_mars_rover_vision</name>
+  <version>0.0.0</version>
+  <description>ROS2 rover vision autonomy package</description>
+  <maintainer email="npalmar@uwaterloo.ca">nico</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>libopencv-dev</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_xmllint</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>