UbiquityRobotics · agutenkunst · Dec 15, 2020 · Apr 9, 2021 · Apr 9, 2021 · Apr 9, 2021
diff --git a/.gitignore b/.gitignore
@@ -1,2 +1,4 @@
 *~
 *.pyc
+*.db3-shm
+*.db3-wal
diff --git a/aruco_detect/CHANGELOG.rst b/aruco_detect/CHANGELOG.rst
@@ -1,6 +1,10 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package aruco_detect
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+FORTHCOMING
+-----------
+* Port to ROS2
+* Contributors: Alexander Gutenkunst
 
 0.11.0 (2019-05-09)
 -------------------

diff --git a/aruco_detect/CMakeLists.txt b/aruco_detect/CMakeLists.txt
@@ -1,74 +1,131 @@
-
-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 ##
 ###########
 
+include_directories(include)
 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}
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
 )
 
-catkin_install_python(PROGRAMS scripts/create_markers.py
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+## 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
-) 
+# install(DIRECTORY launch/
+#         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+# ) 
 
 ###########
 ## Tests ##
 ###########
 
-if(CATKIN_ENABLE_TESTING)
-        find_package(rostest REQUIRED)
+if(BUILD_TESTING)
+
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros_testing REQUIRED)
+
+  # Not sure if this is correct
+  install(DIRECTORY test/test_images
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+  ament_add_gmock(
+    helpers_test
+    test/helpers_test.cpp
+  )
+
+  ament_add_gtest_executable(
+    aruco_images_test
+    test/aruco_images_test.cpp
+  )
+  target_link_libraries(aruco_images_test)
+  ament_target_dependencies(aruco_images_test ${DEPENDENCIES})
+  add_ros_test(test/aruco_images.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+endif()
+
+# 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()
diff --git a/aruco_detect/README.md b/aruco_detect/README.md
@@ -6,3 +6,13 @@ It is based on the [Aruco](http://docs.opencv.org/trunk/d5/dae/tutorial_aruco_de
 contributed module to OpenCV. It is an alternative to fiducial_detect
 
 Documentation is in [the ROS wiki page](http://wiki.ros.org/aruco_detect).
+
+### Running the tests
+```
+colcon build --packages-select aruco_detect && colcon test --packages-select aruco_detect && colcon test-result
+```
+
+With output to the console
+```
+colcon build --packages-select aruco_detect && colcon test --packages-select aruco_detect --event-handlers console_direct+ && colcon test-result
+```
diff --git a/aruco_detect/cfg/fiducials.rviz2.rviz b/aruco_detect/cfg/fiducials.rviz2.rviz
@@ -0,0 +1,153 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 279
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Image1
+        - /Image1/Topic1
+        - /Image2
+      Splitter Ratio: 0.38282209634780884
+    Tree Height: 212
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 1
+        Durability Policy: Volatile
+        History Policy: Keep All
+        Reliability Policy: Best Effort
+        Value: /fiducial_images
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/image
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: camera
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Angle: -1.6349999904632568
+      Class: rviz_default_plugins/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 123.80565643310547
+      Target Frame: map
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: -0.10451977699995041
+      Y: 0.5637970566749573
+    Saved:
+      - Class: rviz_default_plugins/ThirdPersonFollower
+        Distance: 15.140737533569336
+        Enable Stereo Rendering:
+          Stereo Eye Separation: 0.05999999865889549
+          Stereo Focal Distance: 1
+          Swap Stereo Eyes: false
+          Value: false
+        Focal Point:
+          X: 0
+          Y: 0.4169340133666992
+          Z: -0.00839654728770256
+        Focal Shape Fixed Size: true
+        Focal Shape Size: 0.05000000074505806
+        Invert Z Axis: false
+        Name: ThirdPersonFollower
+        Near Clip Distance: 0.009999999776482582
+        Pitch: 1.5347963571548462
+        Target Frame: base_footprint
+        Value: ThirdPersonFollower (rviz_default_plugins)
+        Yaw: 3.1184024810791016
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1136
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000033100000416fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000228000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000026b000000e40000002800fffffffb0000000a0049006d0061006700650100000355000000fe0000002800ffffff000000010000010f00000416fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000416000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002060000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1341
+  X: 72
+  Y: 27
diff --git a/aruco_detect/include/helpers.h b/aruco_detect/include/helpers.h
@@ -0,0 +1,13 @@
+#include <vector>
+#include <string>
+
+void split(std::vector<std::string> &res, const std::string &str, char delim)
+{
+  size_t start;
+  size_t end = 0;
+  while ((start = str.find_first_not_of(delim, end)) != std::string::npos)
+  {
+    end = str.find(delim, start);
+	res.push_back(str.substr(start, end - start));
+  }
+}