Week 1 Training - Coordinate Transform

.devcontainer/Dockerfile

@@ -0,0 +1,18 @@
+FROM mcr.microsoft.com/devcontainers/cpp:1-debian12
+
+ARG REINSTALL_CMAKE_VERSION_FROM_SOURCE="none"
+
+# Optionally install the cmake for vcpkg
+COPY ./reinstall-cmake.sh /tmp/
+
+RUN if [ "${REINSTALL_CMAKE_VERSION_FROM_SOURCE}" != "none" ]; then \
+        chmod +x /tmp/reinstall-cmake.sh && /tmp/reinstall-cmake.sh ${REINSTALL_CMAKE_VERSION_FROM_SOURCE}; \
+    fi \
+    && rm -f /tmp/reinstall-cmake.sh
+
+# [Optional] Uncomment this section to install additional vcpkg ports.
+# RUN su vscode -c "${VCPKG_ROOT}/vcpkg install <your-port-name-here>"
+
+# [Optional] Uncomment this section to install additional packages.
+# RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \
+#     && apt-get -y install --no-install-recommends <your-package-list-here>

.devcontainer/devcontainer.json

@@ -0,0 +1,23 @@
+// 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
+{
+	"name": "C++",
+	"build": {
+		"dockerfile": "Dockerfile"
+	}
+
+	// Features to add to the dev container. More info: https://containers.dev/features.
+	// "features": {},
+
+	// Use 'forwardPorts' to make a list of ports inside the container available locally.
+	// "forwardPorts": [],
+
+	// Use 'postCreateCommand' to run commands after the container is created.
+	// "postCreateCommand": "gcc -v",
+
+	// Configure tool-specific properties.
+	// "customizations": {},
+
+	// Uncomment to connect as root instead. More info: https://aka.ms/dev-containers-non-root.
+	// "remoteUser": "root"
+}

.devcontainer/reinstall-cmake.sh

@@ -0,0 +1,62 @@
+#!/usr/bin/env bash
+#-------------------------------------------------------------------------------------------------------------
+# Copyright (c) Microsoft Corporation. All rights reserved.
+# Licensed under the MIT License. See https://go.microsoft.com/fwlink/?linkid=2090316 for license information.
+#-------------------------------------------------------------------------------------------------------------
+#
+set -e
+
+CMAKE_VERSION=${1:-"none"}
+
+if [ "${CMAKE_VERSION}" = "none" ]; then
+    echo "No CMake version specified, skipping CMake reinstallation"
+    exit 0
+fi
+
+# Cleanup temporary directory and associated files when exiting the script.
+cleanup() {
+    EXIT_CODE=$?
+    set +e
+    if [[ -n "${TMP_DIR}" ]]; then
+        echo "Executing cleanup of tmp files"
+        rm -Rf "${TMP_DIR}"
+    fi
+    exit $EXIT_CODE
+}
+trap cleanup EXIT
+
+
+echo "Installing CMake..."
+apt-get -y purge --auto-remove cmake
+mkdir -p /opt/cmake
+
+architecture=$(dpkg --print-architecture)
+case "${architecture}" in
+    arm64)
+        ARCH=aarch64 ;;
+    amd64)
+        ARCH=x86_64 ;;
+    *)
+        echo "Unsupported architecture ${architecture}."
+        exit 1
+        ;;
+esac
+
+CMAKE_BINARY_NAME="cmake-${CMAKE_VERSION}-linux-${ARCH}.sh"
+CMAKE_CHECKSUM_NAME="cmake-${CMAKE_VERSION}-SHA-256.txt"
+TMP_DIR=$(mktemp -d -t cmake-XXXXXXXXXX)
+
+echo "${TMP_DIR}"
+cd "${TMP_DIR}"
+
+curl -sSL "https://github.com/Kitware/CMake/releases/download/v${CMAKE_VERSION}/${CMAKE_BINARY_NAME}" -O
+curl -sSL "https://github.com/Kitware/CMake/releases/download/v${CMAKE_VERSION}/${CMAKE_CHECKSUM_NAME}" -O
+
+sha256sum -c --ignore-missing "${CMAKE_CHECKSUM_NAME}"
+sh "${TMP_DIR}/${CMAKE_BINARY_NAME}" --prefix=/opt/cmake --skip-license
+
+ln -s /opt/cmake/bin/ccmake /usr/local/bin/ccmake
+ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake
+ln -s /opt/cmake/bin/cmake-gui /usr/local/bin/cmake-gui
+ln -s /opt/cmake/bin/cpack /usr/local/bin/cpack
+ln -s /opt/cmake/bin/ctest /usr/local/bin/ctest

