Merge branch 'main' of github.com:vortexntnu/vortex-freya-sim

.github/workflows/docker-publish.yml

@@ -0,0 +1,55 @@
+name: Docker build and publish
+on:
+  push:
+    branches: [ "main" ]
+  pull_request:
+    branches: [ "main" ]
+env:
+  REGISTRY: ghcr.io
+  IMAGE_NAME: ${{ github.repository }}
+jobs:
+  build:
+    runs-on: ubuntu-22.04
+    permissions:
+      contents: read
+      packages: write
+      id-token: write
+    steps:
+      - name: Checkout this repository
+        uses: actions/checkout@v4
+        with:
+          path: vortex-freya-sim
+
+      - name: Checkout vortex-msgs repository
+        uses: actions/checkout@v4
+        with:
+          repository: vortexntnu/vortex-msgs
+          path: vortex-freya-sim/vortex_msgs
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@f95db51fddba0c2d1ec667646a06c2ce06100226 # v3.0.0
+
+      - name: Log into registry ${{ env.REGISTRY }}
+        if: github.event_name != 'pull_request'
+        uses: docker/login-action@343f7c4344506bcbf9b4de18042ae17996df046d # v3.0.0
+        with:
+          registry: ${{ env.REGISTRY }}
+          username: ${{ github.actor }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Extract Docker metadata
+        id: meta
+        uses: docker/metadata-action@96383f45573cb7f253c731d3b3ab81c87ef81934 # v5.0.0
+        with:
+          images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}
+
+      - name: Build and push Docker image
+        id: build-and-push
+        uses: docker/build-push-action@0565240e2d4ab88bba5387d719585280857ece09 # v5.0.0
+        with:
+          context: vortex-freya-sim
+          push: ${{ github.event_name != 'pull_request' }}
+          tags: ${{ steps.meta.outputs.tags }}
+          labels: ${{ steps.meta.outputs.labels }}
+          cache-from: type=gha
+          cache-to: type=gha,mode=max

Dockerfile

@@ -0,0 +1,22 @@
+FROM ros:humble-perception
+
+RUN DEBIAN_FRONTEND=noninteractive
+
+# Default to using bash > sh
+RUN ln -sf /bin/bash /bin/sh
+
+# Install Gazebo Garden
+RUN apt-get update && apt-get install -y lsb-release curl gnupg
+RUN curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+RUN apt-get update && apt-get install -y gz-garden python3-sdformat13 ros-humble-ros-gzgarden ros-humble-xacro
+
+WORKDIR /ros_ws
+
+RUN mkdir -p /ros_ws/src
+
+COPY . /ros_ws/src
+
+RUN source /opt/ros/humble/setup.bash && cd /ros_ws && colcon build --merge-install
+
+ENTRYPOINT ["/bin/bash", "-c", "/ros_ws/src/vortex_sim_interface/docker_entrypoint.sh"]

docker_entrypoint.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+source /opt/ros/humble/setup.bash && source /ros_ws/install/setup.bash && ros2 launch vrx_gz competition.launch.py world:=sydney_regatta

vortex_sim_interface/package.xml

@@ -13,6 +13,7 @@
   <depend>std_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>ros2launch</depend>

vortex_sim_interface/src/vortex_sim_interface.cpp

@@ -8,6 +8,7 @@
 #include "tf2_ros/transform_broadcaster.h"
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <geometry_msgs/msg/point.hpp>
 #include <vector>
 #include <cmath>
 
@@ -74,6 +75,8 @@ class VortexSimInterface : public rclcpp::Node
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcl_pub_;
 
     bool map_tf_set_ = false;
+    bool first_odom_received_ = false;
+    geometry_msgs::msg::Point initial_position_;
 
     void thruster_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
     {
@@ -94,14 +97,19 @@ class VortexSimInterface : public rclcpp::Node
 
     void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
     {
+        if (!first_odom_received_) {
+            initial_position_ = msg->pose.pose.position;
+            first_odom_received_ = true;
+        }
+
         auto ned_msg = std::make_unique<nav_msgs::msg::Odometry>();
 
         ned_msg->header = msg->header;
         ned_msg->child_frame_id = "wamv/wamv/base_link";
 
-        ned_msg->pose.pose.position.x = msg->pose.pose.position.y;
-        ned_msg->pose.pose.position.y = msg->pose.pose.position.x;
-        ned_msg->pose.pose.position.z = -msg->pose.pose.position.z;
+        ned_msg->pose.pose.position.x = msg->pose.pose.position.y - initial_position_.y;
+        ned_msg->pose.pose.position.y = msg->pose.pose.position.x - initial_position_.x;
+        ned_msg->pose.pose.position.z = -msg->pose.pose.position.z - initial_position_.z;
 
         ned_msg->pose.pose.orientation = enu_to_ned_quaternion(msg->pose.pose.orientation);