Add ros2_aruco package

robot/rospackages/src/ros2_aruco/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 JMU-ROBOTICS-VIVA
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

robot/rospackages/src/ros2_aruco/README.md

@@ -0,0 +1,64 @@
+# ros2_aruco
+
+ROS2 Wrapper for OpenCV Aruco Marker Tracking
+
+This package depends on a recent version of OpenCV python bindings and transforms3d library:
+
+```
+pip3 install opencv-contrib-python transforms3d
+```
+
+## ROS2 API for the ros2_aruco Node
+
+This node locates Aruco AR markers in images and publishes their ids and poses.
+
+Subscriptions:
+* `/camera/image_raw` (`sensor_msgs.msg.Image`)
+* `/camera/camera_info` (`sensor_msgs.msg.CameraInfo`)
+
+Published Topics:
+* `/aruco_poses` (`geometry_msgs.msg.PoseArray`) - Poses of all detected markers (suitable for rviz visualization)
+* `/aruco_markers` (`ros2_aruco_interfaces.msg.ArucoMarkers`) - Provides an array of all poses along with the corresponding marker ids
+
+Parameters:
+* `marker_size` - size of the markers in meters (default .0625)
+* `aruco_dictionary_id` - dictionary that was used to generate markers (default `DICT_5X5_250`)
+* `image_topic` - image topic to subscribe to (default `/camera/image_raw`)
+* `camera_info_topic` - Camera info topic to subscribe to (default `/camera/camera_info`)
+* `camera_frame` - Camera optical frame to use (default to the frame id provided by the camera info message.)
+
+## Running Marker Detection
+
+1. Using the launch file - parameters will be loaded from _aruco\_parameters.yaml_.
+```
+ros2 launch ros2_aruco aruco_recognition.launch.py
+```
+2. As a single ROS 2 node - you can specify parameter values at startup by adding `--ros-args -p marker_size:=.05`, for example.
+```
+ros2 run ros2_aruco aruco_node
+```
+
+## Generating Marker Images
+
+```
+ros2 run ros2_aruco aruco_generate_marker
+```
+
+Pass the `-h` flag for usage information: 
+
+```
+usage: aruco_generate_marker [-h] [--id ID] [--size SIZE] [--dictionary]
+
+Generate a .png image of a specified maker.
+
+optional arguments:
+  -h, --help     show this help message and exit
+  --id ID        Marker id to generate (default: 1)
+  --size SIZE    Side length in pixels (default: 200)
+  --dictionary   Dictionary to use. Valid options include: DICT_4X4_100,
+                 DICT_4X4_1000, DICT_4X4_250, DICT_4X4_50, DICT_5X5_100,
+                 DICT_5X5_1000, DICT_5X5_250, DICT_5X5_50, DICT_6X6_100,
+                 DICT_6X6_1000, DICT_6X6_250, DICT_6X6_50, DICT_7X7_100,
+                 DICT_7X7_1000, DICT_7X7_250, DICT_7X7_50, DICT_ARUCO_ORIGINAL
+                 (default: DICT_5X5_250)
+```

robot/rospackages/src/ros2_aruco/ros2_aruco/config/aruco_parameters.yaml

@@ -0,0 +1,6 @@
+/aruco_node:
+  ros__parameters:
+    marker_size: 0.055
+    aruco_dictionary_id: DICT_5X5_250
+    image_topic: /image_raw
+    camera_info_topic: /camera_info

robot/rospackages/src/ros2_aruco/ros2_aruco/launch/aruco_recognition.launch.py

@@ -0,0 +1,22 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    aruco_params = os.path.join(
+        get_package_share_directory('ros2_aruco'),
+        'config',
+        'aruco_parameters.yaml'
+        )
+
+    aruco_node = Node(
+        package='ros2_aruco',
+        executable='aruco_node',
+        parameters=[aruco_params]
+    )
+
+    return LaunchDescription([
+        aruco_node
+    ])

robot/rospackages/src/ros2_aruco/ros2_aruco/package.xml

@@ -0,0 +1,18 @@
+<?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>ros2_aruco</name>
+  <version>0.1.0</version>
+  <description>ROS2 Aruco Marker Tracking</description>
+  <maintainer email="nathan.r.sprague@gmail.com">Nathan Sprague</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_generate_marker.py

@@ -0,0 +1,49 @@
+"""
+Script for generating Aruco marker images.
+
+Author: Nathan Sprague
+Version: 10/26/2020
+"""
+
+import argparse
+import cv2
+import numpy as np
+
+
+class CustomFormatter(
+    argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter
+):
+    """Trick to allow both defaults and nice formatting in the help."""
+
+    pass
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        formatter_class=CustomFormatter,
+        description="Generate a .png image of a specified maker.",
+    )
+    parser.add_argument("--id", default=1, type=int, help="Marker id to generate")
+    parser.add_argument("--size", default=200, type=int, help="Side length in pixels")
+    dict_options = [s for s in dir(cv2.aruco) if s.startswith("DICT")]
+    option_str = ", ".join(dict_options)
+    dict_help = "Dictionary to use. Valid options include: {}".format(option_str)
+    parser.add_argument(
+        "--dictionary",
+        default="DICT_5X5_250",
+        type=str,
+        choices=dict_options,
+        help=dict_help,
+        metavar="",
+    )
+    args = parser.parse_args()
+
+    dictionary_id = cv2.aruco.__getattribute__(args.dictionary)
+    dictionary = cv2.aruco.Dictionary_get(dictionary_id)
+    image = np.zeros((args.size, args.size), dtype=np.uint8)
+    image = cv2.aruco.drawMarker(dictionary, args.id, args.size, image, 1)
+    cv2.imwrite("marker_{:04d}.png".format(args.id), image)
+
+
+if __name__ == "__main__":
+    main()