Port to ROS2 foxy (draft)

- Builds + basics seem to run
- Tests need to be ported
- CI needs to be integrated
- Further tests especially for fiducial_slam are needed

aruco_detect/CMakeLists.txt

@@ -1,74 +1,105 @@
-
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(aruco_detect)
 
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  tf2_geometry_msgs
-  tf2_ros
-  tf2
-  visualization_msgs
-  vision_msgs
-  image_transport
-  cv_bridge
-  sensor_msgs
-  std_msgs
-  fiducial_msgs
-  dynamic_reconfigure
-)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+
+find_package(rclcpp REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED) 
+find_package(tf2 REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(fiducial_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+#find_package(dynamic_reconfigure REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
-generate_dynamic_reconfigure_options(cfg/DetectorParams.cfg)
+#generate_dynamic_reconfigure_options(cfg/DetectorParams.cfg)
+
+#catkin_package(INCLUDE_DIRS DEPENDS OpenCV)
 
-catkin_package(INCLUDE_DIRS DEPENDS OpenCV)
+
+set(DEPENDENCIES
+  rclcpp
+  sensor_msgs
+  std_msgs
+  std_srvs
+  visualization_msgs
+  vision_msgs
+  fiducial_msgs
+  geometry_msgs
+  cv_bridge
+  tf2_ros
+  image_transport
+)
 
 ###########
 ## Build ##
 ###########
 
 add_definitions(-std=c++11)
 
-include_directories(${catkin_INCLUDE_DIRS})
-include_directories(${OpenCV_INCLUDE_DIRS})
+#include_directories(${OpenCV_INCLUDE_DIRS})
 
 add_executable(aruco_detect src/aruco_detect.cpp)
 
-add_dependencies(aruco_detect ${${PROJECT_NAME}_EXPORTED_TARGETS}
-                 ${catkin_EXPORTED_TARGETS})
+#add_dependencies(aruco_detect ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
-target_link_libraries(aruco_detect ${catkin_LIBRARIES} ${OpenCV_LIBS})
+ament_target_dependencies(aruco_detect ${DEPENDENCIES} OpenCV)
 
 #############
 ## Install ##
 #############
 
-## Mark executables and/or libraries for installation
 install(TARGETS aruco_detect
-   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-catkin_install_python(PROGRAMS scripts/create_markers.py
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
 )
-install(DIRECTORY launch/
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-) 
+
+## Mark executables and/or libraries for installation
+# install(TARGETS aruco_detect
+#    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+install(PROGRAMS scripts/create_markers.py
+        DESTINATION lib/${PROJECT_NAME}
+)
+# install(DIRECTORY launch/
+#         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+# ) 
 
 ###########
 ## Tests ##
 ###########
 
-if(CATKIN_ENABLE_TESTING)
-        find_package(rostest REQUIRED)
+# if(CATKIN_ENABLE_TESTING)
+#         find_package(rostest REQUIRED)
 
-        # Tests need c++11
-        add_definitions(-std=c++11)
+#         # Tests need c++11
+#         add_definitions(-std=c++11)
 
-        add_rostest_gtest(aruco_images_test 
-          test/aruco_images.test 
-          test/aruco_images_test.cpp)
-        target_link_libraries(aruco_images_test ${catkin_LIBRARIES} ${OpenCV_LIBS})
-endif()
+#         add_rostest_gtest(aruco_images_test 
+#           test/aruco_images.test 
+#           test/aruco_images_test.cpp)
+#         target_link_libraries(aruco_images_test ${catkin_LIBRARIES} ${OpenCV_LIBS})
+# endif()
+
+################################################################################
+# Macro for ament package
+################################################################################
+ament_export_include_directories(include)
+ament_package()

aruco_detect/launch/aruco_detect.launch.py

@@ -0,0 +1,38 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('camera', default_value='camera'),
+        DeclareLaunchArgument('image', default_value='image'),
+        DeclareLaunchArgument('transport', default_value='compressed'),
+        DeclareLaunchArgument('fiducial_len', default_value='0.14'),
+        DeclareLaunchArgument('dictionary', default_value='7'),
+        DeclareLaunchArgument('do_pose_estimation', default_value='True'),
+        DeclareLaunchArgument('vis_msgs', default_value="False"),
+        DeclareLaunchArgument('ignore_fiducials', default_value=""),
+        DeclareLaunchArgument('fiducial_len_override', default_value=""),
+        Node(
+            package='aruco_detect',
+            namespace='aruco_detect',
+            executable='aruco_detect',
+            name='aruco_detect',
+            parameters=[
+                {"image_transport":LaunchConfiguration('transport')},
+                {"publish_images":True},
+                {"fiducial_len":LaunchConfiguration('fiducial_len')},
+                {"dictionary":LaunchConfiguration('dictionary')},
+                {"do_pose_estimation":LaunchConfiguration('do_pose_estimation')},
+                {"vis_msgs":LaunchConfiguration('vis_msgs')},
+                {"ignore_fiducials":LaunchConfiguration('ignore_fiducials')},
+                {"fiducial_len_override":LaunchConfiguration('fiducial_len_override')}
+            ],
+            #Not sure if the remapping is working properly TODO
+            remappings=[
+                ('camera/compressed', [LaunchConfiguration('camera'), '/',LaunchConfiguration('image'), '/', LaunchConfiguration('transport')]),
+                ('camera_info', [LaunchConfiguration('camera'), '/camera_info']),
+            ]
+        )
+    ])

