From 4b7d516054284b8aa608e2ccf811aebd0c694890 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sat, 14 Sep 2024 16:12:15 +0200 Subject: [PATCH] Initial commit --- .github/workflows/docker-build-run.yml | 45 ++++++++++ Dockerfile | 30 +++++++ build.sh | 3 + launch_mvsim.launch.py | 55 ++++++++++++ robot_commander.py | 112 +++++++++++++++++++++++++ run.sh | 3 + 6 files changed, 248 insertions(+) create mode 100644 .github/workflows/docker-build-run.yml create mode 100644 Dockerfile create mode 100755 build.sh create mode 100644 launch_mvsim.launch.py create mode 100755 robot_commander.py create mode 100755 run.sh diff --git a/.github/workflows/docker-build-run.yml b/.github/workflows/docker-build-run.yml new file mode 100644 index 0000000..9ca90bc --- /dev/null +++ b/.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 + diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..bf9122b --- /dev/null +++ b/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"] diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..f4d56df --- /dev/null +++ b/build.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +docker build -t ros2_humble_mvsim_demo:latest . diff --git a/launch_mvsim.launch.py b/launch_mvsim.launch.py new file mode 100644 index 0000000..b662611 --- /dev/null +++ b/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' + ) + ]) diff --git a/robot_commander.py b/robot_commander.py new file mode 100755 index 0000000..273f127 --- /dev/null +++ b/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() diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..0a5e68f --- /dev/null +++ b/run.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +docker run --rm -it ros2_humble_mvsim_demo:latest