Merge pull request #127 from vortexntnu/feature/aruco

Feature/aruco

.clang-format

@@ -0,0 +1,137 @@
+---
+Language:        Cpp
+# BasedOnStyle:  LLVM
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveMacros: false
+AlignConsecutiveAssignments: true
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Right
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: true
+AllowAllConstructorInitializersOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: MultiLine
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:
+  AfterCaseLabel:  false
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Stroustrup
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     160
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DeriveLineEnding: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Preserve
+IncludeCategories:
+  - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority:        2
+    SortPriority:    0
+  - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
+    Priority:        3
+    SortPriority:    0
+  - Regex:           '.*'
+    Priority:        1
+    SortPriority:    0
+IncludeIsMainRegex: '(Test)?$'
+IncludeIsMainSourceRegex: ''
+IndentCaseLabels: false
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 60
+PointerAlignment: Right
+ReflowComments:  true
+SortIncludes:    true
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles:  false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
+Standard:        Latest
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        2
+UseCRLF:         false
+UseTab:          ForIndentation
+...
+

.devcontainer/devcontainer.json

@@ -0,0 +1,24 @@
+// For format details, see https://aka.ms/devcontainer.json. For config options, see the
+// README at: https://github.com/devcontainers/templates/tree/main/src/cpp
+
+
+
+// Config for using vscode with the docker-container
+{
+	"name": "Development Container",
+	"dockerComposeFile": "../docker/docker-compose.yml",
+	"service": "cv-aruco",
+	"workspaceFolder": "/vortex_ws/src/vortex_cv",
+
+	// Configure tool-specific properties.
+	"customizations": {
+		"vscode": {
+			"settings": {},
+			"extensions": [
+				"ms-vscode.cpptools-extension-pack",
+				"ms-iot.vscode-ros"
+			]
+		}
+	}
+
+}

.github/workflows/clang-formatter.yml

@@ -1,7 +1,13 @@
 name: Run clang-format Linter
 
-on: [push]
+on: 
+  push:
+    branches: [ main ]
+  workflow_dispatch:
 
+  pull_request:
+    types: [opened, reopened]
+    branches: [ main ]
 jobs:
   build:
     runs-on: ubuntu-latest

.github/workflows/docker-publish.yml

@@ -2,8 +2,13 @@ name: Publish Docker image to ghcr
 
 on:
   push:
+    branches: [ main ]
   workflow_dispatch:
 
+  pull_request:
+    types: [opened, reopened]
+    branches: [ main ]
+
 env:
   REGISTRY: ghcr.io
   IMAGE_NAME: ${{ github.repository }}

.gitignore

@@ -132,4 +132,9 @@ dmypy.json
 .pyre/
 
 # local
-test.py
+test.py
+
+
+# vs-code
+.vscode/browse*
+.vscode/c_cpp_properties.json

.vscode/launch.json

@@ -0,0 +1,15 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [        
+        {
+            "name": "ROS: Attach",
+            "type": "ros",
+            "request": "attach"
+
+
+        }
+    ]
+}

.vscode/tasks.json

@@ -0,0 +1,33 @@
+{
+    "version": "2.0.0",
+    "tasks": [
+        {
+            "label": "catkin_build",
+            "type": "shell",
+            "command": ". /opt/ros/melodic/setup.bash && catkin build -DCMAKE_BUILD_TYPE=Debug && . devel/setup.bash",
+
+            "options": {
+                "cwd": "${workspaceFolder}/../.."
+            },
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            },
+            "presentation": {
+                "reveal": "silent"
+            },
+            "problemMatcher": "$catkin-gcc"
+        },
+        {
+            "label": "clean_build",
+            "type": "shell",
+            "command": "catkin clean -y",
+            "options": {
+                "cwd": "${workspaceFolder}/../.."
+            },
+            "presentation": {
+                "reveal": "silent"
+            }
+        }
+    ]
+}

aruco_detection/CMakeLists.txt

@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(aruco_detection)
+
+
+find_package(catkin REQUIRED 
+  COMPONENTS
+    cv_bridge
+    image_transport
+    sensor_msgs
+    std_msgs
+    vortex_msgs
+    roscpp
+    tf2_ros
+)
+
+
+find_package(OpenCV 4.2.0 REQUIRED 
+  COMPONENTS 
+    core 
+    imgproc 
+    highgui
+    aruco
+    calib3d
+    xphoto
+)
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES 
+    aruco_detection
+  CATKIN_DEPENDS 
+    cv_bridge 
+    image_transport 
+    roscpp 
+    sensor_msgs 
+    std_msgs 
+    vortex_msgs
+  DEPENDS 
+    OpenCV
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+#########################################################
+
+add_executable(${PROJECT_NAME}_docking_node 
+  src/aruco_detection_node.cpp
+  src/aruco_handler.cpp
+)
+target_link_libraries(${PROJECT_NAME}_docking_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+)
+
+#########################################################
+
+add_executable(${PROJECT_NAME}_id_node 
+  src/aruco_id_node.cpp
+  src/file_handler.cpp
+)
+target_link_libraries(${PROJECT_NAME}_id_node
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBS}
+)
+
+#########################################################
+

aruco_detection/include/aruco_detection_node.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h> // Read and publish images
+
+#include <geometry_msgs/TransformStamped.h>      // For frame transforms
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // For frame transforms
+#include <tf2_ros/buffer.h>                      // For frame transforms
+#include <tf2_ros/transform_broadcaster.h>       // For frame transforms
+#include <tf2_ros/transform_listener.h>          // For frame transforms
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/aruco.hpp>
+#include <opencv2/core.hpp>
+#include <vector>
+
+#include <vortex_msgs/ObjectPosition.h>
+#include <aruco_handler.hpp>
+
+class ArucoDetectionNode {
+
+public:
+	ArucoDetectionNode();
+	/**
+	 * The callback function for the op_sub-subscriber.
+	 */
+	void callback(const sensor_msgs::ImageConstPtr &img_source);
+
+	/**
+	 * ros::spinOnce() is called at 10Hz
+	 */
+	void execute();
+
+	void publishCVImg(const cv::Mat &img, ros::Time timestamp);
+	void publishPose(const geometry_msgs::Pose &pose, ros::Time timestamp);
+
+protected:
+	// ROS stuff
+	ros::NodeHandle node;
+	ros::Rate loop_rate;
+	ros::Subscriber opImageSub;
+	ros::Publisher opImagePub;
+	ros::Publisher opPosePubUDFC;
+	ros::Publisher opPosePubODOM;
+	ros::Publisher opObjPubUDFC;
+	ros::Publisher opObjPubODOM;
+
+	// ArUco stuff
+	ArucoHandler arucoHandler;
+	cv::Ptr<cv::aruco::Dictionary> dictionary;
+	cv::Ptr<cv::aruco::Board> board;
+
+	// TF stuff
+	geometry_msgs::TransformStamped odom_udfc_transform;
+	tf2_ros::Buffer tfBuffer;
+	tf2_ros::TransformListener tfListener;
+	tf2_ros::TransformBroadcaster tfBroadcaster;
+};