init bridge files

json_tf_bridge/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.8)
+project(json_tf_bridge)
+
+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(nlohmann_json 3.2.0 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(json_tf_bridge_node src/json_tf_bridge_node.cpp)
+target_link_libraries(json_tf_bridge_node nlohmann_json::nlohmann_json)
+ament_target_dependencies(
+        json_tf_bridge_node
+        geometry_msgs
+        std_msgs
+        rclcpp
+        tf2
+        tf2_ros
+)
+target_include_directories(json_tf_bridge_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(json_tf_bridge_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+
+install(TARGETS json_tf_bridge_node
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

json_tf_bridge/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>json_tf_bridge</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">pac48</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

json_tf_bridge/src/json_tf_bridge_node.cpp

@@ -0,0 +1,181 @@
+#include <functional>
+#include <memory>
+
+#include <nlohmann/json.hpp>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "std_msgs/msg/string.hpp"
+
+
+namespace nlohmann {
+
+///////////////////////////////////////////////////////////////////////////////
+// std::variant
+///////////////////////////////////////////////////////////////////////////////
+// Try to set the value of type T into the variant data if it fails, do nothing
+    template<typename T, typename... Ts>
+    void variant_from_json(const nlohmann::json &j, std::variant<Ts...> &data) {
+        try {
+            data = j.get<T>();
+        } catch (...) {
+        }
+    }
+
+    template<typename... Ts>
+    struct adl_serializer<std::variant<Ts...>> {
+        static void to_json(nlohmann::json &j, const std::variant<Ts...> &data) {
+            // Will call j = v automatically for the right type
+            std::visit([&j](const auto &v) { j = v; }, data);
+        }
+
+        static void from_json(const nlohmann::json &j, std::variant<Ts...> &data) {
+            // Call variant_from_json for all types, only one will succeed
+            (variant_from_json<Ts>(j, data), ...);
+        }
+    };
+
+///////////////////////////////////////////////////////////////////////////////
+// std::optional
+///////////////////////////////////////////////////////////////////////////////
+    template<class T>
+    void optional_to_json(nlohmann::json &j, const char *name, const std::optional<T> &value) {
+        if (value)
+            j[name] = *value;
+    }
+
+    template<class T>
+    void optional_from_json(const nlohmann::json &j, const char *name, std::optional<T> &value) {
+        const auto it = j.find(name);
+        if (it != j.end())
+            value = it->get<T>();
+        else
+            value = std::nullopt;
+    }
+
+///////////////////////////////////////////////////////////////////////////////
+// all together
+///////////////////////////////////////////////////////////////////////////////
+    template<typename>
+    constexpr bool is_optional = false;
+    template<typename T>
+    constexpr bool is_optional<std::optional<T>> = true;
+
+    template<typename T>
+    void extended_to_json(const char *key, nlohmann::json &j, const T &value) {
+        if constexpr (is_optional<T>)
+            nlohmann::optional_to_json(j, key, value);
+        else
+            j[key] = value;
+    }
+
+    template<typename T>
+    void extended_from_json(const char *key, const nlohmann::json &j, T &value) {
+        if constexpr (is_optional<T>)
+            nlohmann::optional_from_json(j, key, value);
+        else
+            j.at(key).get_to(value);
+    }
+
+}
+
+#define EXTEND_JSON_TO(v1) extended_to_json(#v1, nlohmann_json_j, nlohmann_json_t.v1);
+#define EXTEND_JSON_FROM(v1) extended_from_json(#v1, nlohmann_json_j, nlohmann_json_t.v1);
+
+#define NLOHMANN_JSONIFY_ALL_THINGS(Type, ...)                                          \
+  inline void to_json(nlohmann::json &nlohmann_json_j, const Type &nlohmann_json_t) {   \
+      NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(EXTEND_JSON_TO, __VA_ARGS__))            \
+  }                                                                                     \
+  inline void from_json(const nlohmann::json &nlohmann_json_j, Type &nlohmann_json_t) { \
+      NLOHMANN_JSON_EXPAND(NLOHMANN_JSON_PASTE(EXTEND_JSON_FROM, __VA_ARGS__))          \
+  }
+
+
+
+// for convenience
+using json = nlohmann::json;
+
+struct TransformData {
+    double posX;
+    double posY;
+    double posZ;
+    double quatW;
+    double quatX;
+    double quatY;
+    double quatZ;
+    int sec;
+    unsigned int nanosec;
+    std::string frameID;
+};
+
+NLOHMANN_JSONIFY_ALL_THINGS(TransformData, posX, posY, posZ, quatW, quatX, quatY, quatZ, sec, nanosec, frameID)
+
+class FramePublisher : public rclcpp::Node {
+public:
+    FramePublisher()
+            : Node("json_tf_bridge") {
+
+        // Initialize the transform broadcaster
+        tf_broadcaster_ =
+                std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+
+
+        subscription_ = this->create_subscription<std_msgs::msg::String>(
+                "unity_tf_bridge", 10,
+                std::bind(&FramePublisher::callback, this, std::placeholders::_1));
+
+        clock_ = rclcpp::Clock(rcl_clock_type_e::RCL_ROS_TIME);
+    }
+
+private:
+    void callback(const std::shared_ptr<std_msgs::msg::String> msg) {
+        auto json_msg = json::parse(msg->data);
+        auto transform_data_vec = json_msg.get<std::vector<TransformData>>();
+
+        for (auto transform_data: transform_data_vec) {
+
+            geometry_msgs::msg::TransformStamped t;
+
+            // Read message content and assign it to
+            // corresponding tf variables
+//            t.header.stamp = clock_.now();
+            t.header.stamp.sec = transform_data.sec;
+            t.header.stamp.nanosec = transform_data.nanosec;
+            t.header.frame_id = "odom";
+            t.child_frame_id = transform_data.frameID;
+
+            // Turtle only exists in 2D, thus we get x and y translation
+            // coordinates from the message and set the z coordinate to 0
+            t.transform.translation.y = -transform_data.posX;
+            t.transform.translation.z = transform_data.posY;
+            t.transform.translation.x = transform_data.posZ;
+
+            // For the same reason, turtle can only rotate around one axis
+            // and this why we set rotation in x and y to 0 and obtain
+            // rotation in z axis from the message
+            tf2::Quaternion q;
+            t.transform.rotation.w = -transform_data.quatW;
+            t.transform.rotation.y = -transform_data.quatX;
+            t.transform.rotation.z = transform_data.quatY;
+            t.transform.rotation.x = transform_data.quatZ;
+
+            // Send the transformation
+            tf_broadcaster_->sendTransform(t);
+        }
+    }
+
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    rclcpp::Clock clock_;
+};
+
+int main(int argc, char **argv) {
+
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<FramePublisher>());
+    rclcpp::shutdown();
+
+    return 0;
+}

ros_tcp_endpoint/.gitignore

@@ -0,0 +1,8 @@
+.DS_Store
+*.pyc
+.idea
+.coverage
+test-results/
+*~
+build
+devel

ros_tcp_endpoint/.pre-commit-config.yaml

@@ -0,0 +1,12 @@
+repos:
+-   repo: https://github.com/python/black
+    rev: 19.3b0
+    hooks:
+    -   id: black
+        args: [--line-length=100]
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v3.4.0
+    hooks:
+    -   id: trailing-whitespace
+        name: trailing-whitespace-markdown
+        types: [markdown]

ros_tcp_endpoint/.yamato/sonar.yml

@@ -0,0 +1,17 @@
+name: Sonarqube Standard Scan
+agent:
+    type: Unity::metal::macmini
+    image: package-ci/mac
+    flavor: m1.mac
+variables:
+    SONARQUBE_PROJECT_KEY: ai-robotics-endpoint-ros2
+commands:
+    - curl https://binaries.sonarsource.com/Distribution/sonar-scanner-cli/sonar-scanner-cli-4.6.2.2472-macosx.zip -o sonar-scanner-macosx.zip -L
+    - unzip sonar-scanner-macosx.zip -d ~/sonar-scanner
+    - ~/sonar-scanner/sonar-scanner-4.6.2.2472-macosx/bin/sonar-scanner -Dsonar.projectKey=$SONARQUBE_PROJECT_KEY -Dsonar.sources=. -Dsonar.host.url=$SONARQUBE_ENDPOINT_URL_PRD -Dsonar.login=$SONARQUBE_TOKEN_PRD
+triggers:
+    cancel_old_ci: true
+    expression: |
+        ((pull_request.target eq "main" OR pull_request.target eq "dev-ros2")
+        AND NOT pull_request.push.changes.all match "**/*.md") OR
+        (push.branch eq "main" OR push.branch eq "dev-ros2")

ros_tcp_endpoint/.yamato/yamato-config.yml

@@ -0,0 +1,33 @@
+name: Endpoint Unit Tests
+agent:
+    type: Unity::VM
+    image: robotics/ci-ros2-galactic-ubuntu20:v0.0.2-916903
+    flavor: i1.large
+variables:
+  # Coverage measured as a percentage (out of 100)
+  COVERAGE_EXPECTED: 3.5
+commands:
+    # run unit tests and save test results in /home/bokken/build/output/Unity-Technologies/ROS-TCP-Endpoint
+    - command: |
+        source /opt/ros/galactic/setup.bash && echo "ROS_DISTRO == galactic"
+        cd .. && mkdir -p ros_ws/src && cp -r ./ROS-TCP-Endpoint ros_ws/src
+        cd ros_ws && colcon build && source install/local_setup.bash
+        cd src/ROS-TCP-Endpoint
+        python3 -m pytest --cov=. --cov-report xml:./test-results/coverage.xml --cov-report html:./test-results/coverage.html test/
+    # check the test coverage
+    - command: |
+        linecoverage=$(head -2 test-results/coverage.xml | grep -Eo 'line-rate="[0-9]+([.][0-9]+)?"' | grep -Eo "[0-9]+([.][0-9]+)?")
+        echo "Line coverage: $linecoverage"
+        if (( $(echo "$linecoverage * 100.0 < $COVERAGE_EXPECTED" | bc -l) ));
+        then echo "ERROR: Code Coverage is under threshold of $COVERAGE_EXPECTED%" && exit 1
+        fi
+triggers:
+    cancel_old_ci: true
+    expression: |
+        ((pull_request.target eq "main-ros2" OR pull_request.target eq "dev-ros2")
+          AND NOT pull_request.push.changes.all match "**/*.md") OR
+        (push.branch eq "main-ros2" OR push.branch eq "dev-ros2")
+artifacts:
+    logs:
+        paths:
+            - "test-results/**/*"