From 38141fce7dab691357f497f70ef6f31a73140069 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Albert=20Arl=C3=A0?= Date: Fri, 27 Oct 2023 13:51:48 +0200 Subject: [PATCH 1/9] continuous_detector for ros2 added --- apriltag_ros/CMakeLists.txt | 243 ++-- ...ommon_functions.h => common_functions.hpp} | 206 +-- .../apriltag_ros/composition_visibility.h | 58 + ...ous_detector.h => continuous_detector.hpp} | 64 +- .../launch/continuous_detection.launch | 23 - apriltag_ros/launch/launch.py | 83 ++ .../launch/single_image_client.launch | 25 - .../launch/single_image_server.launch | 12 - apriltag_ros/nodelet_plugins.xml | 9 - apriltag_ros/package.xml | 89 +- .../src/apriltag_ros_continuous_node.cpp | 50 - apriltag_ros/src/common_functions.cpp | 1234 ++++++++--------- apriltag_ros/src/continuous_detector.cpp | 154 +- 13 files changed, 1047 insertions(+), 1203 deletions(-) rename apriltag_ros/include/apriltag_ros/{common_functions.h => common_functions.hpp} (51%) create mode 100644 apriltag_ros/include/apriltag_ros/composition_visibility.h rename apriltag_ros/include/apriltag_ros/{continuous_detector.h => continuous_detector.hpp} (57%) delete mode 100644 apriltag_ros/launch/continuous_detection.launch create mode 100644 apriltag_ros/launch/launch.py delete mode 100644 apriltag_ros/launch/single_image_client.launch delete mode 100644 apriltag_ros/launch/single_image_server.launch delete mode 100644 apriltag_ros/nodelet_plugins.xml delete mode 100644 apriltag_ros/src/apriltag_ros_continuous_node.cpp diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index a933668a..79451336 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -1,158 +1,125 @@ -cmake_minimum_required(VERSION 3.0.2) -project(apriltag_ros) - -set(CMAKE_CXX_STANDARD 17) - -find_package(catkin REQUIRED COMPONENTS - cmake_modules - cv_bridge - geometry_msgs - image_geometry - image_transport - message_generation - nodelet - pluginlib - roscpp - sensor_msgs - std_msgs - tf -) - +cmake_minimum_required(VERSION 3.5) +project(atlas_apriltag_ros) + +# 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 -lgtest -lgmock -g) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ageve_interfaces REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(rcutils REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(apriltag REQUIRED) find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) -find_package(apriltag REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(image_geometry REQUIRED) -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -# We default to 'Release' if none was specified -IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) - MESSAGE(STATUS "Setting build type to 'Release' as none was specified.") - SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the build type" FORCE) - SET_PROPERTY(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Coverage" "Debug" "Release" "MinSizeRel" "RelWithDebInfo") -ENDIF() - -add_compile_options("-O3" "-funsafe-loop-optimizations" "-fsee" "-funroll-loops" "-fno-math-errno" "-funsafe-math-optimizations" "-ffinite-math-only" "-fno-signed-zeros") - -# Note: These options have been turned off to allow for binary releases - -# in local builds, they can be reactivated to achieve higher performance. -# if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64" OR "x86_32") -# message("enabling msse2 for x86_64 or x86_32 architecture") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -msse2 ") -# endif() -# if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "arm") -# message("enabling -mfpu=neon -mfloat-abi=softfp for ARM architecture") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv7-a -mcpu=cortex-a9 -mfpu=neon -mtune=cortex-a9 -mvectorize-with-neon-quad -ffast-math ") -# endif() - -add_message_files( - FILES - AprilTagDetection.msg - AprilTagDetectionArray.msg -) -add_service_files( - FILES - AnalyzeSingleImage.srv -) -generate_messages( - DEPENDENCIES - geometry_msgs +set(dependencies + rclcpp + rclcpp_components + ageve_interfaces + cv_bridge + image_transport + rcutils sensor_msgs - std_msgs -) - -# Extract the include directories and libraries from apriltag::apriltag as catkin_package() does not support modern cmake. -get_target_property(apriltag_INCLUDE_DIRS apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES) -get_target_property(apriltag_LIBRARIES apriltag::apriltag INTERFACE_LINK_LIBRARIES) - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - cv_bridge - geometry_msgs - image_transport - message_runtime - nodelet - pluginlib - roscpp - sensor_msgs - std_msgs - tf - DEPENDS - apriltag - OpenCV - LIBRARIES - ${PROJECT_NAME}_common - ${PROJECT_NAME}_continuous_detector - ${PROJECT_NAME}_single_image_detector + tf2_ros + tf2 + apriltag + Eigen3 + OpenCV + image_geometry + yaml-cpp + geometry_msgs ) -########### -## Build ## -########### -include_directories(include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} +# Include directories +include_directories( + include ) -add_library(${PROJECT_NAME}_common src/common_functions.cpp) -add_dependencies(${PROJECT_NAME}_common ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_common ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} apriltag::apriltag) - -add_library(${PROJECT_NAME}_continuous_detector src/continuous_detector.cpp) -target_link_libraries(${PROJECT_NAME}_continuous_detector ${PROJECT_NAME}_common ${catkin_LIBRARIES}) - -add_library(${PROJECT_NAME}_single_image_detector src/single_image_detector.cpp) -target_link_libraries(${PROJECT_NAME}_single_image_detector ${PROJECT_NAME}_common ${catkin_LIBRARIES}) +# create ament index resource which references the libraries in the binary dir +set(node_plugins "") -add_executable(${PROJECT_NAME}_continuous_node src/${PROJECT_NAME}_continuous_node.cpp) -add_dependencies(${PROJECT_NAME}_continuous_node ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_continuous_node ${PROJECT_NAME}_continuous_detector ${catkin_LIBRARIES}) -add_executable(${PROJECT_NAME}_single_image_server_node src/${PROJECT_NAME}_single_image_server_node.cpp) -add_dependencies(${PROJECT_NAME}_single_image_server_node ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_single_image_server_node ${PROJECT_NAME}_single_image_detector ${catkin_LIBRARIES}) -add_executable(${PROJECT_NAME}_single_image_client_node src/${PROJECT_NAME}_single_image_client_node.cpp) -add_dependencies(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_generate_messages_cpp) -target_link_libraries(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_common ${catkin_LIBRARIES}) +# Add cpp libraries +add_library(atlas_apriltag_ros_component SHARED + "src/continuous_detector.cpp" + "src/common_functions.cpp" +) +target_link_libraries(atlas_apriltag_ros_component yaml-cpp apriltag) + +target_compile_definitions(atlas_apriltag_ros_component + PRIVATE "COMPOSITION_BUILDING_DLL") + +ament_target_dependencies(atlas_apriltag_ros_component + rclcpp + rclcpp_components + ageve_interfaces + cv_bridge + image_transport + rcutils + sensor_msgs + tf2_ros + tf2 + apriltag + Eigen3 + OpenCV + image_geometry + yaml-cpp + geometry_msgs + ) + +rclcpp_components_register_node(atlas_apriltag_ros_component PLUGIN "atlas_apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) +set(node_plugins "${node_plugins}atlas_apriltag_ros::ContinuousDetector;$\n") + +# since the package installs libraries without exporting them +# it needs to make sure that the library path is being exported +if(NOT WIN32) + ament_environment_hooks( + "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") +endif() -############# -## Install ## -############# +install(TARGETS + atlas_apriltag_ros_component + DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) +install(TARGETS atlas_apriltag_ros_component + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include + ) -install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(TARGETS - ${PROJECT_NAME}_common - ${PROJECT_NAME}_continuous_detector - ${PROJECT_NAME}_continuous_node - ${PROJECT_NAME}_single_image_client_node - ${PROJECT_NAME}_single_image_detector - ${PROJECT_NAME}_single_image_server_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME} +) -install(PROGRAMS scripts/analyze_image DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +ament_package() \ No newline at end of file diff --git a/apriltag_ros/include/apriltag_ros/common_functions.h b/apriltag_ros/include/apriltag_ros/common_functions.hpp similarity index 51% rename from apriltag_ros/include/apriltag_ros/common_functions.h rename to apriltag_ros/include/apriltag_ros/common_functions.hpp index 62264bec..beb740ab 100644 --- a/apriltag_ros/include/apriltag_ros/common_functions.h +++ b/apriltag_ros/include/apriltag_ros/common_functions.hpp @@ -41,44 +41,49 @@ * Originator: Danylo Malyuta, JPL ******************************************************************************/ -#ifndef APRILTAG_ROS_COMMON_FUNCTIONS_H -#define APRILTAG_ROS_COMMON_FUNCTIONS_H +#ifndef ATLAS_APRILTAG_ROS_COMMON_FUNCTIONS_H +#define ATLAS_APRILTAG_ROS_COMMON_FUNCTIONS_H +// C++ #include #include #include #include #include -#include -#include -#include +// ROS2 +#include "rclcpp/rclcpp.hpp" +#include +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "std_msgs/msg/header.hpp" + +// Image related #include #include #include #include #include #include -#include -#include -#include +#include +#include -#include +// tf related +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/utils.h" -#include "apriltag_ros/AprilTagDetection.h" -#include "apriltag_ros/AprilTagDetectionArray.h" +#include "ageve_interfaces/msg/april_tag_detection_array.hpp" +#include "ageve_interfaces/msg/april_tag_detection.hpp" + +#include "yaml-cpp/yaml.h" -namespace apriltag_ros -{ -template -T getAprilTagOption(ros::NodeHandle& pnh, - const std::string& param_name, const T & default_val) +namespace atlas_apriltag_ros { - T param_val; - pnh.param(param_name, param_val, default_val); - return param_val; -} // Stores the properties of a tag member of a bundle struct TagBundleMember @@ -155,89 +160,86 @@ class TagBundleDescription class TagDetector { - private: - // Detections sorting - static int idComparison(const void* first, const void* second); - - // Remove detections of tags with the same ID - void removeDuplicates(); - - // AprilTag 2 code's attributes - std::string family_; - int threads_; - double decimate_; - double blur_; - int refine_edges_; - int debug_; - int max_hamming_distance_ = 2; // Tunable, but really, 2 is a good choice. Values of >=3 - // consume prohibitively large amounts of memory, and otherwise - // you want the largest value possible. - - // AprilTag 2 objects - apriltag_family_t *tf_; - apriltag_detector_t *td_; - zarray_t *detections_; - - // Other members - std::map standalone_tag_descriptions_; - std::vector tag_bundle_descriptions_; - bool remove_duplicates_; - bool run_quietly_; - bool publish_tf_; - tf::TransformBroadcaster tf_pub_; - - public: + public: + + TagDetector(rclcpp::Node::SharedPtr nh); + ~TagDetector(); + + // Store standalone and bundle tag descriptions + std::map parse_standalone_tags_from_yaml(const std::string& yaml_string); + std::vector parse_tag_bundles_from_yaml(const std::string& yaml_string); + + + bool findStandaloneTagDescription( + int id, StandaloneTagDescription*& descriptionContainer, + bool printWarning = true); + + geometry_msgs::msg::PoseWithCovarianceStamped makeTagPose( + const Eigen::Isometry3d& transform, + const std_msgs::msg::Header& header); + + // Detect tags in an image + ageve_interfaces::msg::AprilTagDetectionArray detectTags( + const cv_bridge::CvImagePtr& image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info); + + // Get the pose of the tag in the camera frame + // Returns homogeneous transformation matrix [R,t;[0 0 0 1]] which + // takes a point expressed in the tag frame to the same point + // expressed in the camera frame. As usual, R is the (passive) + // rotation from the tag frame to the camera frame and t is the + // vector from the camera frame origin to the tag frame origin, + // expressed in the camera frame. + Eigen::Isometry3d getRelativeTransform( + const std::vector& objectPoints, + const std::vector& imagePoints, + double fx, double fy, double cx, double cy) const; + + void addImagePoints(apriltag_detection_t *detection, + std::vector& imagePoints) const; + void addObjectPoints(double s, cv::Matx44d T_oi, + std::vector& objectPoints) const; + + // Draw the detected tags' outlines and payload values on the image + void drawDetections(cv_bridge::CvImagePtr image); + + bool get_publish_tf() const { return publish_tf_; } + + private: + // Detections sorting + static int idComparison(const void* first, const void* second); + + // Remove detections of tags with the same ID + void removeDuplicates(); + + rclcpp::Node::SharedPtr nh_; + + // AprilTag 2 code's attributes + std::string family_; + int threads_; + double decimate_; + double blur_; + int refine_edges_; + int debug_; + int max_hamming_distance_ = 2; // Tunable, but really, 2 is a good choice. Values of >=3 + // consume prohibitively large amounts of memory, and otherwise + // you want the largest value possible. + + // AprilTag 2 objects + apriltag_family_t *tf_; + apriltag_detector_t *td_; + zarray_t *detections_; + + // Other members + std::map standalone_tag_descriptions_; + std::vector tag_bundle_descriptions_; + bool remove_duplicates_; + bool run_quietly_; + bool publish_tf_; + std::unique_ptr tf_pub_; - TagDetector(ros::NodeHandle pnh); - ~TagDetector(); - - // Store standalone and bundle tag descriptions - std::map parseStandaloneTags( - XmlRpc::XmlRpcValue& standalone_tag_descriptions); - std::vector parseTagBundles( - XmlRpc::XmlRpcValue& tag_bundles); - double xmlRpcGetDouble( - XmlRpc::XmlRpcValue& xmlValue, std::string field) const; - double xmlRpcGetDoubleWithDefault( - XmlRpc::XmlRpcValue& xmlValue, std::string field, - double defaultValue) const; - - bool findStandaloneTagDescription( - int id, StandaloneTagDescription*& descriptionContainer, - bool printWarning = true); - - geometry_msgs::PoseWithCovarianceStamped makeTagPose( - const Eigen::Isometry3d& transform, - const std_msgs::Header& header); - - // Detect tags in an image - AprilTagDetectionArray detectTags( - const cv_bridge::CvImagePtr& image, - const sensor_msgs::CameraInfoConstPtr& camera_info); - - // Get the pose of the tag in the camera frame - // Returns homogeneous transformation matrix [R,t;[0 0 0 1]] which - // takes a point expressed in the tag frame to the same point - // expressed in the camera frame. As usual, R is the (passive) - // rotation from the tag frame to the camera frame and t is the - // vector from the camera frame origin to the tag frame origin, - // expressed in the camera frame. - Eigen::Isometry3d getRelativeTransform( - const std::vector& objectPoints, - const std::vector& imagePoints, - double fx, double fy, double cx, double cy) const; - - void addImagePoints(apriltag_detection_t *detection, - std::vector& imagePoints) const; - void addObjectPoints(double s, cv::Matx44d T_oi, - std::vector& objectPoints) const; - - // Draw the detected tags' outlines and payload values on the image - void drawDetections(cv_bridge::CvImagePtr image); - - bool get_publish_tf() const { return publish_tf_; } }; -} // namespace apriltag_ros +} // namespace atlas_apriltag_ros -#endif // APRILTAG_ROS_COMMON_FUNCTIONS_H +#endif // ATLAS_APRILTAG_ROS_COMMON_FUNCTIONS_H diff --git a/apriltag_ros/include/apriltag_ros/composition_visibility.h b/apriltag_ros/include/apriltag_ros/composition_visibility.h new file mode 100644 index 00000000..20a0396e --- /dev/null +++ b/apriltag_ros/include/apriltag_ros/composition_visibility.h @@ -0,0 +1,58 @@ +// 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 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. + +#ifndef COMPOSITION__VISIBILITY_CONTROL_H_ +#define COMPOSITION__VISIBILITY_CONTROL_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 COMPOSITION_EXPORT __attribute__ ((dllexport)) + #define COMPOSITION_IMPORT __attribute__ ((dllimport)) + #else + #define COMPOSITION_EXPORT __declspec(dllexport) + #define COMPOSITION_IMPORT __declspec(dllimport) + #endif + #ifdef COMPOSITION_BUILDING_DLL + #define COMPOSITION_PUBLIC COMPOSITION_EXPORT + #else + #define COMPOSITION_PUBLIC COMPOSITION_IMPORT + #endif + #define COMPOSITION_PUBLIC_TYPE COMPOSITION_PUBLIC + #define COMPOSITION_LOCAL +#else + #define COMPOSITION_EXPORT __attribute__ ((visibility("default"))) + #define COMPOSITION_IMPORT + #if __GNUC__ >= 4 + #define COMPOSITION_PUBLIC __attribute__ ((visibility("default"))) + #define COMPOSITION_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define COMPOSITION_PUBLIC + #define COMPOSITION_LOCAL + #endif + #define COMPOSITION_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // COMPOSITION__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/apriltag_ros/include/apriltag_ros/continuous_detector.h b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp similarity index 57% rename from apriltag_ros/include/apriltag_ros/continuous_detector.h rename to apriltag_ros/include/apriltag_ros/continuous_detector.hpp index f7ae8bca..347f6827 100644 --- a/apriltag_ros/include/apriltag_ros/continuous_detector.h +++ b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp @@ -40,49 +40,55 @@ * Originator: Danylo Malyuta, JPL ******************************************************************************/ -#ifndef APRILTAG_ROS_CONTINUOUS_DETECTOR_H -#define APRILTAG_ROS_CONTINUOUS_DETECTOR_H - -#include "apriltag_ros/common_functions.h" +#ifndef ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H +#define ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H +// C++ #include #include -#include -#include -#include +#include "atlas_apriltag_ros/common_functions.hpp" +#include "atlas_apriltag_ros/composition_visibility.h" + +#include +#include +#include + +// Own +#include "ageve_interfaces/msg/april_tag_detection.hpp" +#include "ageve_interfaces/msg/april_tag_detection_array.hpp" -namespace apriltag_ros +namespace atlas_apriltag_ros { -class ContinuousDetector: public nodelet::Nodelet +class ContinuousDetector { - public: - ContinuousDetector() = default; - ~ContinuousDetector() = default; + public: + COMPOSITION_PUBLIC + explicit ContinuousDetector(const rclcpp::NodeOptions & options); - void onInit(); + COMPOSITION_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() const; - void imageCallback(const sensor_msgs::ImageConstPtr& image_rect, - const sensor_msgs::CameraInfoConstPtr& camera_info); + void ImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info); - void refreshTagParameters(); + rclcpp::Node::SharedPtr nh_; + rclcpp::QoS custom_qos_; - private: - std::mutex detection_mutex_; - std::shared_ptr tag_detector_; - bool draw_tag_detections_image_; - cv_bridge::CvImagePtr cv_image_; + std::mutex detection_mutex_; + std::shared_ptr tag_detector_; + bool draw_tag_detections_image_; + cv_bridge::CvImagePtr cv_image_; - std::shared_ptr it_; - image_transport::CameraSubscriber camera_image_subscriber_; - image_transport::Publisher tag_detections_image_publisher_; - ros::Publisher tag_detections_publisher_; + std::shared_ptr it_; + image_transport::CameraSubscriber camera_image_subscriber_; + image_transport::Publisher tag_detections_image_publisher_; + rclcpp::Publisher::SharedPtr tag_detections_publisher_; - ros::ServiceServer refresh_params_service_; - bool refreshParamsCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); }; -} // namespace apriltag_ros +} // namespace atlas_apriltag_ros -#endif // APRILTAG_ROS_CONTINUOUS_DETECTOR_H +#endif // ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H diff --git a/apriltag_ros/launch/continuous_detection.launch b/apriltag_ros/launch/continuous_detection.launch deleted file mode 100644 index 2fa9002c..00000000 --- a/apriltag_ros/launch/continuous_detection.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/apriltag_ros/launch/launch.py b/apriltag_ros/launch/launch.py new file mode 100644 index 00000000..2f593b9a --- /dev/null +++ b/apriltag_ros/launch/launch.py @@ -0,0 +1,83 @@ +""" + Copyright 2023 @ MOVVO ROBOTICS + --------------------------------------------------------- + Authors: Albert Arlà, Martí Bolet, Bernat Gaston, María Mercadé + Contact: support.idi@ageve.net +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, SetParameter, PushRosNamespace +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression + +PACKAGE_NAME = 'atlas_apriltag_ros' + +def generate_launch_description(): + # Configs files + config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "params.yaml") + tags_config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "tags.yaml") + + # Launch arguments + sim_time_arg = DeclareLaunchArgument( + name='use_sim_time', + default_value='False', + choices=['True', 'False'], + description='Parameter use_sim_time') + + valgrind_arg = DeclareLaunchArgument( + name='valgrind', + default_value='False', + choices=['True', 'False'], + description='Debug with valgrind') + + # Launch arguments + sim_time = LaunchConfiguration('use_sim_time') + valgrind = LaunchConfiguration('valgrind') + + remaps=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] + + # Nodes to launch + node = Node( + package=PACKAGE_NAME, + executable='ContinuousDetector', + arguments=['--ros-args', '--log-level', 'info'], + parameters=[config_file], + remappings=remaps, + output="screen", + emulate_tty=True, + condition=IfCondition(PythonExpression(['not ', valgrind])) + ) + + node_with_valgrind = Node( + package=PACKAGE_NAME, + executable='ContinuousDetector', + arguments=['--ros-args', '--log-level', 'info'], + prefix=["valgrind --leak-check=yes -s"], + parameters=[config_file], + remappings=remaps, + output="screen", + emulate_tty=True, + condition=IfCondition(valgrind) + ) + + return LaunchDescription([ + GroupAction( + actions=[ + PushRosNamespace( + os.getenv('ATLAS_NAMESPACE') if 'ATLAS_NAMESPACE' in os.environ.keys() else "" + ), + sim_time_arg, + valgrind_arg, + SetParameter("use_sim_time", sim_time), + SetParameter("tags_yaml_path", tags_config_file), + node, + node_with_valgrind + ] + ) + ]) diff --git a/apriltag_ros/launch/single_image_client.launch b/apriltag_ros/launch/single_image_client.launch deleted file mode 100644 index 1af41f8c..00000000 --- a/apriltag_ros/launch/single_image_client.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/apriltag_ros/launch/single_image_server.launch b/apriltag_ros/launch/single_image_server.launch deleted file mode 100644 index d26989c6..00000000 --- a/apriltag_ros/launch/single_image_server.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/apriltag_ros/nodelet_plugins.xml b/apriltag_ros/nodelet_plugins.xml deleted file mode 100644 index 14051117..00000000 --- a/apriltag_ros/nodelet_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Continuous AprilTag 3 detector - - - diff --git a/apriltag_ros/package.xml b/apriltag_ros/package.xml index da33a776..99558e8e 100644 --- a/apriltag_ros/package.xml +++ b/apriltag_ros/package.xml @@ -1,63 +1,40 @@ - - apriltag_ros - 3.2.1 - - A ROS wrapper of the AprilTag 3 visual fiducial detection - algorithm. Provides full access to the core AprilTag 3 algorithm's - customizations and makes the tag detection image and detected tags' poses - available over ROS topics (including tf). The core AprilTag 3 algorithm is - extended to allow the detection of tag bundles and a bundle calibration - script is provided (bundle detection is more accurate than single tag - detection). Continuous (camera image stream) and single image detector nodes - are available. - + + + atlas_apriltag_ros + 0.0.0 + utils package + + AlbertArla, + MartiBolet, + BernatGaston, + MariaMercade + - Danylo Malyuta - Wolfgang Merkt - - Danylo Malyuta - - BSD - - http://www.ros.org/wiki/apriltag_ros - https://github.com/AprilRobotics/apriltag_ros/issues - https://github.com/AprilRobotics/apriltag_ros + Proprietary MOVVO ROBOTICS. All rights reserved + + ament_cmake + ament_cmake_python - catkin + rclcpp + rclcpp_components + ageve_interfaces + cv_bridge + image_transport + rcutils + sensor_msgs + tf2_ros + tf2 + apriltag + eigen + OpenCV + image_geometry + geometry_msgs - apriltag - geometry_msgs - image_transport - image_geometry - roscpp - sensor_msgs - message_generation - std_msgs - cv_bridge - tf - nodelet - pluginlib - eigen - libopencv-dev - cmake_modules - - tf - apriltag - geometry_msgs - image_transport - image_geometry - roscpp - sensor_msgs - message_runtime - std_msgs - cv_bridge - nodelet - pluginlib - eigen - libopencv-dev + ament_lint_auto + ament_lint_common - + ament_cmake - + \ No newline at end of file diff --git a/apriltag_ros/src/apriltag_ros_continuous_node.cpp b/apriltag_ros/src/apriltag_ros_continuous_node.cpp deleted file mode 100644 index add67d22..00000000 --- a/apriltag_ros/src/apriltag_ros_continuous_node.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * Copyright (c) 2017, California Institute of Technology. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * The views and conclusions contained in the software and documentation are - * those of the authors and should not be interpreted as representing official - * policies, either expressed or implied, of the California Institute of - * Technology. - */ - -#include - -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "apriltag_ros"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - nodelet.load(ros::this_node::getName(), - "apriltag_ros/ContinuousDetector", - remap, nargv); - - ros::spin(); - return 0; -} diff --git a/apriltag_ros/src/common_functions.cpp b/apriltag_ros/src/common_functions.cpp index ed390d19..67484121 100644 --- a/apriltag_ros/src/common_functions.cpp +++ b/apriltag_ros/src/common_functions.cpp @@ -29,466 +29,535 @@ * Technology. */ -#include "apriltag_ros/common_functions.h" +#include "atlas_apriltag_ros/common_functions.hpp" #include "image_geometry/pinhole_camera_model.h" -#include "common/homography.h" -#include "tagStandard52h13.h" -#include "tagStandard41h12.h" -#include "tag36h11.h" -#include "tag25h9.h" -#include "tag16h5.h" -#include "tagCustom48h12.h" -#include "tagCircle21h7.h" -#include "tagCircle49h12.h" - -namespace apriltag_ros -{ +#include "apriltag/common/homography.h" +#include "apriltag/tagStandard52h13.h" +#include "apriltag/tagStandard41h12.h" +#include "apriltag/tag36h11.h" +#include "apriltag/tag25h9.h" +#include "apriltag/tag16h5.h" +#include "apriltag/tagCustom48h12.h" +#include "apriltag/tagCircle21h7.h" +#include "apriltag/tagCircle49h12.h" + +using namespace atlas_apriltag_ros; -TagDetector::TagDetector(ros::NodeHandle pnh) : - family_(getAprilTagOption(pnh, "tag_family", "tag36h11")), - threads_(getAprilTagOption(pnh, "tag_threads", 0)), - decimate_(getAprilTagOption(pnh, "tag_decimate", 1.0)), - blur_(getAprilTagOption(pnh, "tag_blur", 0.0)), - refine_edges_(getAprilTagOption(pnh, "tag_refine_edges", 1)), - debug_(getAprilTagOption(pnh, "tag_debug", 0)), - max_hamming_distance_(getAprilTagOption(pnh, "max_hamming_dist", 2)), - publish_tf_(getAprilTagOption(pnh, "publish_tf", false)) + +TagDetector::TagDetector(rclcpp::Node::SharedPtr nh) { - // Parse standalone tag descriptions specified by user (stored on ROS - // parameter server) - XmlRpc::XmlRpcValue standalone_tag_descriptions; - if(!pnh.getParam("standalone_tags", standalone_tag_descriptions)) - { - ROS_WARN("No april tags specified"); - } - else - { - try + nh_ = nh; + + // Declare and get parameters + family_ = nh->declare_parameter("tag_family", "tag36h11"); + threads_ = nh->declare_parameter("tag_threads", 0); + decimate_ = nh->declare_parameter("tag_decimate", 1.0); + blur_ = nh->declare_parameter("tag_blur", 0.0); + refine_edges_ = nh->declare_parameter("tag_refine_edges", 1); + debug_ = nh->declare_parameter("tag_debug", 0); + max_hamming_distance_ = nh->declare_parameter("max_hamming_dist", 2); + publish_tf_ = nh->declare_parameter("publish_tf", false); + remove_duplicates_ = nh_->declare_parameter("remove_duplicates", true); + std::string tags_yaml = nh_->declare_parameter("tags_yaml_path", ""); + + + standalone_tag_descriptions_ = parse_standalone_tags_from_yaml(tags_yaml); + + tag_bundle_descriptions_ = parse_tag_bundles_from_yaml(tags_yaml); + + // Initialize the transform broadcaster + tf_pub_ = std::make_unique(*nh_); + + // Define the tag family whose tags should be searched for in the camera + // images + if (family_ == "tagStandard52h13") { - standalone_tag_descriptions_ = - parseStandaloneTags(standalone_tag_descriptions); + tf_ = tagStandard52h13_create(); } - catch(XmlRpc::XmlRpcException e) + else if (family_ == "tagStandard41h12") { - // in case any of the asserts in parseStandaloneTags() fail - ROS_ERROR_STREAM("Error loading standalone tag descriptions: " << - e.getMessage().c_str()); + tf_ = tagStandard41h12_create(); } - } - - // parse tag bundle descriptions specified by user (stored on ROS parameter - // server) - XmlRpc::XmlRpcValue tag_bundle_descriptions; - if(!pnh.getParam("tag_bundles", tag_bundle_descriptions)) - { - ROS_WARN("No tag bundles specified"); - } - else - { - try + else if (family_ == "tag36h11") { - tag_bundle_descriptions_ = parseTagBundles(tag_bundle_descriptions); + tf_ = tag36h11_create(); } - catch(XmlRpc::XmlRpcException e) + else if (family_ == "tag25h9") { - // In case any of the asserts in parseStandaloneTags() fail - ROS_ERROR_STREAM("Error loading tag bundle descriptions: " << - e.getMessage().c_str()); + tf_ = tag25h9_create(); + } + else if (family_ == "tag16h5") + { + tf_ = tag16h5_create(); + } + else if (family_ == "tagCustom48h12") + { + tf_ = tagCustom48h12_create(); + } + else if (family_ == "tagCircle21h7") + { + tf_ = tagCircle21h7_create(); + } + else if (family_ == "tagCircle49h12") + { + tf_ = tagCircle49h12_create(); + } + else + { + RCLCPP_ERROR(nh_->get_logger(),"Invalid tag family specified! Aborting"); + exit(1); } - } - - // Optionally remove duplicate detections in scene. Defaults to removing - if(!pnh.getParam("remove_duplicates", remove_duplicates_)) - { - ROS_WARN("remove_duplicates parameter not provided. Defaulting to true"); - remove_duplicates_ = true; - } - - // Define the tag family whose tags should be searched for in the camera - // images - if (family_ == "tagStandard52h13") - { - tf_ = tagStandard52h13_create(); - } - else if (family_ == "tagStandard41h12") - { - tf_ = tagStandard41h12_create(); - } - else if (family_ == "tag36h11") - { - tf_ = tag36h11_create(); - } - else if (family_ == "tag25h9") - { - tf_ = tag25h9_create(); - } - else if (family_ == "tag16h5") - { - tf_ = tag16h5_create(); - } - else if (family_ == "tagCustom48h12") - { - tf_ = tagCustom48h12_create(); - } - else if (family_ == "tagCircle21h7") - { - tf_ = tagCircle21h7_create(); - } - else if (family_ == "tagCircle49h12") - { - tf_ = tagCircle49h12_create(); - } - else - { - ROS_WARN("Invalid tag family specified! Aborting"); - exit(1); - } - if (threads_ == 0) - { - threads_ = std::max(std::thread::hardware_concurrency() - 1U, 1U); - ROS_INFO("Thread count not specified. Using %d threads", threads_); - } + if (threads_ == 0) + { + threads_ = std::max(std::thread::hardware_concurrency() - 1U, 1U); + RCLCPP_WARN(nh_->get_logger(), "Thread count not specified. Using %d threads", threads_); + } - // Create the AprilTag 2 detector - td_ = apriltag_detector_create(); - apriltag_detector_add_family_bits(td_, tf_, max_hamming_distance_); - td_->quad_decimate = (float)decimate_; - td_->quad_sigma = (float)blur_; - td_->nthreads = threads_; - td_->debug = debug_; - td_->refine_edges = refine_edges_; + // Create the AprilTag 2 detector + td_ = apriltag_detector_create(); + apriltag_detector_add_family_bits(td_, tf_, max_hamming_distance_); + td_->quad_decimate = (float)decimate_; + td_->quad_sigma = (float)blur_; + td_->nthreads = threads_; + td_->debug = debug_; + td_->refine_edges = refine_edges_; - detections_ = NULL; + detections_ = NULL; } // destructor TagDetector::~TagDetector() { - // free memory associated with tag detector - apriltag_detector_destroy(td_); + // free memory associated with tag detector + apriltag_detector_destroy(td_); - // Free memory associated with the array of tag detections - if(detections_) - { - apriltag_detections_destroy(detections_); - } + // Free memory associated with the array of tag detections + if(detections_) + { + apriltag_detections_destroy(detections_); + } - // free memory associated with tag family - if (family_ == "tagStandard52h13") - { - tagStandard52h13_destroy(tf_); - } - else if (family_ == "tagStandard41h12") - { - tagStandard41h12_destroy(tf_); - } - else if (family_ == "tag36h11") - { - tag36h11_destroy(tf_); - } - else if (family_ == "tag25h9") - { - tag25h9_destroy(tf_); - } - else if (family_ == "tag16h5") - { - tag16h5_destroy(tf_); - } - else if (family_ == "tagCustom48h12") - { - tagCustom48h12_destroy(tf_); - } - else if (family_ == "tagCircle21h7") - { - tagCircle21h7_destroy(tf_); - } - else if (family_ == "tagCircle49h12") - { - tagCircle49h12_destroy(tf_); - } + // free memory associated with tag family + if (family_ == "tagStandard52h13") + { + tagStandard52h13_destroy(tf_); + } + else if (family_ == "tagStandard41h12") + { + tagStandard41h12_destroy(tf_); + } + else if (family_ == "tag36h11") + { + tag36h11_destroy(tf_); + } + else if (family_ == "tag25h9") + { + tag25h9_destroy(tf_); + } + else if (family_ == "tag16h5") + { + tag16h5_destroy(tf_); + } + else if (family_ == "tagCustom48h12") + { + tagCustom48h12_destroy(tf_); + } + else if (family_ == "tagCircle21h7") + { + tagCircle21h7_destroy(tf_); + } + else if (family_ == "tagCircle49h12") + { + tagCircle49h12_destroy(tf_); + } } -AprilTagDetectionArray TagDetector::detectTags ( - const cv_bridge::CvImagePtr& image, - const sensor_msgs::CameraInfoConstPtr& camera_info) { - // Convert image to AprilTag code's format - cv::Mat gray_image; - if (image->image.channels() == 1) - { - gray_image = image->image; - } - else - { - cv::cvtColor(image->image, gray_image, CV_BGR2GRAY); - } - image_u8_t apriltag_image = { .width = gray_image.cols, - .height = gray_image.rows, - .stride = gray_image.cols, - .buf = gray_image.data - }; +std::map TagDetector::parse_standalone_tags_from_yaml(const std::string& yaml_string) +{ + // Get the root node of the YAML document. + YAML::Node root = YAML::LoadFile(yaml_string); - image_geometry::PinholeCameraModel camera_model; - camera_model.fromCameraInfo(camera_info); + // Get the `standalone_tags` node. + YAML::Node standalone_tags_node = root["standalone_tags"]; - // Get camera intrinsic properties for rectified image. - double fx = camera_model.fx(); // focal length in camera x-direction [px] - double fy = camera_model.fy(); // focal length in camera y-direction [px] - double cx = camera_model.cx(); // optical center x-coordinate [px] - double cy = camera_model.cy(); // optical center y-coordinate [px] + // Create a map to store the parsed standalone tag descriptions. + std::map standalone_tag_descriptions; - ROS_INFO_STREAM_ONCE("Camera model: fx = " << fx << ", fy = " << fy << ", cx = " << cx << ", cy = " << cy); + // Iterate over the `standalone_tags` node and parse each tag description. + for (YAML::Node tag_node : standalone_tags_node) { + // Get the tag ID. + int id = tag_node["id"].as(); - // Check if camera intrinsics are not available - if not the calculated - // transforms are meaningless. - if (fx == 0 && fy == 0) ROS_WARN_STREAM_THROTTLE(5, "fx and fy are zero. Are the camera intrinsics set?"); + // Get the tag size. + double size = tag_node["size"].as(); - // Run AprilTag 2 algorithm on the image - if (detections_) - { - apriltag_detections_destroy(detections_); - detections_ = NULL; - } - detections_ = apriltag_detector_detect(td_, &apriltag_image); + // Get the tag name + std::string name = tag_node["name"].as(); - // If remove_duplicates_ is set to true, then duplicate tags are not allowed. - // Thus any duplicate tag IDs visible in the scene must include at least 1 - // erroneous detection. Remove any tags with duplicate IDs to ensure removal - // of these erroneous detections - if (remove_duplicates_) - { - removeDuplicates(); - } + // Create a new `StandaloneTagDescription` object. + StandaloneTagDescription tag_description(id, size, name); - // Compute the estimated translation and rotation individually for each - // detected tag - AprilTagDetectionArray tag_detection_array; - std::vector detection_names; - tag_detection_array.header = image->header; - std::map > bundleObjectPoints; - std::map > bundleImagePoints; - for (int i=0; i < zarray_size(detections_); i++) - { - // Get the i-th detected tag - apriltag_detection_t *detection; - zarray_get(detections_, i, &detection); - - // Bootstrap this for loop to find this tag's description amongst - // the tag bundles. If found, add its points to the bundle's set of - // object-image corresponding points (tag corners) for cv::solvePnP. - // Don't yet run cv::solvePnP on the bundles, though, since we're still in - // the process of collecting all the object-image corresponding points - int tagID = detection->id; - bool is_part_of_bundle = false; - for (unsigned int j=0; j TagDetector::parse_tag_bundles_from_yaml(const std::string& yaml_string) +{ + // Get the root node of the YAML document. + YAML::Node root = YAML::LoadFile(yaml_string); + + // Get the `tag_bundles` node. + YAML::Node tag_bundles_node = root["tag_bundles"]; + + // Create a vector to store the parsed tag bundle descriptions. + std::vector tag_bundle_descriptions; + + // Iterate over the `tag_bundles` node and parse each tag bundle description. + for (YAML::Node tag_bundle_node : tag_bundles_node) { + // Get the tag bundle name. + std::string name = tag_bundle_node["name"].as(); + + // Create a new `TagBundleDescription` object. + TagBundleDescription tag_bundle_description(name); + + // Get the `layout` node. + YAML::Node layout_node = tag_bundle_node["layout"]; + + // Iterate over the `layout` node and parse each tag member. + for (YAML::Node tag_member_node : layout_node) { + // Get the tag member ID. + int id = tag_member_node["id"].as(); + + // Get the tag member size. + double size = tag_member_node["size"].as(); + + double x = tag_member_node["x"].as(); + double y = tag_member_node["y"].as(); + double z = tag_member_node["z"].as(); + double qw = tag_member_node["qw"].as(); + double qx = tag_member_node["qx"].as(); + double qy = tag_member_node["qy"].as(); + double qz = tag_member_node["qz"].as(); + + Eigen::Quaterniond q_tag(qw, qx, qy, qz); + q_tag.normalize(); + Eigen::Matrix3d R_oi = q_tag.toRotationMatrix(); + + // Build the rigid transform from tag_j to the bundle origin + cv::Matx44d T_oi(R_oi(0,0), R_oi(0,1), R_oi(0,2), x, + R_oi(1,0), R_oi(1,1), R_oi(1,2), y, + R_oi(2,0), R_oi(2,1), R_oi(2,2), z, + 0, 0, 0, 1); + + // Add the tag member to the bundle description. + tag_bundle_description.addMemberTag(id, size, T_oi); + } + + // Add the tag bundle description to the vector. + tag_bundle_descriptions.push_back(tag_bundle_description); + } + + // Return the vector of tag bundle descriptions. + return tag_bundle_descriptions; +} + +ageve_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( + const cv_bridge::CvImagePtr& image, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info) { + // Convert image to AprilTag code's format + cv::Mat gray_image; + if (image->image.channels() == 1) { - // Iterate over the registered bundles - TagBundleDescription bundle = tag_bundle_descriptions_[j]; - - if (bundle.id2idx_.find(tagID) != bundle.id2idx_.end()) - { - // This detected tag belongs to the j-th tag bundle (its ID was found in - // the bundle description) - is_part_of_bundle = true; - std::string bundleName = bundle.name(); - - //===== Corner points in the world frame coordinates - double s = bundle.memberSize(tagID)/2; - addObjectPoints(s, bundle.memberT_oi(tagID), - bundleObjectPoints[bundleName]); - - //===== Corner points in the image frame coordinates - addImagePoints(detection, bundleImagePoints[bundleName]); - } - } - - // Find this tag's description amongst the standalone tags - // Print warning when a tag was found that is neither part of a - // bundle nor standalone (thus it is a tag in the environment - // which the user specified no description for, or Apriltags - // misdetected a tag (bad ID or a false positive)). - StandaloneTagDescription* standaloneDescription; - if (!findStandaloneTagDescription(tagID, standaloneDescription, - !is_part_of_bundle)) + gray_image = image->image; + } + else { - continue; + cv::cvtColor(image->image, gray_image, CV_BGR2GRAY); } + image_u8_t apriltag_image = { .width = gray_image.cols, + .height = gray_image.rows, + .stride = gray_image.cols, + .buf = gray_image.data + }; - //================================================================= - // The remainder of this for loop is concerned with standalone tag - // poses! - double tag_size = standaloneDescription->size(); - - // Get estimated tag pose in the camera frame. - // - // Note on frames: - // The raw AprilTag 2 uses the following frames: - // - camera frame: looking from behind the camera (like a - // photographer), x is right, y is up and z is towards you - // (i.e. the back of camera) - // - tag frame: looking straight at the tag (oriented correctly), - // x is right, y is down and z is away from you (into the tag). - // But we want: - // - camera frame: looking from behind the camera (like a - // photographer), x is right, y is down and z is straight - // ahead - // - tag frame: looking straight at the tag (oriented correctly), - // x is right, y is up and z is towards you (out of the tag). - // Using these frames together with cv::solvePnP directly avoids - // AprilTag 2's frames altogether. - // TODO solvePnP[Ransac] better? - std::vector standaloneTagObjectPoints; - std::vector standaloneTagImagePoints; - addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints); - addImagePoints(detection, standaloneTagImagePoints); - Eigen::Isometry3d transform = getRelativeTransform(standaloneTagObjectPoints, - standaloneTagImagePoints, - fx, fy, cx, cy); - geometry_msgs::PoseWithCovarianceStamped tag_pose = - makeTagPose(transform, image->header); - - // Add the detection to the back of the tag detection array - AprilTagDetection tag_detection; - tag_detection.pose = tag_pose; - tag_detection.id.push_back(detection->id); - tag_detection.size.push_back(tag_size); - tag_detection_array.detections.push_back(tag_detection); - detection_names.push_back(standaloneDescription->frame_name()); - } + image_geometry::PinholeCameraModel camera_model; + camera_model.fromCameraInfo(camera_info); - //================================================================= - // Estimate bundle origin pose for each bundle in which at least one - // member tag was detected + // Get camera intrinsic properties for rectified image. + double fx = camera_model.fx(); // focal length in camera x-direction [px] + double fy = camera_model.fy(); // focal length in camera y-direction [px] + double cx = camera_model.cx(); // optical center x-coordinate [px] + double cy = camera_model.cy(); // optical center y-coordinate [px] - for (unsigned int j=0; jget_logger(), "Camera model: fx = %f, fy = %f, cx = %f, cy = %f", fx, fy, cx, cy); + + // Check if camera intrinsics are not available - if not the calculated + // transforms are meaningless. + if (fx == 0 && fy == 0) RCLCPP_WARN(nh_->get_logger(), "fx and fy are zero. Are the camera intrinsics set?"); - std::map >::iterator it = - bundleObjectPoints.find(bundleName); - if (it != bundleObjectPoints.end()) + // Run AprilTag 2 algorithm on the image + if (detections_) { - // Some member tags of this bundle were detected, get the bundle's - // position! - TagBundleDescription& bundle = tag_bundle_descriptions_[j]; - - Eigen::Isometry3d transform = - getRelativeTransform(bundleObjectPoints[bundleName], - bundleImagePoints[bundleName], fx, fy, cx, cy); - geometry_msgs::PoseWithCovarianceStamped bundle_pose = - makeTagPose(transform, image->header); - - // Add the detection to the back of the tag detection array - AprilTagDetection tag_detection; - tag_detection.pose = bundle_pose; - tag_detection.id = bundle.bundleIds(); - tag_detection.size = bundle.bundleSizes(); - tag_detection_array.detections.push_back(tag_detection); - detection_names.push_back(bundle.name()); + apriltag_detections_destroy(detections_); + detections_ = NULL; } - } + detections_ = apriltag_detector_detect(td_, &apriltag_image); - // If set, publish the transform /tf topic - if (publish_tf_) { - for (unsigned int i=0; i tag_transform; - tf::poseStampedMsgToTF(pose, tag_transform); - tf_pub_.sendTransform(tf::StampedTransform(tag_transform, - tag_transform.stamp_, - image->header.frame_id, - detection_names[i])); + // If remove_duplicates_ is set to true, then duplicate tags are not allowed. + // Thus any duplicate tag IDs visible in the scene must include at least 1 + // erroneous detection. Remove any tags with duplicate IDs to ensure removal + // of these erroneous detections + if (remove_duplicates_) + { + removeDuplicates(); } - } - return tag_detection_array; + // Compute the estimated translation and rotation individually for each + // detected tag + ageve_interfaces::msg::AprilTagDetectionArray tag_detection_array; + std::vector detection_names; + tag_detection_array.header = image->header; + std::map > bundleObjectPoints; + std::map > bundleImagePoints; + for (int i=0; i < zarray_size(detections_); i++) + { + // Get the i-th detected tag + apriltag_detection_t *detection; + zarray_get(detections_, i, &detection); + + // Bootstrap this for loop to find this tag's description amongst + // the tag bundles. If found, add its points to the bundle's set of + // object-image corresponding points (tag corners) for cv::solvePnP. + // Don't yet run cv::solvePnP on the bundles, though, since we're still in + // the process of collecting all the object-image corresponding points + int tagID = detection->id; + bool is_part_of_bundle = false; + for (unsigned int j=0; jsize(); + + // Get estimated tag pose in the camera frame. + // + // Note on frames: + // The raw AprilTag 2 uses the following frames: + // - camera frame: looking from behind the camera (like a + // photographer), x is right, y is up and z is towards you + // (i.e. the back of camera) + // - tag frame: looking straight at the tag (oriented correctly), + // x is right, y is down and z is away from you (into the tag). + // But we want: + // - camera frame: looking from behind the camera (like a + // photographer), x is right, y is down and z is straight + // ahead + // - tag frame: looking straight at the tag (oriented correctly), + // x is right, y is up and z is towards you (out of the tag). + // Using these frames together with cv::solvePnP directly avoids + // AprilTag 2's frames altogether. + // TODO solvePnP[Ransac] better? + std::vector standaloneTagObjectPoints; + std::vector standaloneTagImagePoints; + addObjectPoints(tag_size/2, cv::Matx44d::eye(), standaloneTagObjectPoints); + addImagePoints(detection, standaloneTagImagePoints); + Eigen::Isometry3d transform = getRelativeTransform(standaloneTagObjectPoints, + standaloneTagImagePoints, + fx, fy, cx, cy); + geometry_msgs::msg::PoseWithCovarianceStamped tag_pose = + makeTagPose(transform, image->header); + + // Add the detection to the back of the tag detection array + ageve_interfaces::msg::AprilTagDetection tag_detection; + tag_detection.pose = tag_pose; + tag_detection.id.push_back(detection->id); + tag_detection.size.push_back(tag_size); + tag_detection_array.detections.push_back(tag_detection); + detection_names.push_back(standaloneDescription->frame_name()); + } + + //================================================================= + // Estimate bundle origin pose for each bundle in which at least one + // member tag was detected + + for (unsigned int j=0; j >::iterator it = + bundleObjectPoints.find(bundleName); + if (it != bundleObjectPoints.end()) + { + // Some member tags of this bundle were detected, get the bundle's + // position! + TagBundleDescription& bundle = tag_bundle_descriptions_[j]; + + Eigen::Isometry3d transform = + getRelativeTransform(bundleObjectPoints[bundleName], + bundleImagePoints[bundleName], fx, fy, cx, cy); + geometry_msgs::msg::PoseWithCovarianceStamped bundle_pose = + makeTagPose(transform, image->header); + + // Add the detection to the back of the tag detection array + ageve_interfaces::msg::AprilTagDetection tag_detection; + tag_detection.pose = bundle_pose; + tag_detection.id = bundle.bundleIds(); + tag_detection.size = bundle.bundleSizes(); + tag_detection_array.detections.push_back(tag_detection); + detection_names.push_back(bundle.name()); + } + } + + // If set, publish the transform /tf topic + if (publish_tf_) { + for (unsigned int i=0; isendTransform(tag_transform); + // tf_pub_->sendTransform(tf::StampedTransform(tag_transform, + // tag_transform.stamp_, + // image->header.frame_id, + // detection_names[i])); + } + } + + return tag_detection_array; } int TagDetector::idComparison (const void* first, const void* second) { - int id1 = (*(apriltag_detection_t**)first)->id; - int id2 = (*(apriltag_detection_t**)second)->id; - return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1); + int id1 = (*(apriltag_detection_t**)first)->id; + int id2 = (*(apriltag_detection_t**)second)->id; + return (id1 < id2) ? -1 : ((id1 == id2) ? 0 : 1); } void TagDetector::removeDuplicates () { - zarray_sort(detections_, &idComparison); - int count = 0; - bool duplicate_detected = false; - while (true) - { - if (count > zarray_size(detections_)-1) - { - // The entire detection set was parsed - return; - } - apriltag_detection_t *next_detection, *current_detection; - zarray_get(detections_, count, ¤t_detection); - int id_current = current_detection->id; - // Default id_next value of -1 ensures that if the last detection - // is a duplicated tag ID, it will get removed - int id_next = -1; - if (count < zarray_size(detections_)-1) + zarray_sort(detections_, &idComparison); + int count = 0; + bool duplicate_detected = false; + while (true) { - zarray_get(detections_, count+1, &next_detection); - id_next = next_detection->id; + if (count > zarray_size(detections_)-1) + { + // The entire detection set was parsed + return; + } + apriltag_detection_t *next_detection, *current_detection; + zarray_get(detections_, count, ¤t_detection); + int id_current = current_detection->id; + // Default id_next value of -1 ensures that if the last detection + // is a duplicated tag ID, it will get removed + int id_next = -1; + if (count < zarray_size(detections_)-1) + { + zarray_get(detections_, count+1, &next_detection); + id_next = next_detection->id; + } + if (id_current == id_next || (id_current != id_next && duplicate_detected)) + { + duplicate_detected = true; + // Remove the current tag detection from detections array + int shuffle = 0; + apriltag_detection_destroy(current_detection); + zarray_remove_index(detections_, count, shuffle); + if (id_current != id_next) + { + RCLCPP_WARN(nh_->get_logger(), "Pruning tag ID %d because it appears more than once in the image.", id_current); + duplicate_detected = false; // Reset + } + continue; + } + else + { + count++; + } } - if (id_current == id_next || (id_current != id_next && duplicate_detected)) - { - duplicate_detected = true; - // Remove the current tag detection from detections array - int shuffle = 0; - apriltag_detection_destroy(current_detection); - zarray_remove_index(detections_, count, shuffle); - if (id_current != id_next) - { - ROS_WARN_STREAM("Pruning tag ID " << id_current << " because it " - "appears more than once in the image."); - duplicate_detected = false; // Reset - } - continue; - } - else - { - count++; - } - } } void TagDetector::addObjectPoints ( double s, cv::Matx44d T_oi, std::vector& objectPoints) const { - // Add to object point vector the tag corner coordinates in the bundle frame - // Going counterclockwise starting from the bottom left corner - objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1)); - objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1)); - objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1)); - objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1)); + // Add to object point vector the tag corner coordinates in the bundle frame + // Going counterclockwise starting from the bottom left corner + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s,-s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s,-s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d( s, s, 0, 1)); + objectPoints.push_back(T_oi.get_minor<3, 4>(0, 0)*cv::Vec4d(-s, s, 0, 1)); } void TagDetector::addImagePoints ( apriltag_detection_t *detection, std::vector& imagePoints) const { - // Add to image point vector the tag corners in the image frame - // Going counterclockwise starting from the bottom left corner - double tag_x[4] = {-1,1,1,-1}; - double tag_y[4] = {1,1,-1,-1}; // Negated because AprilTag tag local - // frame has y-axis pointing DOWN - // while we use the tag local frame - // with y-axis pointing UP - for (int i=0; i<4; i++) - { - // Homography projection taking tag local frame coordinates to image pixels - double im_x, im_y; - homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y); - imagePoints.push_back(cv::Point2d(im_x, im_y)); - } + // Add to image point vector the tag corners in the image frame + // Going counterclockwise starting from the bottom left corner + double tag_x[4] = {-1,1,1,-1}; + double tag_y[4] = {1,1,-1,-1}; // Negated because AprilTag tag local + // frame has y-axis pointing DOWN + // while we use the tag local frame + // with y-axis pointing UP + for (int i=0; i<4; i++) + { + // Homography projection taking tag local frame coordinates to image pixels + double im_x, im_y; + homography_project(detection->H, tag_x[i], tag_y[i], &im_x, &im_y); + imagePoints.push_back(cv::Point2d(im_x, im_y)); + } } Eigen::Isometry3d TagDetector::getRelativeTransform( @@ -496,289 +565,110 @@ Eigen::Isometry3d TagDetector::getRelativeTransform( const std::vector& imagePoints, double fx, double fy, double cx, double cy) const { - Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // homogeneous transformation matrix - - // perform Perspective-n-Point camera pose estimation using the - // above 3D-2D point correspondences - cv::Mat rvec, tvec; - cv::Matx33d cameraMatrix(fx, 0, cx, - 0, fy, cy, - 0, 0, 1); - cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients - // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would - // need to first check WHAT is a bottleneck in this code, and only - // do this if PnP solution is the bottleneck. - cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); - cv::Matx33d R; - cv::Rodrigues(rvec, R); - - // rotation - T.linear() << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2); - - // translation - T.translation() = Eigen::Vector3d::Map(reinterpret_cast(tvec.data)); - - return T; + Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // homogeneous transformation matrix + + // perform Perspective-n-Point camera pose estimation using the + // above 3D-2D point correspondences + cv::Mat rvec, tvec; + cv::Matx33d cameraMatrix(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + cv::Vec4f distCoeffs(0,0,0,0); // distortion coefficients + // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would + // need to first check WHAT is a bottleneck in this code, and only + // do this if PnP solution is the bottleneck. + cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); + cv::Matx33d R; + cv::Rodrigues(rvec, R); + + // rotation + T.linear() << R(0,0), R(0,1), R(0,2), R(1,0), R(1,1), R(1,2), R(2,0), R(2,1), R(2,2); + + // translation + T.translation() = Eigen::Vector3d::Map(reinterpret_cast(tvec.data)); + + return T; } -geometry_msgs::PoseWithCovarianceStamped TagDetector::makeTagPose( +geometry_msgs::msg::PoseWithCovarianceStamped TagDetector::makeTagPose( const Eigen::Isometry3d& transform, - const std_msgs::Header& header) + const std_msgs::msg::Header& header) { - geometry_msgs::PoseWithCovarianceStamped pose; - pose.header = header; - Eigen::Quaterniond rot_quaternion(transform.linear()); - //===== Position and orientation - pose.pose.pose.position.x = transform.translation().x(); - pose.pose.pose.position.y = transform.translation().y(); - pose.pose.pose.position.z = transform.translation().z(); - pose.pose.pose.orientation.x = rot_quaternion.x(); - pose.pose.pose.orientation.y = rot_quaternion.y(); - pose.pose.pose.orientation.z = rot_quaternion.z(); - pose.pose.pose.orientation.w = rot_quaternion.w(); - return pose; + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header = header; + Eigen::Quaterniond rot_quaternion(transform.linear()); + //===== Position and orientation + pose.pose.pose.position.x = transform.translation().x(); + pose.pose.pose.position.y = transform.translation().y(); + pose.pose.pose.position.z = transform.translation().z(); + pose.pose.pose.orientation.x = rot_quaternion.x(); + pose.pose.pose.orientation.y = rot_quaternion.y(); + pose.pose.pose.orientation.z = rot_quaternion.z(); + pose.pose.pose.orientation.w = rot_quaternion.w(); + return pose; } void TagDetector::drawDetections (cv_bridge::CvImagePtr image) { - for (int i = 0; i < zarray_size(detections_); i++) - { - apriltag_detection_t *det; - zarray_get(detections_, i, &det); - - // Check if this ID is present in config/tags.yaml - // Check if is part of a tag bundle - int tagID = det->id; - bool is_part_of_bundle = false; - for (unsigned int j=0; jimage, cv::Point((int)det->p[0][0], (int)det->p[0][1]), - cv::Point((int)det->p[1][0], (int)det->p[1][1]), - cv::Scalar(0, 0xff, 0)); // green - line(image->image, cv::Point((int)det->p[0][0], (int)det->p[0][1]), - cv::Point((int)det->p[3][0], (int)det->p[3][1]), - cv::Scalar(0, 0, 0xff)); // red - line(image->image, cv::Point((int)det->p[1][0], (int)det->p[1][1]), - cv::Point((int)det->p[2][0], (int)det->p[2][1]), - cv::Scalar(0xff, 0, 0)); // blue - line(image->image, cv::Point((int)det->p[2][0], (int)det->p[2][1]), - cv::Point((int)det->p[3][0], (int)det->p[3][1]), - cv::Scalar(0xff, 0, 0)); // blue - - // Print tag ID in the middle of the tag - std::stringstream ss; - ss << det->id; - cv::String text = ss.str(); - int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX; - double fontscale = 0.5; - int baseline; - cv::Size textsize = cv::getTextSize(text, fontface, - fontscale, 2, &baseline); - cv::putText(image->image, text, - cv::Point((int)(det->c[0]-textsize.width/2), - (int)(det->c[1]+textsize.height/2)), - fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2); - } -} - -// Parse standalone tag descriptions -std::map TagDetector::parseStandaloneTags ( - XmlRpc::XmlRpcValue& standalone_tags) -{ - // Create map that will be filled by the function and returned in the end - std::map descriptions; - // Ensure the type is correct - ROS_ASSERT(standalone_tags.getType() == XmlRpc::XmlRpcValue::TypeArray); - // Loop through all tag descriptions - for (int32_t i = 0; i < standalone_tags.size(); i++) - { - - // i-th tag description - XmlRpc::XmlRpcValue& tag_description = standalone_tags[i]; - - // Assert the tag description is a struct - ROS_ASSERT(tag_description.getType() == - XmlRpc::XmlRpcValue::TypeStruct); - // Assert type of field "id" is an int - ROS_ASSERT(tag_description["id"].getType() == - XmlRpc::XmlRpcValue::TypeInt); - // Assert type of field "size" is a double - ROS_ASSERT(tag_description["size"].getType() == - XmlRpc::XmlRpcValue::TypeDouble); - - int id = (int)tag_description["id"]; // tag id - // Tag size (square, side length in meters) - double size = (double)tag_description["size"]; - - // Custom frame name, if such a field exists for this tag - std::string frame_name; - if(tag_description.hasMember("name")) - { - // Assert type of field "name" is a string - ROS_ASSERT(tag_description["name"].getType() == - XmlRpc::XmlRpcValue::TypeString); - frame_name = (std::string)tag_description["name"]; - } - else - { - std::stringstream frame_name_stream; - frame_name_stream << "tag_" << id; - frame_name = frame_name_stream.str(); + apriltag_detection_t *det; + zarray_get(detections_, i, &det); + + // Check if this ID is present in config/tags.yaml + // Check if is part of a tag bundle + int tagID = det->id; + bool is_part_of_bundle = false; + for (unsigned int j=0; jimage, cv::Point((int)det->p[0][0], (int)det->p[0][1]), + cv::Point((int)det->p[1][0], (int)det->p[1][1]), + cv::Scalar(0, 0xff, 0)); // green + line(image->image, cv::Point((int)det->p[0][0], (int)det->p[0][1]), + cv::Point((int)det->p[3][0], (int)det->p[3][1]), + cv::Scalar(0, 0, 0xff)); // red + line(image->image, cv::Point((int)det->p[1][0], (int)det->p[1][1]), + cv::Point((int)det->p[2][0], (int)det->p[2][1]), + cv::Scalar(0xff, 0, 0)); // blue + line(image->image, cv::Point((int)det->p[2][0], (int)det->p[2][1]), + cv::Point((int)det->p[3][0], (int)det->p[3][1]), + cv::Scalar(0xff, 0, 0)); // blue + + // Print tag ID in the middle of the tag + std::stringstream ss; + ss << det->id; + cv::String text = ss.str(); + int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX; + double fontscale = 0.5; + int baseline; + cv::Size textsize = cv::getTextSize(text, fontface, + fontscale, 2, &baseline); + cv::putText(image->image, text, + cv::Point((int)(det->c[0]-textsize.width/2), + (int)(det->c[1]+textsize.height/2)), + fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2); } - - StandaloneTagDescription description(id, size, frame_name); - ROS_INFO_STREAM("Loaded tag config: " << id << ", size: " << - size << ", frame_name: " << frame_name.c_str()); - // Add this tag's description to map of descriptions - descriptions.insert(std::make_pair(id, description)); - } - - return descriptions; -} - -// parse tag bundle descriptions -std::vector TagDetector::parseTagBundles ( - XmlRpc::XmlRpcValue& tag_bundles) -{ - std::vector descriptions; - ROS_ASSERT(tag_bundles.getType() == XmlRpc::XmlRpcValue::TypeArray); - - // Loop through all tag bundle descritions - for (int32_t i=0; isize()); - } - - // Get this tag's pose with respect to the bundle origin - double x = xmlRpcGetDoubleWithDefault(tag, "x", 0.); - double y = xmlRpcGetDoubleWithDefault(tag, "y", 0.); - double z = xmlRpcGetDoubleWithDefault(tag, "z", 0.); - double qw = xmlRpcGetDoubleWithDefault(tag, "qw", 1.); - double qx = xmlRpcGetDoubleWithDefault(tag, "qx", 0.); - double qy = xmlRpcGetDoubleWithDefault(tag, "qy", 0.); - double qz = xmlRpcGetDoubleWithDefault(tag, "qz", 0.); - Eigen::Quaterniond q_tag(qw, qx, qy, qz); - q_tag.normalize(); - Eigen::Matrix3d R_oi = q_tag.toRotationMatrix(); - - // Build the rigid transform from tag_j to the bundle origin - cv::Matx44d T_mj(R_oi(0,0), R_oi(0,1), R_oi(0,2), x, - R_oi(1,0), R_oi(1,1), R_oi(1,2), y, - R_oi(2,0), R_oi(2,1), R_oi(2,2), z, - 0, 0, 0, 1); - - // Register the tag member - bundle_i.addMemberTag(id, size, T_mj); - ROS_INFO_STREAM(" " << j << ") id: " << id << ", size: " << size << ", " - << "p = [" << x << "," << y << "," << z << "], " - << "q = [" << qw << "," << qx << "," << qy << "," - << qz << "]"); - } - descriptions.push_back(bundle_i); - } - return descriptions; -} - -double TagDetector::xmlRpcGetDouble (XmlRpc::XmlRpcValue& xmlValue, - std::string field) const -{ - ROS_ASSERT((xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) || - (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt)); - if (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt) - { - int tmp = xmlValue[field]; - return (double)tmp; - } - else - { - return xmlValue[field]; - } -} - -double TagDetector::xmlRpcGetDoubleWithDefault (XmlRpc::XmlRpcValue& xmlValue, - std::string field, - double defaultValue) const -{ - if (xmlValue.hasMember(field)) - { - ROS_ASSERT((xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeDouble) || - (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt)); - if (xmlValue[field].getType() == XmlRpc::XmlRpcValue::TypeInt) - { - int tmp = xmlValue[field]; - return (double)tmp; - } - else - { - return xmlValue[field]; - } - } - else - { - return defaultValue; - } } bool TagDetector::findStandaloneTagDescription ( @@ -790,8 +680,7 @@ bool TagDetector::findStandaloneTagDescription ( { if (printWarning) { - ROS_WARN_THROTTLE(10.0, "Requested description of standalone tag ID [%d]," - " but no description was found...",id); + RCLCPP_WARN(nh_->get_logger(), "Requested description of standalone tag ID [%d], but no description was found...",id); } return false; } @@ -799,4 +688,3 @@ bool TagDetector::findStandaloneTagDescription ( return true; } -} // namespace apriltag_ros diff --git a/apriltag_ros/src/continuous_detector.cpp b/apriltag_ros/src/continuous_detector.cpp index 8d30ae5f..56a5af62 100644 --- a/apriltag_ros/src/continuous_detector.cpp +++ b/apriltag_ros/src/continuous_detector.cpp @@ -29,102 +29,84 @@ * Technology. */ -#include "apriltag_ros/continuous_detector.h" +#include "atlas_apriltag_ros/continuous_detector.hpp" +#include -#include -PLUGINLIB_EXPORT_CLASS(apriltag_ros::ContinuousDetector, nodelet::Nodelet); +using namespace atlas_apriltag_ros; -namespace apriltag_ros +ContinuousDetector::ContinuousDetector(const rclcpp::NodeOptions & options) +: nh_(std::make_shared("apriltag_node", options)), custom_qos_(1) { -void ContinuousDetector::onInit () -{ - ros::NodeHandle& nh = getNodeHandle(); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - - tag_detector_ = std::shared_ptr(new TagDetector(pnh)); - draw_tag_detections_image_ = getAprilTagOption(pnh, - "publish_tag_detections_image", false); - it_ = std::shared_ptr( - new image_transport::ImageTransport(nh)); - - std::string transport_hint; - pnh.param("transport_hint", transport_hint, "raw"); - - int queue_size; - pnh.param("queue_size", queue_size, 1); - camera_image_subscriber_ = - it_->subscribeCamera("image_rect", queue_size, - &ContinuousDetector::imageCallback, this, - image_transport::TransportHints(transport_hint)); - tag_detections_publisher_ = - nh.advertise("tag_detections", 1); - if (draw_tag_detections_image_) - { - tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1); - } - - refresh_params_service_ = - pnh.advertiseService("refresh_tag_params", - &ContinuousDetector::refreshParamsCallback, this); -} + rclcpp::uninstall_signal_handlers(); + + // Declare and get parameters + draw_tag_detections_image_ = nh_->declare_parameter("publish_tag_detections_image", false); + std::string tag_detections_image_topic = nh_->declare_parameter("tag_detections_image_topic", "tag_detections_image"); + std::string image_topic = nh_->declare_parameter("image_topic", "color/image_raw"); + int queue_size = nh_->declare_parameter("queue_size", 1); + std::string tag_detections_topic = nh_->declare_parameter("tag_detections_topic", "tag_detections"); + + tag_detector_ = std::shared_ptr(new TagDetector(nh_)); + + + // Image_transport + it_ = std::shared_ptr( + new image_transport::ImageTransport(nh_)); + + camera_image_subscriber_ = it_->subscribeCamera(image_topic, queue_size, + &ContinuousDetector::ImageCallback, this, + new image_transport::TransportHints(nh_.get(), "raw", "transport_hint")); + + tag_detections_publisher_ = nh_->create_publisher(tag_detections_topic, 10); + + if (draw_tag_detections_image_) + { + tag_detections_image_publisher_ = it_->advertise(tag_detections_image_topic, 1); + } -void ContinuousDetector::refreshTagParameters() -{ - // Resetting the tag detector will cause a new param server lookup - // So if the parameters have changed (by someone/something), - // they will be updated dynamically - std::scoped_lock lock(detection_mutex_); - ros::NodeHandle& pnh = getPrivateNodeHandle(); - tag_detector_.reset(new TagDetector(pnh)); } -bool ContinuousDetector::refreshParamsCallback(std_srvs::Empty::Request& req, - std_srvs::Empty::Response& res) + +void ContinuousDetector::ImageCallback ( + const sensor_msgs::msg::Image::ConstSharedPtr& image_rect, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info) { - refreshTagParameters(); - return true; + // Convert ROS's sensor_msgs::Image to cv_bridge::CvImagePtr in order to run + // AprilTag 2 on the iamge + try + { + cv_image_ = cv_bridge::toCvCopy(image_rect, image_rect->encoding); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + // Publish detected tags in the image by AprilTag 2 + tag_detections_publisher_->publish( + tag_detector_->detectTags(cv_image_,camera_info)); + + // Publish the camera image overlaid by outlines of the detected tags and + // their payload values + if (draw_tag_detections_image_) + { + tag_detector_->drawDetections(cv_image_); + tag_detections_image_publisher_.publish(cv_image_->toImageMsg()); + } } -void ContinuousDetector::imageCallback ( - const sensor_msgs::ImageConstPtr& image_rect, - const sensor_msgs::CameraInfoConstPtr& camera_info) +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +ContinuousDetector::get_node_base_interface() const { - std::scoped_lock lock(detection_mutex_); - // Lazy updates: - // When there are no subscribers _and_ when tf is not published, - // skip detection. - if (tag_detections_publisher_.getNumSubscribers() == 0 && - tag_detections_image_publisher_.getNumSubscribers() == 0 && - !tag_detector_->get_publish_tf()) - { - // ROS_INFO_STREAM("No subscribers and no tf publishing, skip processing."); - return; - } - - // Convert ROS's sensor_msgs::Image to cv_bridge::CvImagePtr in order to run - // AprilTag 2 on the iamge - try - { - cv_image_ = cv_bridge::toCvCopy(image_rect, image_rect->encoding); - } - catch (cv_bridge::Exception& e) - { - ROS_ERROR("cv_bridge exception: %s", e.what()); - return; - } - - // Publish detected tags in the image by AprilTag 2 - tag_detections_publisher_.publish( - tag_detector_->detectTags(cv_image_,camera_info)); - - // Publish the camera image overlaid by outlines of the detected tags and - // their payload values - if (draw_tag_detections_image_) - { - tag_detector_->drawDetections(cv_image_); - tag_detections_image_publisher_.publish(cv_image_->toImageMsg()); - } + return this->nh_->get_node_base_interface(); } -} // namespace apriltag_ros + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(atlas_apriltag_ros::ContinuousDetector) \ No newline at end of file From 6db62c151bf16248ffa7ceb815935da4afe3a103 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Albert=20Arl=C3=A0?= Date: Fri, 27 Oct 2023 13:59:33 +0200 Subject: [PATCH 2/9] namespace changed --- apriltag_ros/CMakeLists.txt | 6 +-- apriltag_ros/config/params.yaml | 21 +++++++++++ apriltag_ros/config/settings.yaml | 13 ------- .../include/apriltag_ros/common_functions.hpp | 4 +- .../apriltag_ros/continuous_detector.hpp | 8 ++-- apriltag_ros/launch/launch.py | 37 ++----------------- apriltag_ros/package.xml | 2 +- apriltag_ros/src/common_functions.cpp | 4 +- apriltag_ros/src/continuous_detector.cpp | 6 +-- 9 files changed, 39 insertions(+), 62 deletions(-) create mode 100644 apriltag_ros/config/params.yaml delete mode 100644 apriltag_ros/config/settings.yaml diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index 79451336..c19c7740 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(atlas_apriltag_ros) +project(apriltag_ros) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -93,8 +93,8 @@ ament_target_dependencies(atlas_apriltag_ros_component geometry_msgs ) -rclcpp_components_register_node(atlas_apriltag_ros_component PLUGIN "atlas_apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) -set(node_plugins "${node_plugins}atlas_apriltag_ros::ContinuousDetector;$\n") +rclcpp_components_register_node(atlas_apriltag_ros_component PLUGIN "apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) +set(node_plugins "${node_plugins}apriltag_ros::ContinuousDetector;$\n") # since the package installs libraries without exporting them # it needs to make sure that the library path is being exported diff --git a/apriltag_ros/config/params.yaml b/apriltag_ros/config/params.yaml new file mode 100644 index 00000000..41313d3c --- /dev/null +++ b/apriltag_ros/config/params.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + # AprilTag 3 code parameters + # Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector + # apriltag/include/apriltag.h:struct apriltag_family + tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 + tag_threads: 2 # default: 2 + tag_decimate: 1.0 # default: 1.0 + tag_blur: 0.0 # default: 0.0 + tag_refine_edges: 1 # default: 1 + tag_debug: 0 # default: 0 + max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) + # Other parameters + publish_tf: true # default: false + transport_hint: "raw" # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options + publish_tag_detections_image: true + image_topic: "Camera/image_raw" + queue_size: 1 + tag_detections_topic: "tag_detections" + tag_detections_image_topic: "tag_detections_image" + remove_dupicates: true \ No newline at end of file diff --git a/apriltag_ros/config/settings.yaml b/apriltag_ros/config/settings.yaml deleted file mode 100644 index be33540b..00000000 --- a/apriltag_ros/config/settings.yaml +++ /dev/null @@ -1,13 +0,0 @@ -# AprilTag 3 code parameters -# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector -# apriltag/include/apriltag.h:struct apriltag_family -tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 -tag_threads: 2 # default: 2 -tag_decimate: 1.0 # default: 1.0 -tag_blur: 0.0 # default: 0.0 -tag_refine_edges: 1 # default: 1 -tag_debug: 0 # default: 0 -max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) -# Other parameters -publish_tf: true # default: false -transport_hint: "raw" # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options diff --git a/apriltag_ros/include/apriltag_ros/common_functions.hpp b/apriltag_ros/include/apriltag_ros/common_functions.hpp index beb740ab..f6c91d59 100644 --- a/apriltag_ros/include/apriltag_ros/common_functions.hpp +++ b/apriltag_ros/include/apriltag_ros/common_functions.hpp @@ -82,7 +82,7 @@ #include "yaml-cpp/yaml.h" -namespace atlas_apriltag_ros +namespace apriltag_ros { // Stores the properties of a tag member of a bundle @@ -240,6 +240,6 @@ class TagDetector }; -} // namespace atlas_apriltag_ros +} // namespace apriltag_ros #endif // ATLAS_APRILTAG_ROS_COMMON_FUNCTIONS_H diff --git a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp index 347f6827..074dba29 100644 --- a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp +++ b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp @@ -47,8 +47,8 @@ #include #include -#include "atlas_apriltag_ros/common_functions.hpp" -#include "atlas_apriltag_ros/composition_visibility.h" +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros/composition_visibility.h" #include #include @@ -58,7 +58,7 @@ #include "ageve_interfaces/msg/april_tag_detection.hpp" #include "ageve_interfaces/msg/april_tag_detection_array.hpp" -namespace atlas_apriltag_ros +namespace apriltag_ros { class ContinuousDetector @@ -89,6 +89,6 @@ class ContinuousDetector }; -} // namespace atlas_apriltag_ros +} // namespace apriltag_ros #endif // ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H diff --git a/apriltag_ros/launch/launch.py b/apriltag_ros/launch/launch.py index 2f593b9a..ed4ed97d 100644 --- a/apriltag_ros/launch/launch.py +++ b/apriltag_ros/launch/launch.py @@ -13,7 +13,7 @@ from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -PACKAGE_NAME = 'atlas_apriltag_ros' +PACKAGE_NAME = 'apriltag_ros' def generate_launch_description(): # Configs files @@ -27,57 +27,26 @@ def generate_launch_description(): choices=['True', 'False'], description='Parameter use_sim_time') - valgrind_arg = DeclareLaunchArgument( - name='valgrind', - default_value='False', - choices=['True', 'False'], - description='Debug with valgrind') - # Launch arguments sim_time = LaunchConfiguration('use_sim_time') - valgrind = LaunchConfiguration('valgrind') - - remaps=[ - ('/tf', 'tf'), - ('/tf_static', 'tf_static') - ] # Nodes to launch node = Node( package=PACKAGE_NAME, - executable='ContinuousDetector', - arguments=['--ros-args', '--log-level', 'info'], - parameters=[config_file], - remappings=remaps, - output="screen", - emulate_tty=True, - condition=IfCondition(PythonExpression(['not ', valgrind])) - ) - - node_with_valgrind = Node( - package=PACKAGE_NAME, - executable='ContinuousDetector', + executable='ContinuousDetector', # Other option SingleDetector arguments=['--ros-args', '--log-level', 'info'], - prefix=["valgrind --leak-check=yes -s"], parameters=[config_file], - remappings=remaps, output="screen", emulate_tty=True, - condition=IfCondition(valgrind) ) return LaunchDescription([ GroupAction( actions=[ - PushRosNamespace( - os.getenv('ATLAS_NAMESPACE') if 'ATLAS_NAMESPACE' in os.environ.keys() else "" - ), sim_time_arg, - valgrind_arg, SetParameter("use_sim_time", sim_time), SetParameter("tags_yaml_path", tags_config_file), - node, - node_with_valgrind + node ] ) ]) diff --git a/apriltag_ros/package.xml b/apriltag_ros/package.xml index 99558e8e..a881b2f2 100644 --- a/apriltag_ros/package.xml +++ b/apriltag_ros/package.xml @@ -1,7 +1,7 @@ - atlas_apriltag_ros + apriltag_ros 0.0.0 utils package diff --git a/apriltag_ros/src/common_functions.cpp b/apriltag_ros/src/common_functions.cpp index 67484121..39d4a623 100644 --- a/apriltag_ros/src/common_functions.cpp +++ b/apriltag_ros/src/common_functions.cpp @@ -29,7 +29,7 @@ * Technology. */ -#include "atlas_apriltag_ros/common_functions.hpp" +#include "apriltag_ros/common_functions.hpp" #include "image_geometry/pinhole_camera_model.h" #include "apriltag/common/homography.h" @@ -42,7 +42,7 @@ #include "apriltag/tagCircle21h7.h" #include "apriltag/tagCircle49h12.h" -using namespace atlas_apriltag_ros; +using namespace apriltag_ros; TagDetector::TagDetector(rclcpp::Node::SharedPtr nh) diff --git a/apriltag_ros/src/continuous_detector.cpp b/apriltag_ros/src/continuous_detector.cpp index 56a5af62..129c6a5e 100644 --- a/apriltag_ros/src/continuous_detector.cpp +++ b/apriltag_ros/src/continuous_detector.cpp @@ -29,11 +29,11 @@ * Technology. */ -#include "atlas_apriltag_ros/continuous_detector.hpp" +#include "apriltag_ros/continuous_detector.hpp" #include -using namespace atlas_apriltag_ros; +using namespace apriltag_ros; ContinuousDetector::ContinuousDetector(const rclcpp::NodeOptions & options) : nh_(std::make_shared("apriltag_node", options)), custom_qos_(1) @@ -109,4 +109,4 @@ ContinuousDetector::get_node_base_interface() const // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(atlas_apriltag_ros::ContinuousDetector) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::ContinuousDetector) \ No newline at end of file From 4374a5b71a4b5a07d888f0f64a96f8c29f21e86d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Albert=20Arl=C3=A0?= Date: Fri, 27 Oct 2023 14:01:16 +0200 Subject: [PATCH 3/9] imports not used --- apriltag_ros/launch/launch.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apriltag_ros/launch/launch.py b/apriltag_ros/launch/launch.py index ed4ed97d..bde9aaa2 100644 --- a/apriltag_ros/launch/launch.py +++ b/apriltag_ros/launch/launch.py @@ -7,11 +7,10 @@ import os from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node, SetParameter, PushRosNamespace +from launch_ros.actions import Node, SetParameter from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, GroupAction -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration PACKAGE_NAME = 'apriltag_ros' From e2969c0b9af044c6285d85d144d4cc582cb99925 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Albert=20Arl=C3=A0?= Date: Tue, 16 Jan 2024 08:51:53 +0100 Subject: [PATCH 4/9] single_image_node_created --- .gitignore | 2 + apriltag_ros/CMakeLists.txt | 33 ++--- .../include/apriltag_ros/common_functions.hpp | 6 +- .../apriltag_ros/continuous_detector.hpp | 6 +- ...e_detector.h => single_image_detector.hpp} | 38 ++++-- .../{launch.py => launch_continuous.py} | 2 +- apriltag_ros/launch/launch_single.py | 51 ++++++++ apriltag_ros/package.xml | 5 +- .../apriltag_ros_single_image_client_node.cpp | 4 +- .../apriltag_ros_single_image_server_node.cpp | 45 ------- apriltag_ros/src/common_functions.cpp | 8 +- apriltag_ros/src/continuous_detector.cpp | 2 +- apriltag_ros/src/single_image_detector.cpp | 123 ++++++++++-------- apriltag_ros_interfaces/CMakeLists.txt | 42 ++++++ .../msg/AprilTagDetection.msg | 0 .../msg/AprilTagDetectionArray.msg | 0 apriltag_ros_interfaces/package.xml | 31 +++++ .../srv/AnalyzeSingleImage.srv | 2 +- 18 files changed, 257 insertions(+), 143 deletions(-) rename apriltag_ros/include/apriltag_ros/{single_image_detector.h => single_image_detector.hpp} (65%) rename apriltag_ros/launch/{launch.py => launch_continuous.py} (97%) create mode 100644 apriltag_ros/launch/launch_single.py delete mode 100644 apriltag_ros/src/apriltag_ros_single_image_server_node.cpp create mode 100644 apriltag_ros_interfaces/CMakeLists.txt rename {apriltag_ros => apriltag_ros_interfaces}/msg/AprilTagDetection.msg (100%) rename {apriltag_ros => apriltag_ros_interfaces}/msg/AprilTagDetectionArray.msg (100%) create mode 100644 apriltag_ros_interfaces/package.xml rename {apriltag_ros => apriltag_ros_interfaces}/srv/AnalyzeSingleImage.srv (88%) diff --git a/.gitignore b/.gitignore index 8c303ba3..99fd40c0 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,5 @@ **/*.bag .vscode/ build/ +install/ +log/ diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index c19c7740..48c29b2e 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -19,11 +19,12 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(ageve_interfaces REQUIRED) +find_package(apriltag_ros_interfaces REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(rcutils REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) @@ -33,12 +34,10 @@ find_package(OpenCV REQUIRED) find_package(yaml-cpp REQUIRED) find_package(image_geometry REQUIRED) - - set(dependencies rclcpp rclcpp_components - ageve_interfaces + apriltag_ros_interfaces cv_bridge image_transport rcutils @@ -51,6 +50,7 @@ set(dependencies image_geometry yaml-cpp geometry_msgs + std_msgs ) @@ -62,23 +62,22 @@ include_directories( # create ament index resource which references the libraries in the binary dir set(node_plugins "") - - # Add cpp libraries -add_library(atlas_apriltag_ros_component SHARED +add_library(${PROJECT_NAME} SHARED "src/continuous_detector.cpp" + "src/single_image_detector.cpp" "src/common_functions.cpp" ) -target_link_libraries(atlas_apriltag_ros_component yaml-cpp apriltag) +target_link_libraries(${PROJECT_NAME} yaml-cpp apriltag) -target_compile_definitions(atlas_apriltag_ros_component +target_compile_definitions(${PROJECT_NAME} PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(atlas_apriltag_ros_component +ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components - ageve_interfaces + apriltag_ros_interfaces cv_bridge image_transport rcutils @@ -91,10 +90,14 @@ ament_target_dependencies(atlas_apriltag_ros_component image_geometry yaml-cpp geometry_msgs + std_msgs ) -rclcpp_components_register_node(atlas_apriltag_ros_component PLUGIN "apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) -set(node_plugins "${node_plugins}apriltag_ros::ContinuousDetector;$\n") +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) +set(node_plugins "${node_plugins}apriltag_ros::ContinuousDetector;$\n") + +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "apriltag_ros::SingleImageDetector" EXECUTABLE SingleImageDetector) +set(node_plugins "${node_plugins}apriltag_ros::SingleImageDetector;$\n") # since the package installs libraries without exporting them # it needs to make sure that the library path is being exported @@ -104,10 +107,10 @@ if(NOT WIN32) endif() install(TARGETS - atlas_apriltag_ros_component + ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -install(TARGETS atlas_apriltag_ros_component +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/apriltag_ros/include/apriltag_ros/common_functions.hpp b/apriltag_ros/include/apriltag_ros/common_functions.hpp index f6c91d59..ef315109 100644 --- a/apriltag_ros/include/apriltag_ros/common_functions.hpp +++ b/apriltag_ros/include/apriltag_ros/common_functions.hpp @@ -76,8 +76,8 @@ #include "tf2_ros/transform_broadcaster.h" #include "tf2/utils.h" -#include "ageve_interfaces/msg/april_tag_detection_array.hpp" -#include "ageve_interfaces/msg/april_tag_detection.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection_array.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection.hpp" #include "yaml-cpp/yaml.h" @@ -179,7 +179,7 @@ class TagDetector const std_msgs::msg::Header& header); // Detect tags in an image - ageve_interfaces::msg::AprilTagDetectionArray detectTags( + apriltag_ros_interfaces::msg::AprilTagDetectionArray detectTags( const cv_bridge::CvImagePtr& image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info); diff --git a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp index 074dba29..3f6966b5 100644 --- a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp +++ b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp @@ -55,8 +55,8 @@ #include // Own -#include "ageve_interfaces/msg/april_tag_detection.hpp" -#include "ageve_interfaces/msg/april_tag_detection_array.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection_array.hpp" namespace apriltag_ros { @@ -85,7 +85,7 @@ class ContinuousDetector std::shared_ptr it_; image_transport::CameraSubscriber camera_image_subscriber_; image_transport::Publisher tag_detections_image_publisher_; - rclcpp::Publisher::SharedPtr tag_detections_publisher_; + rclcpp::Publisher::SharedPtr tag_detections_publisher_; }; diff --git a/apriltag_ros/include/apriltag_ros/single_image_detector.h b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp similarity index 65% rename from apriltag_ros/include/apriltag_ros/single_image_detector.h rename to apriltag_ros/include/apriltag_ros/single_image_detector.hpp index e7f219ca..38f943b1 100644 --- a/apriltag_ros/include/apriltag_ros/single_image_detector.h +++ b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp @@ -44,26 +44,40 @@ #ifndef APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_H #define APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_H -#include "apriltag_ros/common_functions.h" -#include +#include + +#include + +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros/composition_visibility.h" +#include "apriltag_ros_interfaces/srv/analyze_single_image.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection.hpp" +#include "apriltag_ros_interfaces/msg/april_tag_detection_array.hpp" namespace apriltag_ros { class SingleImageDetector { - private: - TagDetector tag_detector_; - ros::ServiceServer single_image_analysis_service_; + public: + COMPOSITION_PUBLIC + explicit SingleImageDetector(const rclcpp::NodeOptions & options); + + COMPOSITION_PUBLIC + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr + get_node_base_interface() const; + + // The function which provides the single image analysis service + void analyzeImage(const apriltag_ros_interfaces::srv::AnalyzeSingleImage::Request& request, + apriltag_ros_interfaces::srv::AnalyzeSingleImage::Response& response); - ros::Publisher tag_detections_publisher_; - - public: - SingleImageDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh); + rclcpp::Node::SharedPtr nh_; + rclcpp::QoS custom_qos_; + private: + std::shared_ptr tag_detector_; + rclcpp::Service::SharedPtr single_image_analysis_service_; - // The function which provides the single image analysis service - bool analyzeImage(apriltag_ros::AnalyzeSingleImage::Request& request, - apriltag_ros::AnalyzeSingleImage::Response& response); + rclcpp::Publisher::SharedPtr tag_detections_publisher_; }; } // namespace apriltag_ros diff --git a/apriltag_ros/launch/launch.py b/apriltag_ros/launch/launch_continuous.py similarity index 97% rename from apriltag_ros/launch/launch.py rename to apriltag_ros/launch/launch_continuous.py index bde9aaa2..79ac5dbc 100644 --- a/apriltag_ros/launch/launch.py +++ b/apriltag_ros/launch/launch_continuous.py @@ -2,7 +2,7 @@ Copyright 2023 @ MOVVO ROBOTICS --------------------------------------------------------- Authors: Albert Arlà, Martí Bolet, Bernat Gaston, María Mercadé - Contact: support.idi@ageve.net + Contact: support.idi@movvo.eu """ import os diff --git a/apriltag_ros/launch/launch_single.py b/apriltag_ros/launch/launch_single.py new file mode 100644 index 00000000..00f285f5 --- /dev/null +++ b/apriltag_ros/launch/launch_single.py @@ -0,0 +1,51 @@ +""" + Copyright 2023 @ MOVVO ROBOTICS + --------------------------------------------------------- + Authors: Albert Arlà, Martí Bolet, Bernat Gaston, María Mercadé + Contact: support.idi@movvo.eu +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, SetParameter +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration + +PACKAGE_NAME = 'apriltag_ros' + +def generate_launch_description(): + # Configs files + config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "params.yaml") + tags_config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "tags.yaml") + + # Launch arguments + sim_time_arg = DeclareLaunchArgument( + name='use_sim_time', + default_value='False', + choices=['True', 'False'], + description='Parameter use_sim_time') + + # Launch arguments + sim_time = LaunchConfiguration('use_sim_time') + + # Nodes to launch + node = Node( + package=PACKAGE_NAME, + executable='SingleImageDetector', # Other option SingleDetector + arguments=['--ros-args', '--log-level', 'info'], + parameters=[config_file], + output="screen", + emulate_tty=True, + ) + + return LaunchDescription([ + GroupAction( + actions=[ + sim_time_arg, + SetParameter("use_sim_time", sim_time), + SetParameter("tags_yaml_path", tags_config_file), + node + ] + ) + ]) diff --git a/apriltag_ros/package.xml b/apriltag_ros/package.xml index a881b2f2..eca3f548 100644 --- a/apriltag_ros/package.xml +++ b/apriltag_ros/package.xml @@ -14,11 +14,11 @@ Proprietary MOVVO ROBOTICS. All rights reserved ament_cmake - ament_cmake_python + ament_cmake_python rclcpp rclcpp_components - ageve_interfaces + apriltag_ros_interfaces cv_bridge image_transport rcutils @@ -30,6 +30,7 @@ OpenCV image_geometry geometry_msgs + std_msgs ament_lint_auto ament_lint_common diff --git a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp index c24d3217..5d33d30a 100644 --- a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp +++ b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp @@ -29,8 +29,8 @@ * Technology. */ -#include "apriltag_ros/common_functions.h" -#include +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros_interfaces/srv/analyze_single_image.hpp" bool getRosParameter (ros::NodeHandle& pnh, std::string name, double& param) { diff --git a/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp b/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp deleted file mode 100644 index 09ab903f..00000000 --- a/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * Copyright (c) 2017, California Institute of Technology. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * The views and conclusions contained in the software and documentation are - * those of the authors and should not be interpreted as representing official - * policies, either expressed or implied, of the California Institute of - * Technology. - */ - -#include "apriltag_ros/common_functions.h" -#include "apriltag_ros/single_image_detector.h" - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "apriltag_ros_single_image_server"); - - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - apriltag_ros::SingleImageDetector continuous_tag_detector(nh, pnh); - - ros::spin(); -} diff --git a/apriltag_ros/src/common_functions.cpp b/apriltag_ros/src/common_functions.cpp index 39d4a623..04911186 100644 --- a/apriltag_ros/src/common_functions.cpp +++ b/apriltag_ros/src/common_functions.cpp @@ -266,7 +266,7 @@ std::vector TagDetector::parse_tag_bundles_from_yaml(const return tag_bundle_descriptions; } -ageve_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( +apriltag_ros_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( const cv_bridge::CvImagePtr& image, const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info) { // Convert image to AprilTag code's format @@ -319,7 +319,7 @@ ageve_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( // Compute the estimated translation and rotation individually for each // detected tag - ageve_interfaces::msg::AprilTagDetectionArray tag_detection_array; + apriltag_ros_interfaces::msg::AprilTagDetectionArray tag_detection_array; std::vector detection_names; tag_detection_array.header = image->header; std::map > bundleObjectPoints; @@ -405,7 +405,7 @@ ageve_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( makeTagPose(transform, image->header); // Add the detection to the back of the tag detection array - ageve_interfaces::msg::AprilTagDetection tag_detection; + apriltag_ros_interfaces::msg::AprilTagDetection tag_detection; tag_detection.pose = tag_pose; tag_detection.id.push_back(detection->id); tag_detection.size.push_back(tag_size); @@ -438,7 +438,7 @@ ageve_interfaces::msg::AprilTagDetectionArray TagDetector::detectTags ( makeTagPose(transform, image->header); // Add the detection to the back of the tag detection array - ageve_interfaces::msg::AprilTagDetection tag_detection; + apriltag_ros_interfaces::msg::AprilTagDetection tag_detection; tag_detection.pose = bundle_pose; tag_detection.id = bundle.bundleIds(); tag_detection.size = bundle.bundleSizes(); diff --git a/apriltag_ros/src/continuous_detector.cpp b/apriltag_ros/src/continuous_detector.cpp index 129c6a5e..0c0d7e62 100644 --- a/apriltag_ros/src/continuous_detector.cpp +++ b/apriltag_ros/src/continuous_detector.cpp @@ -58,7 +58,7 @@ ContinuousDetector::ContinuousDetector(const rclcpp::NodeOptions & options) &ContinuousDetector::ImageCallback, this, new image_transport::TransportHints(nh_.get(), "raw", "transport_hint")); - tag_detections_publisher_ = nh_->create_publisher(tag_detections_topic, 10); + tag_detections_publisher_ = nh_->create_publisher(tag_detections_topic, 10); if (draw_tag_detections_image_) { diff --git a/apriltag_ros/src/single_image_detector.cpp b/apriltag_ros/src/single_image_detector.cpp index 6fd49f31..2c4a1909 100644 --- a/apriltag_ros/src/single_image_detector.cpp +++ b/apriltag_ros/src/single_image_detector.cpp @@ -29,68 +29,83 @@ * Technology. */ -#include "apriltag_ros/single_image_detector.h" +#include "apriltag_ros/single_image_detector.hpp" #include -#include +#include -namespace apriltag_ros -{ +using namespace apriltag_ros; +using std::placeholders::_1; +using std::placeholders::_2; -SingleImageDetector::SingleImageDetector (ros::NodeHandle& nh, - ros::NodeHandle& pnh) : - tag_detector_(pnh) +SingleImageDetector::SingleImageDetector (const rclcpp::NodeOptions & options) +: nh_(std::make_shared("apriltag_node", options)), custom_qos_(1) { - // Advertise the single image analysis service - single_image_analysis_service_ = - nh.advertiseService("single_image_tag_detection", - &SingleImageDetector::analyzeImage, this); - tag_detections_publisher_ = - nh.advertise("tag_detections", 1); - ROS_INFO_STREAM("Ready to do tag detection on single images"); + rclcpp::uninstall_signal_handlers(); + + // Advertise the single image analysis service + single_image_analysis_service_ = nh_->create_service("single_image_tag_detection", + [this](const std::shared_ptr request, + std::shared_ptr response) { + this->analyzeImage(*request, *response);}); + + tag_detections_publisher_ = nh_->create_publisher("tag_detections", 1); + RCLCPP_INFO(nh_->get_logger(),"Ready to do tag detection on single images"); } -bool SingleImageDetector::analyzeImage( - apriltag_ros::AnalyzeSingleImage::Request& request, - apriltag_ros::AnalyzeSingleImage::Response& response) +void SingleImageDetector::analyzeImage( + const apriltag_ros_interfaces::srv::AnalyzeSingleImage::Request& request, + apriltag_ros_interfaces::srv::AnalyzeSingleImage::Response& response) { - ROS_INFO("[ Summoned to analyze image ]"); - ROS_INFO("Image load path: %s", - request.full_path_where_to_get_image.c_str()); - ROS_INFO("Image save path: %s", - request.full_path_where_to_save_image.c_str()); - - // Read the image - cv::Mat image = cv::imread(request.full_path_where_to_get_image, - cv::IMREAD_COLOR); - if (image.data == NULL) - { - // Cannot read image - ROS_ERROR_STREAM("Could not read image " << - request.full_path_where_to_get_image.c_str()); - return false; - } - - // Detect tags in the image - cv_bridge::CvImagePtr loaded_image(new cv_bridge::CvImage(std_msgs::Header(), - "bgr8", image)); - loaded_image->header.frame_id = "camera"; - response.tag_detections = - tag_detector_.detectTags(loaded_image,sensor_msgs::CameraInfoConstPtr( - new sensor_msgs::CameraInfo(request.camera_info))); - - // Publish detected tags (AprilTagDetectionArray, basically an array of - // geometry_msgs/PoseWithCovarianceStamped) - tag_detections_publisher_.publish(response.tag_detections); - - // Save tag detections image - tag_detector_.drawDetections(loaded_image); - cv::imwrite(request.full_path_where_to_save_image, loaded_image->image); - - ROS_INFO("Done!\n"); - - return true; + RCLCPP_INFO(nh_->get_logger(),"[ Summoned to analyze image ]"); + RCLCPP_INFO(nh_->get_logger(),"Image load path: %s", + request.full_path_where_to_get_image.c_str()); + RCLCPP_INFO(nh_->get_logger(),"Image save path: %s", + request.full_path_where_to_save_image.c_str()); + + // Read the image + cv::Mat image = cv::imread(request.full_path_where_to_get_image, + cv::IMREAD_COLOR); + if (image.data == NULL) + { + // Cannot read image + RCLCPP_ERROR(nh_->get_logger(),"Could not read image %s", + request.full_path_where_to_get_image.c_str()); + return; + } + + // Detect tags in the image + cv_bridge::CvImagePtr loaded_image(new cv_bridge::CvImage(std_msgs::msg::Header(), + "bgr8", image)); + loaded_image->header.frame_id = "camera"; + response.tag_detections = + tag_detector_->detectTags(loaded_image,sensor_msgs::msg::CameraInfo::ConstSharedPtr( + new sensor_msgs::msg::CameraInfo(request.camera_info))); + + // Publish detected tags (AprilTagDetectionArray, basically an array of + // geometry_msgs/PoseWithCovarianceStamped) + tag_detections_publisher_->publish(response.tag_detections); + + // Save tag detections image + tag_detector_->drawDetections(loaded_image); + cv::imwrite(request.full_path_where_to_save_image, loaded_image->image); + + RCLCPP_INFO(nh_->get_logger(),"Done!"); + } -} // namespace apriltag_ros + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +SingleImageDetector::get_node_base_interface() const +{ + return this->nh_->get_node_base_interface(); +} + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::SingleImageDetector) diff --git a/apriltag_ros_interfaces/CMakeLists.txt b/apriltag_ros_interfaces/CMakeLists.txt new file mode 100644 index 00000000..e5e25f04 --- /dev/null +++ b/apriltag_ros_interfaces/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(apriltag_ros_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(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +set(msg_files + "msg/AprilTagDetection.msg" + "msg/AprilTagDetectionArray.msg" +) + +set(srv_files + "srv/AnalyzeSingleImage.srv" +) + + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES sensor_msgs + geometry_msgs + ) + +ament_package() \ No newline at end of file diff --git a/apriltag_ros/msg/AprilTagDetection.msg b/apriltag_ros_interfaces/msg/AprilTagDetection.msg similarity index 100% rename from apriltag_ros/msg/AprilTagDetection.msg rename to apriltag_ros_interfaces/msg/AprilTagDetection.msg diff --git a/apriltag_ros/msg/AprilTagDetectionArray.msg b/apriltag_ros_interfaces/msg/AprilTagDetectionArray.msg similarity index 100% rename from apriltag_ros/msg/AprilTagDetectionArray.msg rename to apriltag_ros_interfaces/msg/AprilTagDetectionArray.msg diff --git a/apriltag_ros_interfaces/package.xml b/apriltag_ros_interfaces/package.xml new file mode 100644 index 00000000..a7a9a590 --- /dev/null +++ b/apriltag_ros_interfaces/package.xml @@ -0,0 +1,31 @@ + + + + apriltag_ros_interfaces + 0.0.0 + TODO: Package description + + AlbertArla, + MartiBolet, + BernatGaston, + MariaMercade + + TODO: License declaration + + ament_cmake + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + sensor_msgs + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/apriltag_ros/srv/AnalyzeSingleImage.srv b/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv similarity index 88% rename from apriltag_ros/srv/AnalyzeSingleImage.srv rename to apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv index afea43c3..cfc11898 100644 --- a/apriltag_ros/srv/AnalyzeSingleImage.srv +++ b/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv @@ -11,4 +11,4 @@ string full_path_where_to_get_image string full_path_where_to_save_image sensor_msgs/CameraInfo camera_info --- -apriltag_ros/AprilTagDetectionArray tag_detections \ No newline at end of file +AprilTagDetectionArray tag_detections \ No newline at end of file From 13160225ebd70029155cf228237e72542254dbe8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Albert=20Arl=C3=A0?= Date: Wed, 31 Jan 2024 15:30:00 +0100 Subject: [PATCH 5/9] signle_image_client added --- apriltag_ros/CMakeLists.txt | 3 + .../apriltag_ros_single_image_client_node.cpp | 74 +++++++------------ 2 files changed, 31 insertions(+), 46 deletions(-) diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index 48c29b2e..f1cd0f99 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -99,6 +99,9 @@ set(node_plugins "${node_plugins}apriltag_ros::ContinuousDetector;$\n") +add_executable(single_image_client src/apriltag_ros_single_image_client_node.cpp) +target_link_libraries(single_image_client ${PROJECT_NAME}) + # since the package installs libraries without exporting them # it needs to make sure that the library path is being exported if(NOT WIN32) diff --git a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp index 5d33d30a..990b4004 100644 --- a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp +++ b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp @@ -32,7 +32,7 @@ #include "apriltag_ros/common_functions.hpp" #include "apriltag_ros_interfaces/srv/analyze_single_image.hpp" -bool getRosParameter (ros::NodeHandle& pnh, std::string name, double& param) +bool getRosParameter (rclcpp::Node::SharedPtr nh, std::string name, double& param) { // Write parameter "name" from ROS Parameter Server into param // Return true if successful, false otherwise @@ -51,55 +51,36 @@ bool getRosParameter (ros::NodeHandle& pnh, std::string name, double& param) int main(int argc, char **argv) { - ros::init(argc, argv, "apriltag_ros_single_image_client"); + rclcpp::init(argc, argv); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); + std::shared_ptr node = rclcpp::Node::make_shared("single_image_client_node"); - ros::ServiceClient client = - nh.serviceClient( - "single_image_tag_detection"); + rclcpp::Client::SharedPtr client = + node->create_client("single_image_tag_detection"); - // Get the request parameters - apriltag_ros::AnalyzeSingleImage service; - service.request.full_path_where_to_get_image = - apriltag_ros::getAprilTagOption( - pnh, "image_load_path", ""); - if (service.request.full_path_where_to_get_image.empty()) - { - return 1; - } - service.request.full_path_where_to_save_image = - apriltag_ros::getAprilTagOption( - pnh, "image_save_path", ""); - if (service.request.full_path_where_to_save_image.empty()) - { - return 1; - } + auto request = std::make_shared(); - // Replicate sensors_msgs/CameraInfo message (must be up-to-date with the - // analyzed image!) - service.request.camera_info.distortion_model = "plumb_bob"; - double fx, fy, cx, cy; - if (!getRosParameter(pnh, "fx", fx)) - return 1; - if (!getRosParameter(pnh, "fy", fy)) - return 1; - if (!getRosParameter(pnh, "cx", cx)) - return 1; - if (!getRosParameter(pnh, "cy", cy)) - return 1; - // Intrinsic camera matrix for the raw (distorted) images - service.request.camera_info.K[0] = fx; - service.request.camera_info.K[2] = cx; - service.request.camera_info.K[4] = fy; - service.request.camera_info.K[5] = cy; - service.request.camera_info.K[8] = 1.0; - service.request.camera_info.P[0] = fx; - service.request.camera_info.P[2] = cx; - service.request.camera_info.P[5] = fy; - service.request.camera_info.P[6] = cy; - service.request.camera_info.P[10] = 1.0; + // Write desired path where to get the image + request->full_path_where_to_get_image = "/full/path/of/image"; + + // Write desired path where to save the image + request->full_path_where_to_save_image = "/full/path/of/image"; + + // Write camera parameters + double fx = 0.0; + double fy = 0.0; + double cx = 0.0; + double cy = 0.0; + request->camera_info.K[0] = fx; + request->camera_info.K[2] = cx; + request->camera_info.K[4] = fy; + request->camera_info.K[5] = cy; + request->camera_info.K[8] = 1.0; + request->camera_info.P[0] = fx; + request->camera_info.P[2] = cx; + request->camera_info.P[5] = fy; + request->camera_info.P[6] = cy; + request->camera_info.P[10] = 1.0; // Call the service (detect tags in the image specified by the // image_load_path) @@ -118,5 +99,6 @@ int main(int argc, char **argv) return 1; } + rclcpp::shutdown(); return 0; // happy ending } From 779a9765b6cfff2b330affa46b73e89ed0d6e92f Mon Sep 17 00:00:00 2001 From: mariamercade Date: Wed, 31 Jan 2024 16:32:35 +0100 Subject: [PATCH 6/9] single_image_detector std::bind --- .../apriltag_ros/single_image_detector.hpp | 4 ++-- apriltag_ros/src/single_image_detector.cpp | 24 +++++++++---------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/apriltag_ros/include/apriltag_ros/single_image_detector.hpp b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp index 38f943b1..cb8840e8 100644 --- a/apriltag_ros/include/apriltag_ros/single_image_detector.hpp +++ b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp @@ -68,8 +68,8 @@ class SingleImageDetector get_node_base_interface() const; // The function which provides the single image analysis service - void analyzeImage(const apriltag_ros_interfaces::srv::AnalyzeSingleImage::Request& request, - apriltag_ros_interfaces::srv::AnalyzeSingleImage::Response& response); + void analyzeImage(const std::shared_ptr request, + std::shared_ptr response); rclcpp::Node::SharedPtr nh_; rclcpp::QoS custom_qos_; diff --git a/apriltag_ros/src/single_image_detector.cpp b/apriltag_ros/src/single_image_detector.cpp index 2c4a1909..fdb251e1 100644 --- a/apriltag_ros/src/single_image_detector.cpp +++ b/apriltag_ros/src/single_image_detector.cpp @@ -45,33 +45,31 @@ SingleImageDetector::SingleImageDetector (const rclcpp::NodeOptions & options) // Advertise the single image analysis service single_image_analysis_service_ = nh_->create_service("single_image_tag_detection", - [this](const std::shared_ptr request, - std::shared_ptr response) { - this->analyzeImage(*request, *response);}); + std::bind(&SingleImageDetector::analyzeImage, this, _1, _2)); tag_detections_publisher_ = nh_->create_publisher("tag_detections", 1); RCLCPP_INFO(nh_->get_logger(),"Ready to do tag detection on single images"); } void SingleImageDetector::analyzeImage( - const apriltag_ros_interfaces::srv::AnalyzeSingleImage::Request& request, - apriltag_ros_interfaces::srv::AnalyzeSingleImage::Response& response) + const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_INFO(nh_->get_logger(),"[ Summoned to analyze image ]"); RCLCPP_INFO(nh_->get_logger(),"Image load path: %s", - request.full_path_where_to_get_image.c_str()); + request->full_path_where_to_get_image.c_str()); RCLCPP_INFO(nh_->get_logger(),"Image save path: %s", - request.full_path_where_to_save_image.c_str()); + request->full_path_where_to_save_image.c_str()); // Read the image - cv::Mat image = cv::imread(request.full_path_where_to_get_image, + cv::Mat image = cv::imread(request->full_path_where_to_get_image, cv::IMREAD_COLOR); if (image.data == NULL) { // Cannot read image RCLCPP_ERROR(nh_->get_logger(),"Could not read image %s", - request.full_path_where_to_get_image.c_str()); + request->full_path_where_to_get_image.c_str()); return; } @@ -79,17 +77,17 @@ void SingleImageDetector::analyzeImage( cv_bridge::CvImagePtr loaded_image(new cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", image)); loaded_image->header.frame_id = "camera"; - response.tag_detections = + response->tag_detections = tag_detector_->detectTags(loaded_image,sensor_msgs::msg::CameraInfo::ConstSharedPtr( - new sensor_msgs::msg::CameraInfo(request.camera_info))); + new sensor_msgs::msg::CameraInfo(request->camera_info))); // Publish detected tags (AprilTagDetectionArray, basically an array of // geometry_msgs/PoseWithCovarianceStamped) - tag_detections_publisher_->publish(response.tag_detections); + tag_detections_publisher_->publish(response->tag_detections); // Save tag detections image tag_detector_->drawDetections(loaded_image); - cv::imwrite(request.full_path_where_to_save_image, loaded_image->image); + cv::imwrite(request->full_path_where_to_save_image, loaded_image->image); RCLCPP_INFO(nh_->get_logger(),"Done!"); From a8772b093035cb526149883e68a6b701d1a2c866 Mon Sep 17 00:00:00 2001 From: mariamercade Date: Thu, 1 Feb 2024 16:09:33 +0100 Subject: [PATCH 7/9] ros2 single imager detector tested --- README.md | 124 +++++++++++++++--- apriltag_ros/CMakeLists.txt | 21 ++- .../apriltag_ros/single_image_detector.hpp | 20 +-- apriltag_ros/launch/launch_continuous.py | 7 - apriltag_ros/launch/launch_single.py | 7 - .../launch/single_image_client_launch.py | 26 ++++ .../launch/single_image_server_launch.py | 42 ++++++ .../apriltag_ros_single_image_client_node.cpp | 103 +++++++-------- .../apriltag_ros_single_image_server_node.cpp | 45 +++++++ apriltag_ros/src/single_image_detector.cpp | 48 +++---- .../srv/AnalyzeSingleImage.srv | 3 +- 11 files changed, 301 insertions(+), 145 deletions(-) create mode 100644 apriltag_ros/launch/single_image_client_launch.py create mode 100644 apriltag_ros/launch/single_image_server_launch.py create mode 100644 apriltag_ros/src/apriltag_ros_single_image_server_node.cpp diff --git a/README.md b/README.md index 988140ec..9deec059 100644 --- a/README.md +++ b/README.md @@ -1,28 +1,40 @@ # apriltag_ros -`apriltag_ros` is a Robot Operating System (ROS) wrapper of the [AprilTag 3 visual fiducial detector](https://april.eecs.umich.edu/software/apriltag.html). For details and tutorials, please see the [ROS wiki](http://wiki.ros.org/apriltag_ros). - -`apriltag_ros` depends on the latest release of the [AprilTag library](https://github.com/AprilRobotics/apriltag). Clone it into your catkin workspace before building. +`apriltag_ros` is a ROS2 wrapper of the [AprilTag 3 visual fiducial detector](https://april.eecs.umich.edu/software/apriltag.html). **Authors**: Danylo Malyuta, Wolfgang Merkt **Maintainers**: [Danylo Malyuta](mailto:danylo.malyuta@gmail.com) ([Autonomous Control Laboratory](https://www.aa.washington.edu/research/acl), University of Washington), [Wolfgang Merkt](https://github.com/wxmerkt) +## Package Overview + +This package provides functionality for detecting AprilTags in both continuous image streams and individual images. It subscribes to the following default input topics, which can be remapped according to user needs: + +- `/camera_rect/image_rect`: a `sensor_msgs/Image` topic containing the image, assuming it's undistorted.. +- `/camera_rect/camera_info`: a `sensor_msgs/CameraInfo` topic providing the camera calibration matrix in `/camera/camera_info/K`. + +The behavior of the ROS wrapper is defined by two configuration files: +- `config/tags.yaml`: Defines the tags and tag bundles to detect. +- `config/settings.yaml`: Configures the core Apriltag 3 algorithm parameters. + +The package outputs the following topics: +- `/tf`: Provides the relative pose between the camera frame and each detected tag's or tag bundle's frame, specified in `tags.yaml`, using tf. This is published only if `publish_tf` is set to true in `settings.yaml`. +- `/tag_detections`: Carries the same information as `/tf`, but as a custom message containing the tag ID(s), size(s), and `geometry_msgs/PoseWithCovarianceStamped` pose information (applies in the case of tag bundles). This topic is always published. +- `/tag_detections_image`: Displays the same input image from `/camera/image_rect`, but with the detected tags highlighted. This is published only if `publish_tag_detections_image` is set to true in `config/params.yaml`. + ## Quickstart -Starting with a working ROS installation (Kinetic and Melodic are supported): +Starting with a working ROS2 installation (Humble is supported): ``` -export ROS_DISTRO=melodic # Set this to your distro, e.g. kinetic or melodic -source /opt/ros/$ROS_DISTRO/setup.bash # Source your ROS distro -mkdir -p ~/catkin_ws/src # Make a new workspace -cd ~/catkin_ws/src # Navigate to the source space -git clone https://github.com/AprilRobotics/apriltag.git # Clone Apriltag library -git clone https://github.com/AprilRobotics/apriltag_ros.git # Clone Apriltag ROS wrapper -cd ~/catkin_ws # Navigate to the workspace -rosdep install --from-paths src --ignore-src -r -y # Install any missing packages -catkin build # Build all packages in the workspace (catkin_make_isolated will work also) +export ROS_DISTRO=humble # Set this to your distro, e.g. humble +source /opt/ros/$ROS_DISTRO/setup.bash # Source your ROS distro +mkdir -p ~/ros2_ws/src # Make a new workspace +cd ~/ros2_ws/src # Navigate to the source space +git clone https://github.com/AprilRobotics/apriltag_ros.git # Clone Apriltag ROS wrapper +cd ~/ros2_ws # Navigate to the workspace +rosdep install --from-paths src --ignore-src # Install any missing packages +colcon build --symlink-install # Build all packages in the workspace ``` -See the [ROS wiki](http://wiki.ros.org/apriltag_ros) for details and tutorials. ## Tag Size Definition @@ -31,6 +43,86 @@ For a correct depth estimation (and hence the correct full pose) it is necessary Below is a visualization of the tag size (red arrow) to be specified for the most common tag classes: ![Tag Size Guide](./apriltag_ros/docs/tag_size_guide.svg) +## Nodes + +1. **apriltag_ros** + + **Description:** + This node provides detection for a continuous image stream (e.g., video). It detects AprilTags in images produced by a pinhole camera and publishes the detections' pose relative to the camera. + + **Subscribed Topics:** + - `/image_rect` (`sensor_msgs/Image`): Undistorted image from the camera. + - `/camera_info` (`sensor_msgs/CameraInfo`): Camera calibration matrix K. + + **Published Topics:** + - `/tag_detections` (`apriltag_ros/AprilTagDetectionArray`): Array of tag and tag bundle detections' pose relative to the camera. + - `/tag_detections_image` (`sensor_msgs/Image`): Same as `image_rect` but with the detected tags highlighted. + + **Parameters:** + - `tag_family` (string, default: `tag36h11`): AprilTag family to use for detection. + - `tag_border` (int, default: `1`): Width of the tag outer black border. + - `tag_threads` (int, default: `4`): Number of threads the Apriltag 3 core algorithm should use. + - `tag_decimate` (double, default: `1.0`): Decimation factor for quad detection. + - `tag_blur` (double, default: `0.0`): Standard deviation for Gaussian blur applied to the segmented image. + - `tag_refine_edges` (int, default: `1`): Whether to adjust quad edges to strong gradients nearby. + - `tag_refine_decode` (int, default: `0`): Whether to refine detections to increase the number of detected tags. + - `tag_refine_pose` (int, default: `0`): Whether to refine detections to increase pose accuracy. + - `publish_tf` (bool, default: `false`): Enable publishing tag-camera relative poses on `/tf`. + - `camera_frame` (string, default: `camera`): Camera frame name. + - `publish_tag_detections_image` (bool, default: `false`): Enable publishing on `/tag_detections_image`. + + **Provided tf Transforms:** + - `tag` → `camera`: Relative pose of the camera frame to the tag frame. + +2. **apriltag_ros_single_image_server** + + **Description:** + This node detects tags in a single provided image via a ROS2 service. It doesn't subscribe to topics or publish topics like the main `apriltag_ros` node. + + **Services:** + - `single_image_tag_detection` (`apriltag_ros/AnalyzeSingleImage`): Takes in the absolute file path of the input image, the absolute file path where to store the output image, and the camera intrinsics (particularly the K matrix). Returns detected tags' and tag bundles' poses. + +3. **apriltag_ros_single_image_client** + + **Description:** + This node is a client for the `single_image_tag_detection` service provided by `apriltag_ros_single_image_server`. + + **Services Called:** + - `single_image_tag_detection` (`apriltag_ros/AnalyzeSingleImage`): See description above. + + **Arguments:** + - `image_load_path` (string): Absolute file path of the image to detect tags in. + - `image_save_path` (string): Absolute file path where to save the image with detected tags highlighted. + + **Parameters:** + - `fx` (double): Camera x focal length (in pixels). + - `fy` (double): Camera y focal length (in pixels). + - `cx` (double): Camera image principal point x coordinate (in pixels). + - `cy` (double): Camera image principal point y coordinate (in pixels). + +## Tutorials +The main idea is to fill out `config/tags.yaml` with the standalone tags and tag bundles which you would like to detect (bundles potentially require a calibration process). Then, you simply run the continuous or single image detector + +### Detection in a single image + + +The single image detector is based on a ROS2 service. Set the camera intrinsics fx, fy, cx and cy and provide a file path to the image. Then, the server runs it through the AprilTag detector and provides you the detection results and saves an output image. + +Begin by running the service's server node: + +``` +ros2 launch apriltag_ros single_image_server_launch.py +``` + +Now you can run the client in order to detect tags and tag bundles in your image: +``` +ros2 launch apriltag_ros single_image_client_launch.py image_load_path:= image_save_path:= +``` + +PNG images work well (others may work too, but have not been tested). The client will run and you will see the server print out Done! if everything is successful (the server will continue running, waiting for another single image detection service call). The output image will be at your indicated output image path. + +### Detection in a video stream + ## Contributing Pull requests are welcome! Especially for the following areas: @@ -40,10 +132,6 @@ Pull requests are welcome! Especially for the following areas: - Extend calibration to support calibrating tags that cannot appear simultaneously with the master tag, but do appear simultaneously with other tags which themselves or via a similar relationship appear with the master tag (e.g. a bundle with the geometry of a cube - if the master is on one face, tags on the opposite face cannot currently be calibrated). This is basically "transform chaining" and potentially allows calibration of bundles with arbitrary geometry as long as a transform chain exists from any tag to the master tag - Supporting multiple tag family detection (currently all tags have to be of the same family). This means calling the detector once for each family. Because the core AprilTag 2 algorithm is the performance bottleneck, detection of `n` tag families will possibly decrease performance by `1/n` (tbd if this still holds for v3) -## Changelog - -- In March 2019, the code was upgraded to AprilTag 3 and as thus the options `refine_pose`, `refine_decode`, and `black_border` were removed. - ## Copyright The source code in `apriltag_ros/` is original code that is the ROS wrapper itself, see the [LICENSE](https://github.com/AprilRobotics/apriltag_ros/blob/526b9455121ae0bb6b4c1c3db813f0fbdf78393c/LICENSE). It is inspired by [apriltags_ros](https://github.com/RIVeR-Lab/apriltags_ros) and provides a superset of its functionalities. diff --git a/apriltag_ros/CMakeLists.txt b/apriltag_ros/CMakeLists.txt index f1cd0f99..53e1b2a6 100644 --- a/apriltag_ros/CMakeLists.txt +++ b/apriltag_ros/CMakeLists.txt @@ -59,9 +59,6 @@ include_directories( include ) -# create ament index resource which references the libraries in the binary dir -set(node_plugins "") - # Add cpp libraries add_library(${PROJECT_NAME} SHARED "src/continuous_detector.cpp" @@ -91,26 +88,24 @@ ament_target_dependencies(${PROJECT_NAME} yaml-cpp geometry_msgs std_msgs - ) +) +# Register nodes rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "apriltag_ros::ContinuousDetector" EXECUTABLE ContinuousDetector) -set(node_plugins "${node_plugins}apriltag_ros::ContinuousDetector;$\n") - rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "apriltag_ros::SingleImageDetector" EXECUTABLE SingleImageDetector) -set(node_plugins "${node_plugins}apriltag_ros::SingleImageDetector;$\n") +# Build executables add_executable(single_image_client src/apriltag_ros_single_image_client_node.cpp) target_link_libraries(single_image_client ${PROJECT_NAME}) -# since the package installs libraries without exporting them -# it needs to make sure that the library path is being exported -if(NOT WIN32) - ament_environment_hooks( - "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") -endif() +add_executable(single_image_server src/apriltag_ros_single_image_server_node.cpp) +target_link_libraries(single_image_server ${PROJECT_NAME}) +# Install targets install(TARGETS ${PROJECT_NAME} + single_image_client + single_image_server DESTINATION lib/${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME} diff --git a/apriltag_ros/include/apriltag_ros/single_image_detector.hpp b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp index cb8840e8..5f96ef63 100644 --- a/apriltag_ros/include/apriltag_ros/single_image_detector.hpp +++ b/apriltag_ros/include/apriltag_ros/single_image_detector.hpp @@ -28,7 +28,7 @@ * policies, either expressed or implied, of the California Institute of * Technology. * - ** single_image_detector.h **************************************************** + * single_image_detector.hpp **************************************************** * * Wrapper class of TagDetector class which calls TagDetector::detectTags on a * an image stored at a specified load path and stores the output at a specified @@ -41,15 +41,14 @@ * Originator: Danylo Malyuta, JPL ******************************************************************************/ -#ifndef APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_H -#define APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_H +#ifndef APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_HPP +#define APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_HPP #include #include #include "apriltag_ros/common_functions.hpp" -#include "apriltag_ros/composition_visibility.h" #include "apriltag_ros_interfaces/srv/analyze_single_image.hpp" #include "apriltag_ros_interfaces/msg/april_tag_detection.hpp" #include "apriltag_ros_interfaces/msg/april_tag_detection_array.hpp" @@ -60,21 +59,14 @@ namespace apriltag_ros class SingleImageDetector { public: - COMPOSITION_PUBLIC - explicit SingleImageDetector(const rclcpp::NodeOptions & options); - - COMPOSITION_PUBLIC - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_base_interface() const; + SingleImageDetector(rclcpp::Node::SharedPtr node); // The function which provides the single image analysis service void analyzeImage(const std::shared_ptr request, std::shared_ptr response); - - rclcpp::Node::SharedPtr nh_; - rclcpp::QoS custom_qos_; private: std::shared_ptr tag_detector_; + rclcpp::Node::SharedPtr nh_; rclcpp::Service::SharedPtr single_image_analysis_service_; rclcpp::Publisher::SharedPtr tag_detections_publisher_; @@ -82,4 +74,4 @@ class SingleImageDetector } // namespace apriltag_ros -#endif // APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_H +#endif // APRILTAG_ROS_SINGLE_IMAGE_DETECTOR_HPP diff --git a/apriltag_ros/launch/launch_continuous.py b/apriltag_ros/launch/launch_continuous.py index 79ac5dbc..5345b29e 100644 --- a/apriltag_ros/launch/launch_continuous.py +++ b/apriltag_ros/launch/launch_continuous.py @@ -1,10 +1,3 @@ -""" - Copyright 2023 @ MOVVO ROBOTICS - --------------------------------------------------------- - Authors: Albert Arlà, Martí Bolet, Bernat Gaston, María Mercadé - Contact: support.idi@movvo.eu -""" - import os from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node, SetParameter diff --git a/apriltag_ros/launch/launch_single.py b/apriltag_ros/launch/launch_single.py index 00f285f5..0aa3e366 100644 --- a/apriltag_ros/launch/launch_single.py +++ b/apriltag_ros/launch/launch_single.py @@ -1,10 +1,3 @@ -""" - Copyright 2023 @ MOVVO ROBOTICS - --------------------------------------------------------- - Authors: Albert Arlà, Martí Bolet, Bernat Gaston, María Mercadé - Contact: support.idi@movvo.eu -""" - import os from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node, SetParameter diff --git a/apriltag_ros/launch/single_image_client_launch.py b/apriltag_ros/launch/single_image_client_launch.py new file mode 100644 index 00000000..809bcea5 --- /dev/null +++ b/apriltag_ros/launch/single_image_client_launch.py @@ -0,0 +1,26 @@ +import launch +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import Command, LaunchConfiguration +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + return LaunchDescription([ + Node( + package='apriltag_ros', + executable='single_image_client', + output='screen', + parameters=[ + {"fx": 652.7934615847107}, + {"fy": 653.9480389077635}, + {"cx": 307.1288710375904}, + {"cy": 258.7823279214385}, + ], + remappings=[], + arguments=[ + LaunchConfiguration('image_load_path'), + LaunchConfiguration('image_save_path') + ] + ), + ]) \ No newline at end of file diff --git a/apriltag_ros/launch/single_image_server_launch.py b/apriltag_ros/launch/single_image_server_launch.py new file mode 100644 index 00000000..24a3cdfb --- /dev/null +++ b/apriltag_ros/launch/single_image_server_launch.py @@ -0,0 +1,42 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, SetParameter +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration + +PACKAGE_NAME = 'apriltag_ros' + +def generate_launch_description(): + # Configuración de los archivos + config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "settings.yaml") + tags_config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "tags.yaml") + + # Argumento de lanzamiento + use_sim_time_arg = DeclareLaunchArgument( + name='use_sim_time', + default_value='False', + choices=['True', 'False'], + description='Parameter use_sim_time') + + # Substitución de lanzamiento + use_sim_time = LaunchConfiguration('use_sim_time') + + # Nodos para lanzar + node = Node( + package=PACKAGE_NAME, + executable='single_image_server', + output='screen', + emulate_tty=True, + ) + + return LaunchDescription([ + GroupAction( + actions=[ + use_sim_time_arg, + SetParameter("use_sim_time", use_sim_time), + SetParameter("tags_yaml_path", tags_config_file), + node + ] + ) + ]) \ No newline at end of file diff --git a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp index 990b4004..ec33a3ae 100644 --- a/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp +++ b/apriltag_ros/src/apriltag_ros_single_image_client_node.cpp @@ -32,73 +32,62 @@ #include "apriltag_ros/common_functions.hpp" #include "apriltag_ros_interfaces/srv/analyze_single_image.hpp" -bool getRosParameter (rclcpp::Node::SharedPtr nh, std::string name, double& param) -{ - // Write parameter "name" from ROS Parameter Server into param - // Return true if successful, false otherwise - if (pnh.hasParam(name.c_str())) - { - pnh.getParam(name.c_str(), param); - ROS_INFO_STREAM("Set camera " << name.c_str() << " = " << param); - return true; - } - else - { - ROS_ERROR_STREAM("Could not find " << name.c_str() << " parameter!"); - return false; - } -} - int main(int argc, char **argv) { - rclcpp::init(argc, argv); + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("single_image_client_node"); - std::shared_ptr node = rclcpp::Node::make_shared("single_image_client_node"); + auto client = node->create_client("single_image_tag_detection"); - rclcpp::Client::SharedPtr client = - node->create_client("single_image_tag_detection"); + auto request = std::make_shared(); + request->full_path_where_to_get_image = argv[1]; + request->full_path_where_to_save_image = argv[2]; - auto request = std::make_shared(); + // Replicate sensor_msgs/CameraInfo message + request->camera_info.distortion_model = "plumb_bob"; - // Write desired path where to get the image - request->full_path_where_to_get_image = "/full/path/of/image"; + // Declare parameters with default values + node->declare_parameter("fx", 652.7934615847107); + node->declare_parameter("fy", 653.9480389077635); + node->declare_parameter("cx", 307.1288710375904); + node->declare_parameter("cy", 258.7823279214385); - // Write desired path where to save the image - request->full_path_where_to_save_image = "/full/path/of/image"; + // Get parameters + double fx, fy, cx, cy; + node->get_parameter("fx", fx); + node->get_parameter("fy", fy); + node->get_parameter("cx", cx); + node->get_parameter("cy", cy); - // Write camera parameters - double fx = 0.0; - double fy = 0.0; - double cx = 0.0; - double cy = 0.0; - request->camera_info.K[0] = fx; - request->camera_info.K[2] = cx; - request->camera_info.K[4] = fy; - request->camera_info.K[5] = cy; - request->camera_info.K[8] = 1.0; - request->camera_info.P[0] = fx; - request->camera_info.P[2] = cx; - request->camera_info.P[5] = fy; - request->camera_info.P[6] = cy; - request->camera_info.P[10] = 1.0; + // Intrinsic camera matrix for the raw (distorted) images + request->camera_info.k[0] = fx; + request->camera_info.k[2] = cx; + request->camera_info.k[4] = fy; + request->camera_info.k[5] = cy; + request->camera_info.k[8] = 1.0; + request->camera_info.p[0] = fx; + request->camera_info.p[2] = cx; + request->camera_info.p[5] = fy; + request->camera_info.p[6] = cy; + request->camera_info.p[10] = 1.0; + + // Call the service (detect tags in the image specified by the + // image_load_path) + auto result_future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "Service call failed."); + return 1; + } - // Call the service (detect tags in the image specified by the - // image_load_path) - if (client.call(service)) - { - // use parameter run_quielty=false in order to have the service - // print out the tag position and orientation - if (service.response.tag_detections.detections.size() == 0) + auto result = result_future.get(); + auto tag_detections = result->tag_detections; + if (tag_detections.detections.size() == 0) { - ROS_WARN_STREAM("No detected tags!"); + RCLCPP_WARN(node->get_logger(), "No detected tags!"); } - } - else - { - ROS_ERROR("Failed to call service single_image_tag_detection"); - return 1; - } - rclcpp::shutdown(); - return 0; // happy ending + rclcpp::shutdown(); + return 0; // happy ending } diff --git a/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp b/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp new file mode 100644 index 00000000..6fa9e91b --- /dev/null +++ b/apriltag_ros/src/apriltag_ros_single_image_server_node.cpp @@ -0,0 +1,45 @@ +/** + * Copyright (c) 2017, California Institute of Technology. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * The views and conclusions contained in the software and documentation are + * those of the authors and should not be interpreted as representing official + * policies, either expressed or implied, of the California Institute of + * Technology. + */ + +#include "apriltag_ros/common_functions.hpp" +#include "apriltag_ros/single_image_detector.hpp" + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("apriltag_ros_single_image_server"); + + apriltag_ros::SingleImageDetector continuous_tag_detector(node); + + rclcpp::spin(node); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/apriltag_ros/src/single_image_detector.cpp b/apriltag_ros/src/single_image_detector.cpp index fdb251e1..c4d07796 100644 --- a/apriltag_ros/src/single_image_detector.cpp +++ b/apriltag_ros/src/single_image_detector.cpp @@ -29,26 +29,30 @@ * Technology. */ +#include + #include "apriltag_ros/single_image_detector.hpp" #include -#include +#include +#include -using namespace apriltag_ros; using std::placeholders::_1; using std::placeholders::_2; -SingleImageDetector::SingleImageDetector (const rclcpp::NodeOptions & options) -: nh_(std::make_shared("apriltag_node", options)), custom_qos_(1) +namespace apriltag_ros { - rclcpp::uninstall_signal_handlers(); +SingleImageDetector::SingleImageDetector(rclcpp::Node::SharedPtr node) + : nh_(node) +{ // Advertise the single image analysis service single_image_analysis_service_ = nh_->create_service("single_image_tag_detection", std::bind(&SingleImageDetector::analyzeImage, this, _1, _2)); tag_detections_publisher_ = nh_->create_publisher("tag_detections", 1); - RCLCPP_INFO(nh_->get_logger(),"Ready to do tag detection on single images"); + tag_detector_ = std::shared_ptr(new TagDetector(nh_)); + RCLCPP_INFO(nh_->get_logger(), "Ready to do tag detection on single images"); } void SingleImageDetector::analyzeImage( @@ -56,20 +60,21 @@ void SingleImageDetector::analyzeImage( std::shared_ptr response) { - RCLCPP_INFO(nh_->get_logger(),"[ Summoned to analyze image ]"); - RCLCPP_INFO(nh_->get_logger(),"Image load path: %s", + RCLCPP_INFO(nh_->get_logger(), "[ Summoned to analyze image ]"); + RCLCPP_INFO(nh_->get_logger(), "Image load path: %s", request->full_path_where_to_get_image.c_str()); - RCLCPP_INFO(nh_->get_logger(),"Image save path: %s", + RCLCPP_INFO(nh_->get_logger(), "Image save path: %s", request->full_path_where_to_save_image.c_str()); // Read the image cv::Mat image = cv::imread(request->full_path_where_to_get_image, cv::IMREAD_COLOR); - if (image.data == NULL) + if (image.empty()) { // Cannot read image - RCLCPP_ERROR(nh_->get_logger(),"Could not read image %s", + RCLCPP_ERROR(nh_->get_logger(), "Could not read image %s", request->full_path_where_to_get_image.c_str()); + response->success = false; return; } @@ -78,8 +83,7 @@ void SingleImageDetector::analyzeImage( "bgr8", image)); loaded_image->header.frame_id = "camera"; response->tag_detections = - tag_detector_->detectTags(loaded_image,sensor_msgs::msg::CameraInfo::ConstSharedPtr( - new sensor_msgs::msg::CameraInfo(request->camera_info))); + tag_detector_->detectTags(loaded_image, std::make_shared(request->camera_info)); // Publish detected tags (AprilTagDetectionArray, basically an array of // geometry_msgs/PoseWithCovarianceStamped) @@ -89,21 +93,9 @@ void SingleImageDetector::analyzeImage( tag_detector_->drawDetections(loaded_image); cv::imwrite(request->full_path_where_to_save_image, loaded_image->image); - RCLCPP_INFO(nh_->get_logger(),"Done!"); - -} - + RCLCPP_INFO(nh_->get_logger(), "Done!"); -rclcpp::node_interfaces::NodeBaseInterface::SharedPtr -SingleImageDetector::get_node_base_interface() const -{ - return this->nh_->get_node_base_interface(); + response->success = true; } - -#include "rclcpp_components/register_node_macro.hpp" - -// Register the component with class_loader. -// This acts as a sort of entry point, allowing the component to be discoverable when its library -// is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(apriltag_ros::SingleImageDetector) +} // namespace apriltag_ros diff --git a/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv b/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv index cfc11898..9398362b 100644 --- a/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv +++ b/apriltag_ros_interfaces/srv/AnalyzeSingleImage.srv @@ -11,4 +11,5 @@ string full_path_where_to_get_image string full_path_where_to_save_image sensor_msgs/CameraInfo camera_info --- -AprilTagDetectionArray tag_detections \ No newline at end of file +AprilTagDetectionArray tag_detections +bool success \ No newline at end of file From af814de89ecebeedab373aa3beb22f44ee615b3a Mon Sep 17 00:00:00 2001 From: mariamercade Date: Fri, 2 Feb 2024 09:35:34 +0100 Subject: [PATCH 8/9] small corrections --- README.md | 20 +++++++++++++++++++ .../include/apriltag_ros/common_functions.hpp | 6 +++--- .../apriltag_ros/continuous_detector.hpp | 6 +++--- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 9deec059..04929353 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,19 @@ For a correct depth estimation (and hence the correct full pose) it is necessary Below is a visualization of the tag size (red arrow) to be specified for the most common tag classes: ![Tag Size Guide](./apriltag_ros/docs/tag_size_guide.svg) +## Tags Parameter Setup + +The `config/tags.yaml` file defines the standalone tags and tag bundles that should be looked for in the input image. Rogue tags (i.e., those that are not defined in `config/tags.yaml`) will be ignored. **IMPORTANT**: + +- No tag ID should appear twice with different sizes (this creates ambiguity in the detection). +- No tag ID should appear twice in the image (this creates ambiguity in the detection). +- It is fine for a tag with the same ID to be listed both in `standalone_tags` and in `tag_bundles`, as long as it has the same size. + +Furthermore, make sure that you print your tags surrounded by at least a 1-bit wide white border. The core AprilTag 2 algorithm samples this surrounding white border for creating a light model over the tag surface so do not, e.g., cut or print the tags out flush with their black border. + +The `tag_bundles` are filled out with the relative poses of the constituent tags to the bundle origin. Sometimes you may know these transforms (e.g., you print the bundle on a single sheet of paper where you lay out the tags in a known fashion). When you do not know these relative poses (e.g., you stick individual tags roughly on the floor/wall), perform a calibration to easily determine them. + + ## Nodes 1. **apriltag_ros** @@ -122,6 +135,13 @@ ros2 launch apriltag_ros single_image_client_launch.py image_load_path:= @@ -242,4 +242,4 @@ class TagDetector } // namespace apriltag_ros -#endif // ATLAS_APRILTAG_ROS_COMMON_FUNCTIONS_H +#endif // APRILTAG_ROS_COMMON_FUNCTIONS_HPP diff --git a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp index 3f6966b5..e28c65d0 100644 --- a/apriltag_ros/include/apriltag_ros/continuous_detector.hpp +++ b/apriltag_ros/include/apriltag_ros/continuous_detector.hpp @@ -40,8 +40,8 @@ * Originator: Danylo Malyuta, JPL ******************************************************************************/ -#ifndef ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H -#define ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H +#ifndef APRILTAG_ROS_CONTINUOUS_DETECTOR_HPP +#define APRILTAG_ROS_CONTINUOUS_DETECTOR_HPP // C++ #include @@ -91,4 +91,4 @@ class ContinuousDetector } // namespace apriltag_ros -#endif // ATLAS_APRILTAG_ROS_CONTINUOUS_DETECTOR_H +#endif // APRILTAG_ROS_CONTINUOUS_DETECTOR_HPP From c67d72bfb538fe57ad4e2a4b4bdcf6917c5e9fd8 Mon Sep 17 00:00:00 2001 From: mariamercade Date: Fri, 2 Feb 2024 10:30:40 +0100 Subject: [PATCH 9/9] update settings.yaml to params.yaml --- README.md | 4 ++-- apriltag_ros/launch/single_image_server_launch.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 04929353..14658cfc 100644 --- a/README.md +++ b/README.md @@ -15,10 +15,10 @@ This package provides functionality for detecting AprilTags in both continuous i The behavior of the ROS wrapper is defined by two configuration files: - `config/tags.yaml`: Defines the tags and tag bundles to detect. -- `config/settings.yaml`: Configures the core Apriltag 3 algorithm parameters. +- `config/params.yaml`: Configures the core Apriltag 3 algorithm parameters. The package outputs the following topics: -- `/tf`: Provides the relative pose between the camera frame and each detected tag's or tag bundle's frame, specified in `tags.yaml`, using tf. This is published only if `publish_tf` is set to true in `settings.yaml`. +- `/tf`: Provides the relative pose between the camera frame and each detected tag's or tag bundle's frame, specified in `tags.yaml`, using tf. This is published only if `publish_tf` is set to true in `params.yaml`. - `/tag_detections`: Carries the same information as `/tf`, but as a custom message containing the tag ID(s), size(s), and `geometry_msgs/PoseWithCovarianceStamped` pose information (applies in the case of tag bundles). This topic is always published. - `/tag_detections_image`: Displays the same input image from `/camera/image_rect`, but with the detected tags highlighted. This is published only if `publish_tag_detections_image` is set to true in `config/params.yaml`. diff --git a/apriltag_ros/launch/single_image_server_launch.py b/apriltag_ros/launch/single_image_server_launch.py index 24a3cdfb..e5f79608 100644 --- a/apriltag_ros/launch/single_image_server_launch.py +++ b/apriltag_ros/launch/single_image_server_launch.py @@ -9,7 +9,7 @@ def generate_launch_description(): # Configuración de los archivos - config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "settings.yaml") + config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "params.yaml") tags_config_file = os.path.join(get_package_share_directory(PACKAGE_NAME), "config", "tags.yaml") # Argumento de lanzamiento