aruco_detect/package.xml

@@ -11,9 +11,7 @@
 
   <author email="[email protected]">Jim Vaughan</author>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <depend>roscpp</depend>
+  <depend>rclcpp</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tf2</depend>
@@ -23,8 +21,15 @@
   <depend>sensor_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>fiducial_msgs</depend>
-  <depend>dynamic_reconfigure</depend>
-  <depend>python-cairosvg</depend>
-  <depend>python-joblib</depend>
+  <depend>std_srvs</depend>
+  <depend>geometry_msgs</depend>
+  <!--<depend>dynamic_reconfigure</depend>-->
+
+  <!-- <depend>python-cairosvg</depend> Installed via pip3 install cairosvg -->
+  <depend>python-joblib</depend> <!-- TODO Manually needed to install apt python3-joblib -->
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 
 </package>

aruco_detect/scripts/create_markers.py

@@ -1,8 +1,9 @@
-#!/usr/bin/python
+#!/usr/bin/python3
 
 import os, sys, argparse
 import subprocess
 import imp, importlib
+import cairosvg
 
 # Hack if a user has 'em' instead of 'empy' installed with pip
 # If the module em doesn't have the expand function use the full path
@@ -15,9 +16,11 @@
              break
 # For-else: else is called if loop doesn't break 
 else:
-    print "ERROR: could not find module em, please sudo apt install python-empy"
+    print("ERROR: could not find module em, please sudo apt install python-empy")
     exit(2)
 
+
+# TODO Needs `python3 -m pip install opencv-contrib-python` on Ubuntu 20
 import cv2
 import cv2.aruco as aruco
 
@@ -28,8 +31,8 @@
 def checkCmd(cmd, package):
     rc = os.system("which %s > /dev/null" % cmd)
     if rc != 0:
-        print """This utility requires %s. It can be installed by typing:
-    sudo apt install %s""" % (cmd, package)
+        print("""This utility requires %s. It can be installed by typing:
+    sudo apt install %s""" % (cmd, package))
         sys.exit(1)
 
 def genSvg(id, dicno, paper_size):
@@ -83,22 +86,24 @@ def genSvg(id, dicno, paper_size):
 """, {"id": id, "dicno": dicno, "paper_width": paper_size[0], "paper_height": paper_size[1], "fid_len": 140.0})
 
 def genMarker(i, dicno, paper_size):
-    print " Marker %d\r" % i,
+    print(" Marker %d\r" % i)
     sys.stdout.flush()
     aruco_dict = aruco.Dictionary_get(dicno)
     img = aruco.drawMarker(aruco_dict, i, 2000)
     cv2.imwrite("/tmp/marker%d.png" % i, img)
     svg = genSvg(i, dicno, paper_size)
-    cairo = subprocess.Popen(('cairosvg', '-f', 'pdf', '-o', '/tmp/marker%d.pdf' % i, '/dev/stdin'), stdin=subprocess.PIPE)
-    cairo.communicate(input=svg)
+
+    # This would cause errors with python3
+    #cairo = subprocess.Popen(('cairosvg', '-f', 'pdf', '-o', '/tmp/marker%d.pdf' % i, '/dev/stdin'), stdin=subprocess.PIPE)
+    #cairo.communicate(input=svg)
     # This way is faster than subprocess, but causes problems when cairosvg is installed from pip
     # because newer versions only support python3, and opencv3 from ros does not
-    # cairosvg.svg2pdf(bytestring=svg, write_to='/tmp/marker%d.pdf' % i)
+    cairosvg.svg2pdf(bytestring=svg, write_to='/tmp/marker%d.pdf' % i)
     os.remove("/tmp/marker%d.png" % i)
 
 if __name__ == "__main__":
     checkCmd("pdfunite", "poppler-utils")
-    checkCmd("cairosvg", "python-cairosvg")
+    #checkCmd("cairosvg", "python-cairosvg")
 
 
     parser = argparse.ArgumentParser(description='Generate Aruco Markers.')
@@ -135,11 +140,11 @@ def genMarker(i, dicno, paper_size):
         for i in markers:
             genMarker(i, dicno, paper_size)
 
-    print "Combining into %s" % outfile
+    print("Combining into %s" % outfile)
     os.system("pdfunite %s %s" % (" ".join(pdfs), outfile))
     for f in pdfs:
         os.remove(f)
 
-    print '\033[91m' + """After printing, please make sure that the long lines around the marker are
-EXACTLY 14.0 cm long. This is required for accurate position estimation.""" + '\033[0m'
+    print('\033[91m' + """After printing, please make sure that the long lines around the marker are
+EXACTLY 14.0 cm long. This is required for accurate position estimation.""" + '\033[0m')