diff --git a/Dockerfile b/Dockerfile old mode 100755 new mode 100644 index de2bf8c7..1d16f964 --- a/Dockerfile +++ b/Dockerfile @@ -1,275 +1,38 @@ -# multi-stage for caching -FROM ros:humble - -# This variable tells future scripts that user input isn't available during the Docker build. -ENV DEBIAN_FRONTEND noninteractive - -LABEL maintainer="Nova" - -# Get wget first so we can get the NVIDIA keys -#RUN apt-get install -y wget - -# To fix GPG key error when running apt-get update -# https://developer.nvidia.com/blog/updating-the-cuda-linux-gpg-repository-key/ -###RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/3bf863cc.pub -###RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/machine-learning/repos/ubuntu2204/x86_64/7fa2af80.pub -#RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.0-1_all.deb && dpkg -i cuda-keyring_1.0-1_all.deb - -# Install torch -#RUN apt install -y nvidia-cuda-toolkit -#RUN pip3 install torch torchvision torchaudio --extra-index-url https://download.pytorch.org/whl/cu101 - -### REMOVE?? ### -### RUN pip3 install -U openmim && mim install mmcv-full && git clone https://github.com/open-mmlab/mmsegmentation.git /mmsegmentation -### WORKDIR /mmsegmentation -### RUN pip install -v -e . && mim download mmsegmentation --config pspnet_r50-d8_512x1024_40k_cityscapes --dest . - -# Install MMCV https://github.com/open-mmlab/mmcv (requires PyTorch) -#RUN pip3 install -U openmim && mim install mmcv-full - -# apt-get & apt installs -# Also need to upgrade the apt packages, otherwise there are incompatibilities running ros2/rviz2 -RUN apt-get update && apt update && apt upgrade -y && \ - # ================================= - ## apt-get installs =============== - apt-get install -y \ - # important linux tool - wget \ - # install python libraries - python3-pip \ - # be able to use git inside container - git \ - # - fprintd \ - # - libfprint-2-2:amd64 \ - # - libgl1-mesa-glx \ - # - libglib2.0-0 \ - # - libpam-fprintd:amd64 \ - # - libsm6 \ - # - libxrender-dev \ - # - libxext6 \ - # - ninja-build \ - # - ros-humble-diagnostic-updater \ - # ================================= - # apt installs ==================== - && apt install -y \ - # - alsa-base \ - # - alsa-utils \ - # - byobu \ - # - espeak \ - # - ffmpeg \ - # - libespeak1 \ - # - libsndfile1-dev \ - # - minicom \ - # - python3-matplotlib \ - # - python3-nose \ - # - python3-scipy \ - # - ros-humble-ament-cmake* \ - # needed for ros2_numpy package (maybe others) - ros-humble-ament-cmake-nose \ - # - ros-humble-async-web-server-cpp \ - # - ros-humble-cv-bridge \ - # - ros-humble-image-transport \ - # - ros-humble-joy-linux \ - # - ros-humble-lgsvl-msgs \ - # Nav2 stack - used for path planning - ros-humble-navigation2 \ - # - ros-humble-pcl-conversions \ - # - ros-humble-pcl-ros \ - # - ros-humble-rmw-fastrtps-cpp \ - # - ros-humble-robot-localization \ - # - ros-humble-rosbridge-server \ - # - ros-humble-ros-testing \ - # rqt provides oscilliscope-style plotting of topics, etc - ros-humble-rqt* \ - # - ros-humble-rviz2 \ - # - ros-humble-tf-transformations \ - # ROS Support for Velodyne LiDAR - ros-humble-velodyne-driver \ - ros-humble-velodyne-laserscan \ - ros-humble-velodyne-msgs \ - ros-humble-velodyne-pointcloud \ - # - ros-humble-vision-opencv \ - # - software-properties-common \ - # cleanup to make image smaller - && apt clean && rm -rf /var/lib/apt/lists/* - -# install cmake 3.24+ -RUN wget https://github.com/Kitware/CMake/releases/download/v3.24.4/cmake-3.24.4-linux-x86_64.tar.gz -RUN tar -zxvf cmake-3.24.4-linux-x86_64.tar.gz -RUN mv cmake-3.24.4-linux-x86_64 /opt/cmake -RUN ln -s /opt/cmake/bin/cmake /usr/local/bin/cmake - -# pip3 installs -RUN pip3 install --ignore-installed \ - # - autopep8==2.3.1 \ - # - cmake_format==0.6.11 \ - # - dictor==0.1.12 \ - # - ephem==4.1.5 \ - # Used for the road signs classifier API call to the model. - inference-sdk==0.26.0 \ - # - matplotlib \ - # (possibly needs to be version 2.2) - networkx==3.3 \ - # Scientific Computing - used widely - numpy==1.26.4 \ - # - open3d==0.18.0 \ - # - opencv-python==4.10.0.84 \ - # - pathfinding==1.0.11 \ - # - pep8==1.7.1 \ - # - pexpect==4.9.0 \ - # - psutil==5.9.0 \ - # - pygame==2.6.0 \ - # - pylint==3.2.7 \ - # - pymap3d==2.9.1 \ - # - pynmea2==1.19.0 \ - # - pyproj==3.6.1 \ - # - pyserial==3.5 \ - # - python-can==4.4.2 \ - # - py-trees==0.8.3 \ - # - requests==2.28.2 \ - # - scikit-image==0.24.0 \ - # Scientific Computing - used widely - scipy==1.14.1 \ - # - shapely==2.0.2 \ - # - simple-pid==2.0.1 \ - # - simple-watchdog-timer==0.1.1 \ - # - six==1.16.0 \ - # - tabulate==0.9.0 \ - # - transforms3d==0.4.2 \ - # - xmlschema==1.0.18 \ - # distro \ - torch \ - torchvision \ - # openmim \ - # mmcv-full==1.6.0 \ - # packaging \ - # prettytable \ - # python-dateutil>=2.1 \ - # kiwisolver>=1.0.1 \ - # pyparsing>=2.0.1 \ - # cycler>=0.10 \ - # wcwidth \ - # six>=1.5 \ - # mmcls \ - easydict==1.13 \ - kiss-icp==1.0.0 \ - g2o-python==0.0.12 \ - rosbags==0.10.4 \ - ultralytics - -# Install SAM2 seperately to avoid storage space errors on GitHub Actions -RUN pip3 install sam2 - -# Install Black for Python code formatting. -RUN pip3 install black==24.10.0 - -# install mmdetection3d for 3d object detection -RUN pip3 install -U openmim -RUN mim install mmengine -RUN mim install 'mmcv>=2.0.0rc4' -RUN mim install 'mmdet>=3.0.0' -RUN mim install "mmdet3d>=1.1.0" -RUN mim install "mmpose>=1.1.0" - - -# install loop closure package "MapClosures" -# this issue was addressed here: https://github.com/abetlen/llama-cpp-python/issues/707 -# RUN python3 -m pip uninstall -y exceptiongroup -# RUN sudo apt remove python3-pathspec -y -# RUN pip3 install --no-cache-dir pathspec yamllint "exceptiongroup<=1.2.0" -# RUN git clone https://github.com/PRBonn/MapClosures.git -# WORKDIR MapClosures -# RUN cmake -B build -S cpp -# RUN cmake --build build -j8 -# RUN make - -WORKDIR / - -# https://stackoverflow.com/questions/66669735/ubuntu-20-04-cant-find-pcl-because-of-incorrect-include-directory-after-install -RUN mkdir /lib/x86_64-linux-gnu/cmake/pcl/include && ln -s /usr/include/pcl-1.10/pcl /lib/x86_64-linux-gnu/cmake/pcl/include/pcl - -# Lemon -# Library for Efficient Modeling and Optimization in Networks. -# It is a C++ template library providing efficient implementations of common data structures and algorithms -# with focus on combinatorial optimization tasks connected mainly with graphs and networks. -RUN git clone https://github.com/The-OpenROAD-Project/lemon-graph.git && cd lemon-graph \ - # This specific commit is for Lemon-1.3.1. See https://github.com/The-OpenROAD-Project/lemon-graph/commits/master. - && git checkout 62ac753 \ - && mkdir build && cd build && cmake .. && make && make install - -# RUN useradd -ms /bin/bash docker -RUN usermod -a -G dialout root -RUN usermod -a -G tty root -# USER docker - -ENV ROS_VERSION 2 - -WORKDIR /navigator -COPY ./docker/entrypoint.sh /opt/entrypoint.sh - -ENTRYPOINT [ "/opt/entrypoint.sh" ] +# Base ROS 2 Humble image with desktop + turtlesim +FROM osrf/ros:humble-desktop + +SHELL ["/bin/bash", "-lc"] + +# System deps (+ CycloneDDS RMW) +RUN apt-get update && apt-get install -y \ + python3-pip python3-tk python3-colcon-common-extensions \ + python3-ament-package \ + ros-humble-turtlesim x11-apps \ + ros-humble-rmw-cyclonedds-cpp \ + && rm -rf /var/lib/apt/lists/* + +# Pin setuptools compatible with Humble, plus wheel +RUN python3 -m pip install --no-cache-dir -U pip && \ + pip3 install --no-cache-dir 'setuptools==65.5.1' 'setuptools_scm<7' wheel + +# Overlay workspace (your packages) +WORKDIR /workspaces/overlay +COPY overlay_src/ ./src/ + +# Build overlay +RUN source /opt/ros/humble/setup.bash && \ + colcon build --merge-install + +# Entrypoint +COPY ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh + +# DDS + X11 defaults +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ENV ROS_DOMAIN_ID=23 +ENV ROS_LOCALHOST_ONLY=0 +ENV QT_X11_NO_MITSHM=1 +ENV LIBGL_ALWAYS_SOFTWARE=1 + +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/Dockerfile.bak b/Dockerfile.bak new file mode 100644 index 00000000..4a940176 --- /dev/null +++ b/Dockerfile.bak @@ -0,0 +1,56 @@ +FROM ubuntu:22.04 + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y locales && locale-gen en_US.UTF-8 +ENV LANG=en_US.UTF-8 +ENV LC_ALL=en_US.UTF-8 + +# Base deps; colcon/vcstool/rosdep via pip +RUN apt-get update && apt-get install -y \ + build-essential cmake git wget curl gnupg2 lsb-release \ + python3-pip python3-venv python3-empy python3-numpy python3-tk \ + libasio-dev libtinyxml2-dev libeigen3-dev libyaml-cpp-dev \ + qtbase5-dev libqt5widgets5 \ + libx11-6 x11-apps \ + && rm -rf /var/lib/apt/lists/* + +RUN python3 -m pip install --no-cache-dir -U pip \ + && pip3 install --no-cache-dir colcon-common-extensions vcstool rosdep + +# rosdep init/update +RUN rosdep init || true && rosdep update || true + +# === Build ROS 2 Humble from source === +WORKDIR /opt/ros_ws +RUN mkdir -p src +RUN wget https://raw.githubusercontent.com/ros2/ros2/humble/ros2.repos +RUN vcs import src < ros2.repos + +# IMPORTANT: skip the ROS binary key "fastcdr" so the vendor pulls source +RUN apt-get update && \ + rosdep install --from-paths src --ignore-src -y --rosdistro humble --skip-keys "fastcdr" && \ + rm -rf /var/lib/apt/lists/* + +RUN colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release + +# === Overlay workspace (your packages) === +WORKDIR /workspaces/overlay +RUN mkdir -p src +COPY overlay_src/ src/ +RUN . /opt/ros_ws/install/setup.sh && \ + rosdep install --from-paths src --ignore-src -y --rosdistro humble || true && \ + colcon build --merge-install --cmake-args -DCMAKE_BUILD_TYPE=Release || true + +# Entrypoint +COPY ros_entrypoint.sh /ros_entrypoint.sh +RUN chmod +x /ros_entrypoint.sh + +# Env +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ENV ROS_DOMAIN_ID=23 +ENV ROS_LOCALHOST_ONLY=0 +ENV QT_X11_NO_MITSHM=1 +ENV LIBGL_ALWAYS_SOFTWARE=1 + +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] diff --git a/docker-compose.yml b/docker-compose.yml old mode 100755 new mode 100644 index 9c8c8812..9e8635de --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,140 +1,28 @@ -version: "2.3" -name: navigator services: - navigator: - image: navigator - container_name: navigator_${USER} + sim: build: context: . dockerfile: Dockerfile - - navigator_carla: - privileged: true - stdin_open: true - tty: true - runtime: nvidia - - image: navigator - container_name: navigator_carla_${USER} - - volumes: - - type: bind - source: $PWD - target: /navigator - - type: bind - source: /home/share/carla - target: /carla - - type: bind - source: /storage/bags - target: /bags - - type: bind - source: /storage/navigator_binaries - target: /navigator_binaries - - type: bind - source: ${HOME}/.Xauthority - target: /root/.Xauthority - - type: bind - source: /tmp/.X11-unix - target: /tmp/.X11-unix - # need to enable shared memory for ros to communicate between docker containers - # see: https://answers.ros.org/question/370595/ros2-foxy-nodes-cant-communicate-through-docker-container-border/ - - type: bind - source: /dev - target: /dev - #- type: bind - # source: /dev/shm - # target: /dev/shm - + platforms: ["linux/arm64"] environment: - - DISPLAY=${DISPLAY} - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} - - NVIDIA_VISIBLE_DEVICES=all - # Specify the ROS Middleware to be consistent across containers: - # see: https://discourse.ros.org/t/ros-cross-distribution-communication/27335 - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - - # deploy: - # resources: - # reservations: - # devices: - # - driver: nvidia - # count: all - # capabilities: [gpu] - + - DISPLAY=host.docker.internal:0 + - QT_X11_NO_MITSHM=1 + - LIBGL_ALWAYS_SOFTWARE=1 + - ROS_DOMAIN_ID=23 + - ROS_LOCALHOST_ONLY=0 + command: > + bash -lc "ros2 launch turtlesim_tasks sim_bringup.launch.py" + + follower: build: context: . dockerfile: Dockerfile - - network_mode: "host" - - # On Board Computer (OBC) does not have Carla or Bags directory - # runtime nvidia has yet to be set up on the OBC - navigator_obc: - privileged: true - stdin_open: true - tty: true - - image: navigator - container_name: navigator_obc_${USER} - - volumes: - - type: bind - source: $PWD - target: /navigator - - type: bind - source: ${HOME}/.Xauthority - target: /root/.Xauthority - - type: bind - source: /tmp/.X11-unix - target: /tmp/.X11-unix - # need to enable shared memory for ros to communicate between docker containers - # see: https://answers.ros.org/question/370595/ros2-foxy-nodes-cant-communicate-through-docker-container-border/ - - type: bind - source: /dev - target: /dev - #- type: bind - # source: /dev/shm - # target: /dev/shm - + platforms: ["linux/arm64"] environment: - - DISPLAY=${DISPLAY} - - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} - - NVIDIA_VISIBLE_DEVICES=all - # Specify the ROS Middleware to be consistent across containers: - # see: https://discourse.ros.org/t/ros-cross-distribution-communication/27335 - - RMW_IMPLEMENTATION=rmw_fastrtps_cpp - - deploy: - resources: - reservations: - devices: - - driver: nvidia - count: all - capabilities: [gpu] - - build: - context: . - dockerfile: Dockerfile - - network_mode: "host" - - zenith: - extends: - file: zenith/docker-compose.yaml - service: zenith - - zenith_backend: - extends: - file: zenith/docker-compose.yaml - service: zenith_backend - - zenith_dev: - extends: - file: zenith/docker-compose.yaml - service: zenith_dev - - zenith_backend_dev: - extends: - file: zenith/docker-compose.yaml - service: zenith_backend_dev - + - DISPLAY=host.docker.internal:0 + - QT_X11_NO_MITSHM=1 + - LIBGL_ALWAYS_SOFTWARE=1 + - ROS_DOMAIN_ID=23 + - ROS_LOCALHOST_ONLY=0 + command: > + bash -lc "ros2 launch turtlesim_tasks follower_bringup.launch.py" diff --git a/overlay_src/my_interfaces/CMakeLists.txt b/overlay_src/my_interfaces/CMakeLists.txt new file mode 100644 index 00000000..18d54763 --- /dev/null +++ b/overlay_src/my_interfaces/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.5) +project(my_interfaces) +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ChaseMode.msg" +) +ament_package() diff --git a/overlay_src/my_interfaces/msg/ChaseMode.msg b/overlay_src/my_interfaces/msg/ChaseMode.msg new file mode 100644 index 00000000..cc1bb6e0 --- /dev/null +++ b/overlay_src/my_interfaces/msg/ChaseMode.msg @@ -0,0 +1 @@ +bool chase diff --git a/overlay_src/my_interfaces/package.xml b/overlay_src/my_interfaces/package.xml new file mode 100644 index 00000000..0dc5cfad --- /dev/null +++ b/overlay_src/my_interfaces/package.xml @@ -0,0 +1,12 @@ + + + my_interfaces + 0.0.1 + Custom interfaces for turtlesim task. + You + Apache-2.0 + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + diff --git a/overlay_src/turtlesim_tasks/launch/follower_bringup.launch.py b/overlay_src/turtlesim_tasks/launch/follower_bringup.launch.py new file mode 100644 index 00000000..16611b38 --- /dev/null +++ b/overlay_src/turtlesim_tasks/launch/follower_bringup.launch.py @@ -0,0 +1,8 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node(package='turtlesim_tasks', executable='mode_toggle', name='mode_toggle', output='screen'), + Node(package='turtlesim_tasks', executable='follower_node', name='follower', output='screen'), + ]) diff --git a/overlay_src/turtlesim_tasks/launch/sim_bringup.launch.py b/overlay_src/turtlesim_tasks/launch/sim_bringup.launch.py new file mode 100644 index 00000000..14a015e3 --- /dev/null +++ b/overlay_src/turtlesim_tasks/launch/sim_bringup.launch.py @@ -0,0 +1,10 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node(package='turtlesim', executable='turtlesim_node', name='turtlesim'), + Node(package='turtlesim_tasks', executable='spawn_second', name='spawn_second', output='screen'), + Node(package='turtlesim_tasks', executable='mouse_goal_publisher', name='mouse_goal', output='screen'), + Node(package='turtlesim_tasks', executable='turtle1_controller', name='turtle1_controller', output='screen'), + ]) diff --git a/overlay_src/turtlesim_tasks/package.xml b/overlay_src/turtlesim_tasks/package.xml new file mode 100644 index 00000000..ae9a2051 --- /dev/null +++ b/overlay_src/turtlesim_tasks/package.xml @@ -0,0 +1,23 @@ + + + turtlesim_tasks + 0.0.1 + Custom nodes and launches for the turtlesim task. + You + Apache-2.0 + + + ament_python + + + rclpy + turtlesim + geometry_msgs + std_msgs + my_interfaces + python3-tk + + + ament_python + + diff --git a/overlay_src/turtlesim_tasks/resource/turtlesim_tasks b/overlay_src/turtlesim_tasks/resource/turtlesim_tasks new file mode 100644 index 00000000..b0c7601c --- /dev/null +++ b/overlay_src/turtlesim_tasks/resource/turtlesim_tasks @@ -0,0 +1 @@ +turtlesim_tasks diff --git a/overlay_src/turtlesim_tasks/setup.cfg b/overlay_src/turtlesim_tasks/setup.cfg new file mode 100644 index 00000000..99324e81 --- /dev/null +++ b/overlay_src/turtlesim_tasks/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/turtlesim_tasks +[install] +install-scripts=$base/lib/turtlesim_tasks diff --git a/overlay_src/turtlesim_tasks/setup.py b/overlay_src/turtlesim_tasks/setup.py new file mode 100644 index 00000000..528a6017 --- /dev/null +++ b/overlay_src/turtlesim_tasks/setup.py @@ -0,0 +1,30 @@ +from setuptools import setup +package_name = 'turtlesim_tasks' +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', [ + 'launch/sim_bringup.launch.py', + 'launch/follower_bringup.launch.py' + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='You', + maintainer_email='you@example.com', + description='Turtlesim task nodes', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'mouse_goal_publisher = turtlesim_tasks.mouse_goal_publisher:main', + 'turtle1_controller = turtlesim_tasks.turtle1_controller:main', + 'spawn_second = turtlesim_tasks.spawn_second:main', + 'follower_node = turtlesim_tasks.follower_node:main', + 'mode_toggle = turtlesim_tasks.mode_toggle:main', + ], + }, +) diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/__init__.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/__init__.py new file mode 100644 index 00000000..1bb8bf6d --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/__init__.py @@ -0,0 +1 @@ +# empty diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/follower_node.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/follower_node.py new file mode 100644 index 00000000..f342e295 --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/follower_node.py @@ -0,0 +1,64 @@ +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from turtlesim.msg import Pose +from my_interfaces.msg import ChaseMode + +def norm_angle(a): + while a > math.pi: a -= 2*math.pi + while a < -math.pi: a += 2*math.pi + return a + +class Follower(Node): + def __init__(self): + super().__init__('follower_node') + self.pose1 = None + self.pose2 = None + self.chase = True + self.pub = self.create_publisher(Twist, '/turtle2/cmd_vel', 10) + self.sub1 = self.create_subscription(Pose, '/turtle1/pose', self.on_pose1, 10) + self.sub2 = self.create_subscription(Pose, '/turtle2/pose', self.on_pose2, 10) + self.subm = self.create_subscription(ChaseMode, '/chase_mode', self.on_mode, 10) + self.timer = self.create_timer(0.03, self.loop) + self.k_lin = 2.0 + self.k_ang = 6.0 + self.close_thresh = 0.5 + self.far_thresh = 6.0 + + def on_pose1(self, msg): self.pose1 = msg + def on_pose2(self, msg): self.pose2 = msg + def on_mode(self, msg): + self.chase = bool(msg.chase) + self.get_logger().info(f"Follower mode -> {'CHASE' if self.chase else 'RUN AWAY'}") + + def loop(self): + cmd = Twist() + if self.pose1 is None or self.pose2 is None: + self.pub.publish(cmd); return + dx = self.pose1.x - self.pose2.x + dy = self.pose1.y - self.pose2.y + dist = math.hypot(dx, dy) + target_theta = math.atan2(dy, dx) + if self.chase: + if dist > self.close_thresh: + ang_err = norm_angle(target_theta - self.pose2.theta) + cmd.linear.x = max(min(self.k_lin * dist, 2.0), -2.0) + cmd.angular.z = max(min(self.k_ang * ang_err, 4.0), -4.0) + else: + if dist < self.far_thresh: + away_theta = norm_angle(target_theta + math.pi) + ang_err = norm_angle(away_theta - self.pose2.theta) + cmd.linear.x = 2.0 + cmd.angular.z = max(min(self.k_ang * ang_err, 4.0), -4.0) + self.pub.publish(cmd) + +def main(): + rclpy.init() + node = Follower() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/mode_toggle.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/mode_toggle.py new file mode 100644 index 00000000..4bada5c0 --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/mode_toggle.py @@ -0,0 +1,30 @@ +import rclpy +from rclpy.node import Node +from my_interfaces.msg import ChaseMode + +class ModeToggle(Node): + def __init__(self): + super().__init__('mode_toggle') + self.pub = self.create_publisher(ChaseMode, '/chase_mode', 10) + self.mode = True + self.timer = self.create_timer(10.0, self.flip) + self.publish_now() + + def publish_now(self): + msg = ChaseMode(chase=self.mode) + self.pub.publish(msg) + self.get_logger().info(f"Mode: {'CHASE' if self.mode else 'RUN AWAY'}") + + def flip(self): + self.mode = not self.mode + self.publish_now() + +def main(): + rclpy.init() + node = ModeToggle() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/mouse_goal_publisher.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/mouse_goal_publisher.py new file mode 100644 index 00000000..83a15ba9 --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/mouse_goal_publisher.py @@ -0,0 +1,46 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Point +import tkinter as tk + +WORLD = 11.0 +PIXELS = 550 +SCALE = WORLD / PIXELS + +class MouseGoalPublisher(Node): + def __init__(self): + super().__init__('mouse_goal_publisher') + self.pub = self.create_publisher(Point, '/mouse_goal', 10) + self.get_logger().info('Mouse window: click to set a target for turtle1') + self.root = tk.Tk() + self.root.title("Click-to-move (maps to turtlesim 11x11)") + self.canvas = tk.Canvas(self.root, width=PIXELS, height=PIXELS, bg="white") + self.canvas.pack() + self.canvas.bind("", self.on_click) + self.timer = self.create_timer(0.02, self._tk_poll) + + def _tk_poll(self): + self.root.update_idletasks() + self.root.update() + + def on_click(self, event): + x = event.x * SCALE + y = (PIXELS - event.y) * SCALE + msg = Point(x=x, y=y, z=0.0) + self.pub.publish(msg) + self.get_logger().info(f"Clicked -> goal ({x:.2f}, {y:.2f})") + r = 4 + self.canvas.create_oval(event.x-r, event.y-r, event.x+r, event.y+r) + +def main(): + rclpy.init() + node = MouseGoalPublisher() + try: + rclpy.spin(node) + except tk.TclError: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/spawn_second.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/spawn_second.py new file mode 100644 index 00000000..566bce14 --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/spawn_second.py @@ -0,0 +1,36 @@ +import rclpy +from rclpy.node import Node +from turtlesim.srv import Spawn + +class Spawner(Node): + def __init__(self): + super().__init__('spawn_second') + self.cli = self.create_client(Spawn, '/spawn') + self.timer = self.create_timer(0.5, self.try_spawn) + + def try_spawn(self): + if not self.cli.wait_for_service(timeout_sec=0.1): + self.get_logger().info('Waiting for /spawn...') + return + req = Spawn.Request() + req.x = 8.0; req.y = 8.0; req.theta = 0.0; req.name = 'turtle2' + fut = self.cli.call_async(req) + fut.add_done_callback(self._done) + self.timer.cancel() + + def _done(self, fut): + try: + res = fut.result() + self.get_logger().info(f"Spawned: {res.name}") + except Exception as e: + self.get_logger().error(f"Spawn failed: {e}") + +def main(): + rclpy.init() + node = Spawner() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/overlay_src/turtlesim_tasks/turtlesim_tasks/turtle1_controller.py b/overlay_src/turtlesim_tasks/turtlesim_tasks/turtle1_controller.py new file mode 100644 index 00000000..2eca66e5 --- /dev/null +++ b/overlay_src/turtlesim_tasks/turtlesim_tasks/turtle1_controller.py @@ -0,0 +1,51 @@ +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, Point +from turtlesim.msg import Pose + +def norm_angle(a): + while a > math.pi: a -= 2*math.pi + while a < -math.pi: a += 2*math.pi + return a + +class Turtle1Controller(Node): + def __init__(self): + super().__init__('turtle1_controller') + self.goal = None + self.pose = None + self.pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10) + self.sub_pose = self.create_subscription(Pose, '/turtle1/pose', self.on_pose, 10) + self.sub_goal = self.create_subscription(Point, '/mouse_goal', self.on_goal, 10) + self.timer = self.create_timer(0.03, self.loop) + self.k_lin = 2.0 + self.k_ang = 6.0 + self.stop_dist = 0.15 + + def on_pose(self, msg): self.pose = msg + def on_goal(self, msg): self.goal = (msg.x, msg.y) + + def loop(self): + if self.pose is None or self.goal is None: + self.pub.publish(Twist()) + return + dx = self.goal[0] - self.pose.x + dy = self.goal[1] - self.pose.y + dist = math.hypot(dx, dy) + target_theta = math.atan2(dy, dx) + ang_err = norm_angle(target_theta - self.pose.theta) + cmd = Twist() + if dist > self.stop_dist: + cmd.linear.x = max(min(self.k_lin * dist, 2.0), -2.0) + cmd.angular.z = max(min(self.k_ang * ang_err, 4.0), -4.0) + self.pub.publish(cmd) + +def main(): + rclpy.init() + node = Turtle1Controller() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ros_entrypoint.sh b/ros_entrypoint.sh new file mode 100755 index 00000000..b46608a9 --- /dev/null +++ b/ros_entrypoint.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -e +source /opt/ros/humble/setup.bash +if [ -f /workspaces/overlay/install/setup.bash ]; then + source /workspaces/overlay/install/setup.bash +fi +export DISPLAY="${DISPLAY:-host.docker.internal:0}" +exec "$@"