Initial commit

.github/workflows/docker-build-run.yml

@@ -0,0 +1,45 @@
+name: Docker Build and Run
+
+# Trigger the workflow on every push or pull request to the 'main' branch
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+    branches:
+      - main
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+    # Checkout the repository code
+    - name: Checkout repository
+      uses: actions/checkout@v3
+
+    # Set up Docker Buildx (to enable cross-platform builds, if necessary)
+    - name: Set up Docker Buildx
+      uses: docker/setup-buildx-action@v2
+
+    # Login to GitHub Docker registry (optional, for private registries)
+    # Uncomment the following lines if you need to push to GitHub Container Registry
+    # - name: Log in to GitHub Container Registry
+    #   uses: docker/login-action@v2
+    #   with:
+    #     registry: ghcr.io
+    #     username: ${{ github.repository_owner }}
+    #     password: ${{ secrets.GITHUB_TOKEN }}
+
+    # Build the Docker image
+    - name: Build Docker image
+      run: docker build -t ros2_humble_mvsim_demo:latest .
+
+    # Run the Docker container
+    - name: Run Docker container
+      run: docker run --rm -it ros2_humble_mvsim_demo:latest
+
+    # (Optional) Run tests inside the Docker container if you want
+    # - name: Run tests
+    #   run: docker run my-docker-image:latest ./run-tests.sh
+

Dockerfile

@@ -0,0 +1,30 @@
+# Dockerfile for ROS 2 Humble + mvsim
+FROM ros:humble
+
+# Install necessary dependencies for mvsim
+RUN apt-get update && apt-get install -y \
+    ros-humble-mvsim \
+    python3-colcon-common-extensions \
+    python3-pip \
+    && rm -rf /var/lib/apt/lists/*
+
+# Create working directories
+WORKDIR /ros2_ws/src
+
+# Copy the launch.py and the Python script to the image
+COPY launch_mvsim.launch.py /ros2_ws/src/
+COPY robot_commander.py /ros2_ws/src/
+
+# Return to the workspace root directory
+WORKDIR /ros2_ws
+
+# Build the workspace
+RUN colcon build
+
+# Source ROS 2 upon starting the container
+RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
+RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc
+RUN echo "ros2 launch src/launch_mvsim.launch.py" >> ~/.bashrc
+
+# Default command when starting the container
+CMD ["/bin/bash"]

build.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+docker build -t ros2_humble_mvsim_demo:latest .

launch_mvsim.launch.py

@@ -0,0 +1,55 @@
+# launch_mvsim.launch.py
+from launch import LaunchDescription
+from launch.substitutions import TextSubstitution
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    mvsimDir = get_package_share_directory("mvsim")
+    # print('mvsimDir: ' + mvsimDir)
+
+    # args that can be set from the command line or a default will be used
+    world_file_launch_arg = DeclareLaunchArgument(
+        "world_file", default_value=TextSubstitution(
+            text=os.path.join(mvsimDir, 'mvsim_tutorial', 'demo_warehouse.world.xml')))
+
+    headless_launch_arg = DeclareLaunchArgument(
+        "headless", default_value='True')
+
+    do_fake_localization_arg = DeclareLaunchArgument(
+        "do_fake_localization", default_value='True', description='publish tf odom -> base_link')
+
+    mvsim_node = Node(
+        package='mvsim',
+        executable='mvsim_node',
+        name='mvsim',
+        output='screen',
+        parameters=[
+            os.path.join(mvsimDir, 'mvsim_tutorial',
+                         'mvsim_ros2_params.yaml'),
+            {
+                "world_file": LaunchConfiguration('world_file'),
+                "headless": LaunchConfiguration('headless'),
+                "do_fake_localization": LaunchConfiguration('do_fake_localization'),
+            }]
+    )
+
+    return LaunchDescription([
+        world_file_launch_arg,
+        headless_launch_arg,
+        do_fake_localization_arg,
+        mvsim_node,
+        # Node to execute the Python script
+        Node(
+            # package='ros2_ws',  # Change this if the script is in another package
+            executable='/ros2_ws/src/robot_commander.py',
+            name='robot_commander',
+            output='screen'
+        )
+    ])

robot_commander.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from tf2_ros import TransformListener, Buffer
+from sensor_msgs.msg import LaserScan
+import time
+import subprocess
+from tf2_ros import Buffer, TransformListener
+
+
+class RobotCommander(Node):
+    def __init__(self):
+        super().__init__('robot_commander')
+        # Publisher to send velocity commands
+        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
+        # Buffer to store transformations
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.get_logger().info("RobotCommander initialized")
+
+        # Subscribe to the /scanner1 LaserScan topic
+        self.scan_sub = self.create_subscription(
+            LaserScan,
+            '/scanner1',
+            self.scan_callback,
+            10
+        )
+
+        # Create a TF buffer and listener
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.iter = 0
+
+        # Wait a bit for frames to populate
+        self.create_timer(5.0, self.on_timer)  # Run after 2 seconds
+
+    def on_timer(self):
+        try:
+            self.iter += 1
+
+            # Run different code based on the current value of the counter
+            if self.iter == 1:
+                # Publish a velocity command
+                twist = Twist()
+                twist.linear.x = 1.0  # Move forward
+                twist.angular.z = 0.0  # No rotation
+
+                self.cmd_vel_pub.publish(twist)
+                self.get_logger().info("Velocity command sent: move forward")
+                self.get_logger().info(
+                    f"INITIAL LIDAR RANGE: {self.middle_range}")
+
+            elif self.iter == 2:
+                # Publish a velocity command
+                twist = Twist()
+                twist.linear.x = 0.0  # Stop
+                twist.angular.z = 0.0  # No rotation
+
+                self.cmd_vel_pub.publish(twist)
+                self.get_logger().info("Velocity command sent: stop")
+                self.get_logger().info(
+                    f"FINAL LIDAR RANGE: {self.middle_range}")
+
+            elif self.iter == 3:
+                # For debugging, you can print all tf frames.
+                # frames = self.tf_buffer.all_frames_as_string()
+                # self.get_logger().info("Available TF frames:\n" + frames)
+
+                # Query the /tf transformation from /base_link to /map
+                trans = self.tf_buffer.lookup_transform(
+                    'map', 'base_link', rclpy.time.Time())
+                self.get_logger().info(
+                    f"Robot's position: {trans.transform.translation}")
+                self.get_logger().info(
+                    f"Robot's orientation: {trans.transform.rotation}")
+
+            elif self.iter == 4:
+                print("Cleaning up...")
+                rclpy.shutdown()
+            else:
+                print("Run", self.iter)
+
+        except Exception as e:
+            self.get_logger().error(
+                f"Error: {str(e)}")
+
+    def scan_callback(self, msg):
+        # Print the middle scan range
+        ranges = msg.ranges
+        if len(ranges) > 0:
+            middle_index = len(ranges) // 2
+            middle_range = ranges[middle_index]
+            # self.get_logger().info(f"Middle scan range: {middle_range:.2f}")
+            self.middle_range = middle_range
+        else:
+            # self.get_logger().warn("No ranges in LaserScan message.")
+            self.middle_range = middle_range
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RobotCommander()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

run.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+docker run --rm -it ros2_humble_mvsim_demo:latest