.github/dependabot.yml

@@ -0,0 +1,12 @@
+# To get started with Dependabot version updates, you'll need to specify which
+# package ecosystems to update and where the package manifests are located.
+# Please see the documentation for more information:
+# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates
+# https://containers.dev/guide/dependabot
+
+version: 2
+updates:
+ - package-ecosystem: "devcontainers"
+   directory: "/"
+   schedule:
+     interval: weekly

.gitignore

@@ -14,3 +14,7 @@ build
 data
 
 wiki edit
+
+build/
+install/
+log/

.vscode/settings.json

@@ -64,5 +64,6 @@
         "xtr1common": "cpp",
         "xtree": "cpp",
         "xutility": "cpp"
-    }
+    },
+    "cmake.sourceDirectory": "/workspaces/software-training/coordinate_transform"
 }

coordinate_transform/src/coordinate_transform_component.cpp

@@ -23,6 +23,8 @@
 #include <tf2_ros/transform_listener.h>
 #include <string>
 // BEGIN STUDENT CODE
+#include <array>
+#include <vector>
 // END STUDENT CODE
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
@@ -80,6 +82,33 @@ class CoordinateTransformComponent : public rclcpp::Node
       getTransformationMatrixForOpticalFrame();
 
     // BEGIN STUDENT CODE
+    std::vector<stsl_interfaces::msg::Tag> new_tags;
+
+    for(const auto tag : tag_array_msg->tags) {
+        stsl_interfaces::msg::Tag new_tag;
+        new_tag.id = tag.id;
+
+        Eigen::Vector4d position(
+            tag.pose.position.x,
+            tag.pose.position.y,
+            tag.pose.position.z,
+            1
+        );
+
+        position = camera_to_base_transform * camera_optical_to_conventional_transform * position;
+
+        new_tag.pose.position.x = position.x();
+        new_tag.pose.position.y = position.y();
+        new_tag.pose.position.z = position.z();
+
+        Eigen::Matrix4d tag_orientation = quaternionMessageToTransformationMatrix(tag.pose.orientation);
+
+        tag_orientation = camera_to_base_transform * camera_optical_to_conventional_transform * tag_orientation;
+
+        new_tag.pose.orientation = transformationMatrixToQuaternionMessage(tag_orientation);
+
+        new_tags.push_back(new_tag);
+    }
     // END STUDENT CODE
 
     // create a new tag array message
@@ -91,6 +120,7 @@ class CoordinateTransformComponent : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // set message tags to new_tags vector
+    new_tag_array_msg.tags = new_tags;
     // END STUDENT CODE
 
     // publish new tag message
@@ -100,7 +130,26 @@ class CoordinateTransformComponent : public rclcpp::Node
   Eigen::Matrix4d getTransformationMatrixForOpticalFrame()
   {
     // BEGIN STUDENT CODE
-    return {};
+    // Rotation about the X axis by pi/2
+    std::array<double, 16> R_roll_data = {
+        1, 0, 0, 0,
+        0, 0, -1, 0,
+        0, 1, 0, 0,
+        0, 0, 0, 1
+    };
+
+    // Rotation about the Z axis by pi/2
+    std::array<double, 16> R_yaw_data = {
+        0, -1, 0, 0,
+        1, 0, 0, 0,
+        0, 0, 1, 0,
+        0, 0, 0, 1
+    };
+
+    Eigen::Matrix4d R_roll(R_roll_data.data());
+    Eigen::Matrix4d R_yaw(R_yaw_data.data());
+
+    return R_yaw * R_roll;
     // END STUDENT CODE
   }
 
@@ -121,4 +170,4 @@ class CoordinateTransformComponent : public rclcpp::Node
 };
 }  // namespace coordinate_transform
 
-RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent)
+RCLCPP_COMPONENTS_REGISTER_NODE(coordinate_transform::CoordinateTransformComponent)

install/.colcon_install_layout

@@ -0,0 +1 @@
+isolated