diff --git a/robot/rospackages/src/ros2_aruco/LICENSE b/robot/rospackages/src/ros2_aruco/LICENSE new file mode 100644 index 000000000..7ecd03812 --- /dev/null +++ b/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. diff --git a/robot/rospackages/src/ros2_aruco/README.md b/robot/rospackages/src/ros2_aruco/README.md new file mode 100644 index 000000000..0e4dd2938 --- /dev/null +++ b/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) +``` diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/config/aruco_parameters.yaml b/robot/rospackages/src/ros2_aruco/ros2_aruco/config/aruco_parameters.yaml new file mode 100644 index 000000000..f2d4aa9c5 --- /dev/null +++ b/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 diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/launch/aruco_recognition.launch.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/launch/aruco_recognition.launch.py new file mode 100644 index 000000000..9b71c931a --- /dev/null +++ b/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 + ]) diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/package.xml b/robot/rospackages/src/ros2_aruco/ros2_aruco/package.xml new file mode 100644 index 000000000..3517556c1 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/package.xml @@ -0,0 +1,18 @@ + + + + ros2_aruco + 0.1.0 + ROS2 Aruco Marker Tracking + Nathan Sprague + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/resource/ros2_aruco b/robot/rospackages/src/ros2_aruco/ros2_aruco/resource/ros2_aruco new file mode 100644 index 000000000..e69de29bb diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/__init__.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_generate_marker.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_generate_marker.py new file mode 100644 index 000000000..b403c9ba3 --- /dev/null +++ b/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() diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_node.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_node.py new file mode 100644 index 000000000..2fe85ec1f --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/ros2_aruco/aruco_node.py @@ -0,0 +1,222 @@ +""" +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) + /camera/camera_info (sensor_msgs.msg.CameraInfo) + +Published Topics: + /aruco_poses (geometry_msgs.msg.PoseArray) + Pose 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) + +Author: Nathan Sprague +Version: 10/26/2020 + +""" + +import rclpy +import rclpy.node +from rclpy.qos import qos_profile_sensor_data +from cv_bridge import CvBridge +import numpy as np +import cv2 +import tf_transformations +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseArray, Pose +from ros2_aruco_interfaces.msg import ArucoMarkers +from rcl_interfaces.msg import ParameterDescriptor, ParameterType + + +class ArucoNode(rclpy.node.Node): + def __init__(self): + super().__init__("aruco_node") + + # Declare and read parameters + self.declare_parameter( + name="marker_size", + value=0.0625, + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_DOUBLE, + description="Size of the markers in meters.", + ), + ) + + self.declare_parameter( + name="aruco_dictionary_id", + value="DICT_5X5_250", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Dictionary that was used to generate markers.", + ), + ) + + self.declare_parameter( + name="image_topic", + value="/camera/image_raw", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Image topic to subscribe to.", + ), + ) + + self.declare_parameter( + name="camera_info_topic", + value="/camera/camera_info", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Camera info topic to subscribe to.", + ), + ) + + self.declare_parameter( + name="camera_frame", + value="", + descriptor=ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description="Camera optical frame to use.", + ), + ) + + self.marker_size = ( + self.get_parameter("marker_size").get_parameter_value().double_value + ) + self.get_logger().info(f"Marker size: {self.marker_size}") + + dictionary_id_name = ( + self.get_parameter("aruco_dictionary_id").get_parameter_value().string_value + ) + self.get_logger().info(f"Marker type: {dictionary_id_name}") + + image_topic = ( + self.get_parameter("image_topic").get_parameter_value().string_value + ) + self.get_logger().info(f"Image topic: {image_topic}") + + info_topic = ( + self.get_parameter("camera_info_topic").get_parameter_value().string_value + ) + self.get_logger().info(f"Image info topic: {info_topic}") + + self.camera_frame = ( + self.get_parameter("camera_frame").get_parameter_value().string_value + ) + + # Make sure we have a valid dictionary id: + try: + dictionary_id = cv2.aruco.__getattribute__(dictionary_id_name) + if type(dictionary_id) != type(cv2.aruco.DICT_5X5_100): + raise AttributeError + except AttributeError: + self.get_logger().error( + "bad aruco_dictionary_id: {}".format(dictionary_id_name) + ) + options = "\n".join([s for s in dir(cv2.aruco) if s.startswith("DICT")]) + self.get_logger().error("valid options: {}".format(options)) + + # Set up subscriptions + self.info_sub = self.create_subscription( + CameraInfo, info_topic, self.info_callback, qos_profile_sensor_data + ) + + self.create_subscription( + Image, image_topic, self.image_callback, qos_profile_sensor_data + ) + + # Set up publishers + self.poses_pub = self.create_publisher(PoseArray, "aruco_poses", 10) + self.markers_pub = self.create_publisher(ArucoMarkers, "aruco_markers", 10) + + # Set up fields for camera parameters + self.info_msg = None + self.intrinsic_mat = None + self.distortion = None + + self.aruco_dictionary = cv2.aruco.Dictionary_get(dictionary_id) + self.aruco_parameters = cv2.aruco.DetectorParameters_create() + self.bridge = CvBridge() + + def info_callback(self, info_msg): + self.info_msg = info_msg + self.intrinsic_mat = np.reshape(np.array(self.info_msg.k), (3, 3)) + self.distortion = np.array(self.info_msg.d) + # Assume that camera parameters will remain the same... + self.destroy_subscription(self.info_sub) + + def image_callback(self, img_msg): + if self.info_msg is None: + self.get_logger().warn("No camera info has been received!") + return + + cv_image = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding="mono8") + markers = ArucoMarkers() + pose_array = PoseArray() + if self.camera_frame == "": + markers.header.frame_id = self.info_msg.header.frame_id + pose_array.header.frame_id = self.info_msg.header.frame_id + else: + markers.header.frame_id = self.camera_frame + pose_array.header.frame_id = self.camera_frame + + markers.header.stamp = img_msg.header.stamp + pose_array.header.stamp = img_msg.header.stamp + + corners, marker_ids, rejected = cv2.aruco.detectMarkers( + cv_image, self.aruco_dictionary, parameters=self.aruco_parameters + ) + if marker_ids is not None: + if cv2.__version__ > "4.0.0": + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, self.marker_size, self.intrinsic_mat, self.distortion + ) + else: + rvecs, tvecs = cv2.aruco.estimatePoseSingleMarkers( + corners, self.marker_size, self.intrinsic_mat, self.distortion + ) + for i, marker_id in enumerate(marker_ids): + pose = Pose() + pose.position.x = tvecs[i][0][0] + pose.position.y = tvecs[i][0][1] + pose.position.z = tvecs[i][0][2] + + rot_matrix = np.eye(4) + rot_matrix[0:3, 0:3] = cv2.Rodrigues(np.array(rvecs[i][0]))[0] + quat = tf_transformations.quaternion_from_matrix(rot_matrix) + + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + pose_array.poses.append(pose) + markers.poses.append(pose) + markers.marker_ids.append(marker_id[0]) + + self.poses_pub.publish(pose_array) + self.markers_pub.publish(markers) + + +def main(): + rclpy.init() + node = ArucoNode() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.cfg b/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.cfg new file mode 100644 index 000000000..0574aa830 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ros2_aruco +[install] +install_scripts=$base/lib/ros2_aruco diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.py new file mode 100644 index 000000000..ea4b99e08 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/setup.py @@ -0,0 +1,31 @@ +from setuptools import setup +import os +from glob import glob + +package_name = 'ros2_aruco' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Nathan Sprague', + maintainer_email='nathan.r.sprague@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'aruco_node = ros2_aruco.aruco_node:main', + 'aruco_generate_marker = ros2_aruco.aruco_generate_marker:main' + ], + }, +) diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_copyright.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_copyright.py new file mode 100644 index 000000000..cc8ff03f7 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_flake8.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_flake8.py new file mode 100644 index 000000000..eff829969 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 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. + +from ament_flake8.main import main +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_pep257.py b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_pep257.py new file mode 100644 index 000000000..b234a3840 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/CMakeLists.txt b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/CMakeLists.txt new file mode 100644 index 000000000..61a36ba90 --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_aruco_interfaces) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(rosidl_default_generators REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ArucoMarkers.msg" + DEPENDENCIES geometry_msgs + DEPENDENCIES std_msgs + ) + + +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() + +ament_package() diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/msg/ArucoMarkers.msg b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/msg/ArucoMarkers.msg new file mode 100644 index 000000000..184003ddd --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/msg/ArucoMarkers.msg @@ -0,0 +1,5 @@ + +std_msgs/Header header + +int64[] marker_ids +geometry_msgs/Pose[] poses diff --git a/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/package.xml b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/package.xml new file mode 100644 index 000000000..5c5c91b1d --- /dev/null +++ b/robot/rospackages/src/ros2_aruco/ros2_aruco_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + ros2_aruco_interfaces + 0.1.0 + Custom messages for ros2_aruco + Nathan Sprague + TODO: License declaration + + ament_cmake + + geometry_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + +