diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 9dff356..22678f3 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,4 +1,5 @@ -FROM ros:kilted +# FROM ros:kilted +FROM osrf/ros:kilted-desktop-full ARG USERNAME=USERNAME ARG USER_UID=1000 ARG USER_GID=$USER_UID @@ -8,7 +9,7 @@ RUN if id -u $USER_UID ; then userdel `id -un $USER_UID` ; fi # Create the user RUN groupadd --gid $USER_GID $USERNAME \ - && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME -s /bin/bash \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ # # [Optional] Add sudo support. Omit if you don't need to install software after connecting. && apt-get update \ @@ -18,25 +19,26 @@ RUN groupadd --gid $USER_GID $USERNAME \ RUN apt-get update && apt-get upgrade -y # C++ development tools -RUN apt-get update && apt-get install -y \ +RUN apt-get install -y \ build-essential \ cmake \ gdb \ clang \ clang-format \ clang-tidy \ - libboost-all-dev + libboost-all-dev \ + xterm # Python development tools -RUN apt-get update && apt-get install -y \ +RUN apt-get install -y \ python3-pip \ - python3-venv \ python3-dev \ python3-argcomplete \ python3-colcon-common-extensions \ python3-colcon-mixin \ python3-rosdep \ - python3-vcstool + python3-vcstool \ + python3-smbus # ROS 2 ament build tools # RUN apt-get install -y \ @@ -46,51 +48,6 @@ RUN apt-get update && apt-get install -y \ # ros-${ROS_DISTRO}-ament-lint-auto \ # ros-${ROS_DISTRO}-ament-lint-common -# URDF / xacro tooling — needed to generate flat URDF for IDE preview -RUN apt-get update && apt-get install -y \ - ros-${ROS_DISTRO}-xacro \ - ros-${ROS_DISTRO}-robot-state-publisher - -# Gazebo Harmonic + ROS 2 integration -RUN apt-get update && apt-get install -y \ - ros-${ROS_DISTRO}-ros-gz \ - ros-${ROS_DISTRO}-gz-ros2-control - -# Navigation and SLAM -RUN apt-get update && apt-get install -y \ - ros-${ROS_DISTRO}-nav2-bringup \ - ros-${ROS_DISTRO}-slam-toolbox \ - ros-${ROS_DISTRO}-joint-state-publisher \ - ros-${ROS_DISTRO}-joint-state-broadcaster \ - ros-${ROS_DISTRO}-joint-trajectory-controller \ - ros-${ROS_DISTRO}-controller-manager - -# Node.js for frontend development -RUN curl -fsSL https://deb.nodesource.com/setup_22.x | bash - \ - && apt-get install -y nodejs - -# Python venv for webui backend (isolated from system packages) -RUN python3 -m venv /opt/webui-venv \ - && /opt/webui-venv/bin/pip install --no-cache-dir \ - fastapi \ - "uvicorn[standard]" \ - numpy - -# Docker CLI for accessing the host daemon via the mounted socket -# docker-compose-plugin is only in Docker's official repo, not Ubuntu's. -# .docker_gid is written by initializeCommand on the host before the build, -# ensuring the in-container docker group GID matches the host socket GID. -COPY .docker_gid /tmp/.docker_gid -RUN apt-get update && apt-get install -y ca-certificates curl gnupg \ - && install -m 0755 -d /etc/apt/keyrings \ - && curl -fsSL https://download.docker.com/linux/ubuntu/gpg | gpg --dearmor -o /etc/apt/keyrings/docker.gpg \ - && chmod a+r /etc/apt/keyrings/docker.gpg \ - && echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(. /etc/os-release && echo "$VERSION_CODENAME") stable" \ - > /etc/apt/sources.list.d/docker.list \ - && apt-get update && apt-get install -y docker-ce-cli docker-compose-plugin \ - && groupadd -g "$(cat /tmp/.docker_gid)" docker \ - && usermod -aG docker $USERNAME - ENV SHELL /bin/bash # ******************************************************** diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 9da4688..196a7ab 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,6 @@ "name": "ROS 2 Development Container", "privileged": true, "remoteUser": "matt", - "initializeCommand": "stat -c %g /var/run/docker.sock > .devcontainer/.docker_gid", "build": { "dockerfile": "Dockerfile", "args": { @@ -13,34 +12,33 @@ "workspaceMount": "source=${localWorkspaceFolder},target=/home/ws,type=bind", "customizations": { "vscode": { - "extensions":[ - "ms-vscode.cpptools", - "ms-vscode.cpptools-themes", + "extensions": [ "twxs.cmake", "donjayamanne.python-extension-pack", "eamodio.gitlens", "ms-iot.vscode-ros", + "morningfrog.urdf-visualizer", "anthropic.claude-code", - "morningfrog.urdf-visualizer" + "redhat.vscode-xml" ] } }, "containerEnv": { "DISPLAY": "unix:0", - "ROS_AUTOMATIC_DISCOVERY_RANGE": "LOCALHOST", - "ROS_DOMAIN_ID": "42" + "ROS_DOMAIN_ID": "0", + // "FASTDDS_BUILTIN_TRANSPORTS": "LARGE_DATA?max_msg_size=1MB&sockets_size=1MB&non_blocking=true&tcp_negotiation_timeout=50" + "FASTDDS_BUILTIN_TRANSPORTS": "LARGE_DATA" }, "runArgs": [ "--net=host", "--pid=host", "--ipc=host", - "-e", "DISPLAY=${env:DISPLAY}" + "-e", + "DISPLAY=${env:DISPLAY}" ], "mounts": [ - "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", - "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached", - "source=${localEnv:HOME}/.gitconfig,target=/home/matt/.gitconfig,type=bind,consistency=cached,readonly", - "source=/var/run/docker.sock,target=/var/run/docker.sock,type=bind" + "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", + "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached" ], - "postCreateCommand": "sudo rosdep update && sudo rosdep install --from-paths lidar/src robot/src --ignore-src -y && sudo chown -R $(whoami) /home/ws/ && npm --prefix /home/ws/webui/frontend install" + "postCreateCommand": "sudo apt update && sudo rosdep update && sudo rosdep install -y --from-paths src --ignore-src -y && sudo chown -R $(whoami) /home/ws/" } \ No newline at end of file diff --git a/ansible/setup_robot.yml b/ansible/setup_robot.yml index 5041546..0f7f0f8 100644 --- a/ansible/setup_robot.yml +++ b/ansible/setup_robot.yml @@ -12,5 +12,38 @@ command: raspi-config nonint do_spi 0 changed_when: false + # Real-time scheduling setup per + # https://control.ros.org/kilted/doc/ros2_control/controller_manager/doc/userdoc.html#determinism + - name: Create realtime group + group: + name: realtime + state: present + + - name: Add robot user to realtime group + user: + name: "{{ robot_user }}" + groups: realtime + append: true + + - name: Configure real-time scheduling limits + copy: + dest: /etc/security/limits.d/99-realtime.conf + content: | + @realtime soft rtprio 99 + @realtime soft priority 99 + @realtime soft memlock unlimited + @realtime hard rtprio 99 + @realtime hard priority 99 + @realtime hard memlock unlimited + owner: root + group: root + mode: '0644' + + # - name: Install low-latency kernel for improved RT performance + # apt: + # name: linux-lowlatency + # state: present + # update_cache: true + roles: - geerlingguy.docker diff --git a/docker-compose.yml b/docker-compose.yml index bbc90fb..2afea6f 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -6,8 +6,8 @@ services: robot: build: - context: . - dockerfile: robot/Dockerfile + context: ./raspbot_v2 + dockerfile: Dockerfile platforms: - linux/arm64 image: raspbot_v2:latest @@ -17,6 +17,13 @@ services: - /dev/i2c-1:/dev/i2c-1 environment: - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} + # Required for ros2_control to set SCHED_FIFO real-time thread priority. + # See https://control.ros.org/kilted/doc/ros2_control/controller_manager/doc/userdoc.html#determinism + cap_add: + - SYS_NICE + ulimits: + rtprio: 99 + memlock: -1 restart: unless-stopped lidar: diff --git a/raspbot_v2/Dockerfile b/raspbot_v2/Dockerfile new file mode 100644 index 0000000..52e8761 --- /dev/null +++ b/raspbot_v2/Dockerfile @@ -0,0 +1,83 @@ +# syntax=docker/dockerfile:1 +# ───────────────────────────────────────────────────────────────────────────── +# Raspbot V2 — robot deployment image +# +# Target: Raspberry Pi 5 (linux/arm64) +# Build: docker buildx build --platform linux/arm64 \ +# -t raspbot-v2:latest -f Dockerfile.robot . +# +# Run: docker run --rm -it \ +# --network host \ +# --device /dev/i2c-1 \ +# raspbot-v2:latest +# +# Override CMD to pass launch arguments, e.g.: +# docker run ... raspbot-v2:latest \ +# ros2 launch raspbot_v2_hardware hardware.launch.py enable_camera:=false +# ───────────────────────────────────────────────────────────────────────────── + +ARG ROS_DISTRO=kilted + +# ── Stage 1: build ─────────────────────────────────────────────────────────── +FROM ros:${ROS_DISTRO}-ros-base AS builder +ARG ROS_DISTRO + +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + cmake \ + python3-colcon-common-extensions \ + python3-rosdep \ + ros-${ROS_DISTRO}-hardware-interface \ + ros-${ROS_DISTRO}-pluginlib \ + ros-${ROS_DISTRO}-rclcpp \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /ros2_ws + +# raspbot_v2_sim is intentionally excluded — Gazebo is not needed on the robot +COPY src/raspbot_v2_bringup src/raspbot_v2_bringup +COPY src/raspbot_v2_control src/raspbot_v2_control +COPY src/raspbot_v2_description src/raspbot_v2_description +COPY src/raspbot_v2_hardware src/raspbot_v2_hardware +COPY src/raspbot_v2_hardware_interface src/raspbot_v2_hardware_interface + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && sudo apt-get update \ + && rosdep update --rosdistro ${ROS_DISTRO} \ + && rosdep install --from-paths src --ignore-src -r -y \ + && rm -rf /var/lib/apt/lists/* + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh \ + && colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release \ + --parallel-workers "$(nproc)" + +# ── Stage 2: runtime ───────────────────────────────────────────────────────── +FROM ros:${ROS_DISTRO}-ros-base AS runtime +ARG ROS_DISTRO + +RUN apt-get update && apt-get install -y --no-install-recommends \ + # I2C access for Yahboom MCU (motors, LEDs, servos, ultrasonic) + python3-smbus \ + # ros2_control: controller_manager, hardware_interface, spawner CLI + ros-${ROS_DISTRO}-ros2-control \ + # Standard controllers: MecanumDriveController, JointStateBroadcaster, + # ForwardCommandController (pan/tilt) + ros-${ROS_DISTRO}-ros2-controllers \ + # URDF/xacro processing and joint-state → TF broadcasting + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-xacro \ + && rm -rf /var/lib/apt/lists/* + +COPY --from=builder /ros2_ws/install /ros2_ws/install + +# Entrypoint sources both the ROS underlay and the workspace overlay +RUN echo '#!/bin/bash' > /entrypoint.sh \ + && echo 'set -e' >> /entrypoint.sh \ + && echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /entrypoint.sh \ + && echo 'source /ros2_ws/install/setup.bash' >> /entrypoint.sh \ + && echo 'exec "$@"' >> /entrypoint.sh \ + && chmod +x /entrypoint.sh + +ENTRYPOINT ["/entrypoint.sh"] +CMD ["ros2", "launch", "raspbot_v2_hardware", "hardware.launch.py"] diff --git a/raspbot_v2/README.md b/raspbot_v2/README.md new file mode 100644 index 0000000..219f76a --- /dev/null +++ b/raspbot_v2/README.md @@ -0,0 +1,46 @@ +# Raspbot-v2 modeling + +## Build + + ```bash +# Load the ros environment +. /opt/ros/kilted/setup.bash + +# Ensure that all the ros package dependencies are installed +rosdep install -i --from-path src --rosdistro kilted -y + +# Build the packages +colcon build --symlink-install + +# Load local ros environment +. install/local_setup.bash + +``` + +## Launch + +```bash +# Terminal 1 +ros2 launch raspbot_v2 start_world.launch.py + +# Terminal 2 +ros2 run teleop_twist_keyboard teleop_twist_keyboard +``` + + + +## TODO +- devcontainer + - Move rosdep install into base Dockerfle +- raspbot-v2 + - ... + +## Notes + +### Understand container to container networking with dds + +Untested, but this might be needed on host. + +```bash +sudo sysctl -w net.core.rmem_max=4194304 net.core.wmem_max=4194304 +``` \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_bringup/CMakeLists.txt b/raspbot_v2/src/raspbot_v2_bringup/CMakeLists.txt new file mode 100644 index 0000000..5689316 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_bringup/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(raspbot_v2_bringup) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(DIRECTORY + launch + DESTINATION share/${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() \ No newline at end of file diff --git a/robot/LICENSE b/raspbot_v2/src/raspbot_v2_bringup/LICENSE similarity index 60% rename from robot/LICENSE rename to raspbot_v2/src/raspbot_v2_bringup/LICENSE index 711a507..30e8e2e 100644 --- a/robot/LICENSE +++ b/raspbot_v2/src/raspbot_v2_bringup/LICENSE @@ -1,7 +1,3 @@ -MIT License - -Copyright (c) 2026 Matt Spencer - Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights @@ -9,24 +5,13 @@ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. - ---- - -Third-party code ----------------- - -raspbot_v2_interface/Raspbot_Lib/ - Source: Yahboom RASPBOT-V2 resources (https://www.yahboom.net/study/RASPBOT-V2) - Copyright: Yahboom Technology Co., Ltd. - License: No explicit open-source license provided by the upstream source. - Included here for hardware compatibility; all credit to Yahboom. +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/raspbot_v2/src/raspbot_v2_bringup/launch/raspbot_v2_bringup.launch.py b/raspbot_v2/src/raspbot_v2_bringup/launch/raspbot_v2_bringup.launch.py new file mode 100644 index 0000000..2f30b57 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_bringup/launch/raspbot_v2_bringup.launch.py @@ -0,0 +1,80 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node, PushRosNamespace +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + namespace = LaunchConfiguration('namespace') + enable_camera = LaunchConfiguration('enable_camera') + enable_lidar = LaunchConfiguration('enable_lidar') + enable_sonar = LaunchConfiguration('enable_sonar') + use_sim_time = LaunchConfiguration('use_sim_time') + use_hardware = LaunchConfiguration('use_hardware') + + declared_args = [ + DeclareLaunchArgument( + 'namespace', default_value='', + description='ROS namespace for all robot nodes (e.g. "robot1").' + ), + + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation time.' + ), + + DeclareLaunchArgument( + 'use_hardware', default_value='false', + description='Use hardware nodes.' + ), + + DeclareLaunchArgument( + 'enable_camera', default_value='true', + description='Enable the camera node.' + ), + + DeclareLaunchArgument( + 'enable_lidar', default_value='true', + description='Enable the LiDAR node.' + ), + + DeclareLaunchArgument( + 'enable_sonar', default_value='true', + description='Enable the sonar node.' + ) + ] + + controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('raspbot_v2_control'), 'launch', 'controller_manager.launch.py' + ]) + ]), + launch_arguments={ + 'namespace': namespace, + 'use_sim_time': LaunchConfiguration('use_sim_time') + }.items() + ) + + hardware_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('raspbot_v2_hardware'), 'launch', 'hardware.launch.py' + ]), + ]), + condition=IfCondition(use_hardware), + launch_arguments={ + 'namespace': namespace, + }.items(), + ) + + + return LaunchDescription([ + *declared_args, + PushRosNamespace(namespace), + controller_launch, + hardware_launch, + ]) \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_bringup/package.xml b/raspbot_v2/src/raspbot_v2_bringup/package.xml new file mode 100644 index 0000000..93087bf --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_bringup/package.xml @@ -0,0 +1,18 @@ + + + + raspbot_v2_bringup + 0.0.0 + TODO: Package description + Matt Spencer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_control/CMakeLists.txt b/raspbot_v2/src/raspbot_v2_control/CMakeLists.txt new file mode 100644 index 0000000..8e93852 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.20) +project(raspbot_v2_control) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install( + DIRECTORY config launch + DESTINATION share/${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() diff --git a/raspbot_v2/src/raspbot_v2_control/LICENSE b/raspbot_v2/src/raspbot_v2_control/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/raspbot_v2/src/raspbot_v2_control/config/controllers.yaml b/raspbot_v2/src/raspbot_v2_control/config/controllers.yaml new file mode 100644 index 0000000..ccc71e5 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/config/controllers.yaml @@ -0,0 +1,46 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz — hardware-limited by STM32 I2C clock-stretch (~22 ms/write × 4 motors) + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + mecanum_drive_controller: + type: mecanum_drive_controller/MecanumDriveController + + pan_tilt_controller: + type: forward_command_controller/ForwardCommandController + +# ── Mecanum drive controller ───────────────────────────────────────────────── +mecanum_drive_controller: + ros__parameters: + front_left_wheel_command_joint_name: joint_wheel_front_left + front_right_wheel_command_joint_name: joint_wheel_front_right + rear_left_wheel_command_joint_name: joint_wheel_rear_left + rear_right_wheel_command_joint_name: joint_wheel_rear_right + + # Geometry (metres) — derived from raspbot_v2_params.urdf.xacro: + # wx = base_length/2 - wheel_radius = 0.09 - 0.03 = 0.06 m (front-rear) + # wy = base_width/2 + wheel_width/2 = 0.045 + 0.015 = 0.06 m (left-right) + # sum_of_robot_center_projection = wx + wy = 0.12 + kinematics: + wheels_radius: 0.03 + sum_of_robot_center_projection_on_X_Y_axis: 0.12 + + # Odometry + enable_odom_tf: true + odom_frame_id: odom + base_frame_id: base_footprint + + pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] + +# ── Pan / tilt position controller ─────────────────────────────────────────── +# Accepts std_msgs/Float64MultiArray — index 0 = pan (rad), index 1 = tilt (rad) +# Topic: /pan_tilt_controller/commands +pan_tilt_controller: + ros__parameters: + joints: + - pan + - tilt + interface_name: position diff --git a/raspbot_v2/src/raspbot_v2_control/config/sim_overrides.yaml b/raspbot_v2/src/raspbot_v2_control/config/sim_overrides.yaml new file mode 100644 index 0000000..775d5c6 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/config/sim_overrides.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + use_sim_time: true diff --git a/raspbot_v2/src/raspbot_v2_control/launch/controller_manager.launch.py b/raspbot_v2/src/raspbot_v2_control/launch/controller_manager.launch.py new file mode 100644 index 0000000..ca12179 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/launch/controller_manager.launch.py @@ -0,0 +1,119 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription +from launch.conditions import IfCondition,UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration + +from launch_ros.actions import Node, PushRosNamespace +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time') + use_hardware = LaunchConfiguration('use_hardware') + namespace = LaunchConfiguration('namespace') + enable_camera = LaunchConfiguration('enable_camera') + + declared_args = [ + DeclareLaunchArgument( + 'namespace', default_value='', + description='ROS namespace for all robot nodes (e.g. "robot1").' + ), + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use simulation time.' + ), + DeclareLaunchArgument( + 'use_hardware', default_value='false', + description='Load the real I2C hardware interface plugin (set true on physical robot).' + ), + DeclareLaunchArgument( + 'enable_camera', default_value='false', + description='Enable the camera node.' + ), + ] + + + robot_controllers = PathJoinSubstitution([ + FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml' + ]) + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro' + ]), + ' use_sim_time:=', use_sim_time, + ' use_hardware:=', use_hardware, + ' controller_config:=', robot_controllers, + ]) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, robot_controllers], + output='both', + remappings=[ + # ("~/robot_description", "robot_description"), + ("mecanum_drive_controller/reference", "/cmd_vel"), + ("mecanum_drive_controller/odometry", "/odom"), + ("mecanum_drive_controller/tf_odometry", "/tf"), + ], + condition=UnlessCondition(use_sim_time), + ) + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster', '--controller-manager', + PathJoinSubstitution(["/", namespace, "controller_manager"]), + '--controller-manager-timeout', '30', + ], + ) + + robot_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['mecanum_drive_controller', '--controller-manager', + PathJoinSubstitution(["/", namespace, "controller_manager"]), + '--controller-manager-timeout', '30', + ] + ) + + # Delay starting the robot controller until the joint state broadcaster has finished starting up. + delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ), + ) + + pan_tilt_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['pan_tilt_controller', '--controller-manager', + PathJoinSubstitution(["/", namespace, "controller_manager"]), + '--controller-manager-timeout', '30', + ], + condition=IfCondition(enable_camera), + ) + + return LaunchDescription([ + *declared_args, + PushRosNamespace(namespace), + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + delay_robot_controller_after_joint_state_broadcaster, + pan_tilt_controller_spawner, + ]) diff --git a/raspbot_v2/src/raspbot_v2_control/package.xml b/raspbot_v2/src/raspbot_v2_control/package.xml new file mode 100644 index 0000000..be91779 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_control/package.xml @@ -0,0 +1,18 @@ + + + + raspbot_v2_control + 0.0.0 + TODO: Package description + Matt Spencer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/raspbot_v2/src/raspbot_v2_description/CMakeLists.txt b/raspbot_v2/src/raspbot_v2_description/CMakeLists.txt new file mode 100644 index 0000000..2ef147d --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(raspbot_v2_description) + +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(urdf REQUIRED) + +install( + DIRECTORY meshes urdf launch + DESTINATION share/${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() \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/LICENSE b/raspbot_v2/src/raspbot_v2_description/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/raspbot_v2/src/raspbot_v2_description/launch/raspbot_v2_display.launch.py b/raspbot_v2/src/raspbot_v2_description/launch/raspbot_v2_display.launch.py new file mode 100644 index 0000000..aee148e --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/launch/raspbot_v2_display.launch.py @@ -0,0 +1,36 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node,PushRosNamespace +from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command,LaunchConfiguration,PythonExpression +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + namespace = LaunchConfiguration('namespace') + use_rviz = LaunchConfiguration('use_rviz', default='true') + + + urdf_file = os.path.join( + get_package_share_directory('raspbot_v2_description'), + 'urdf', 'raspbot_v2.urdf.xacro') + + robot_description_content = Command([ + 'xacro ', urdf_file, + ]) + + return LaunchDescription([ + PushRosNamespace(namespace), + Node( + package="joint_state_publisher", + executable="joint_state_publisher_gui", + name="joint_state_publisher", + ), + + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": robot_description_content}], + ), + ]) \ No newline at end of file diff --git a/robot/src/raspbot_v2/raspbot_v2/__init__.py b/raspbot_v2/src/raspbot_v2_description/meshes/empty similarity index 100% rename from robot/src/raspbot_v2/raspbot_v2/__init__.py rename to raspbot_v2/src/raspbot_v2_description/meshes/empty diff --git a/raspbot_v2/src/raspbot_v2_description/package.xml b/raspbot_v2/src/raspbot_v2_description/package.xml new file mode 100644 index 0000000..2da6389 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/package.xml @@ -0,0 +1,18 @@ + + + + raspbot_v2_description + 0.0.0 + TODO: Package description + Matt Spencer + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/base.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/base.xacro new file mode 100644 index 0000000..4d94663 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/base.xacro @@ -0,0 +1,176 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.0 + ${wheel_roller_friction} + ${cos(roller_angle)} ${-x*y*sin(roller_angle)} 0 + + + + + 100000.0 + 1.0 + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + raspbot_v2_hardware_interface/RaspbotHardwareInterface + + + mock_components/GenericSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find raspbot_v2_control)/config/controllers.yaml + $(find raspbot_v2_control)/config/sim_overrides.yaml + + mecanum_drive_controller/reference:=/cmd_vel + mecanum_drive_controller/odometry:=/odom + mecanum_drive_controller/tf_odometry:=/tf + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/camera.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/camera.xacro new file mode 100644 index 0000000..ec5f38a --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/camera.xacro @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 30 + true + + ${camera_fov} + + ${camera_width} + ${camera_height} + R8G8B8 + + + 0.05 + 50.0 + + + gaussian + 0.0 + 0.007 + + + + /camera/image + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/cpu_housing.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/cpu_housing.xacro new file mode 100644 index 0000000..a8b259f --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/cpu_housing.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/lidar.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/lidar.xacro new file mode 100644 index 0000000..9ee90e4 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/lidar.xacro @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 10 + true + /scan + laser + + + + 360 + 1 + -3.14159265 + 3.14159265 + + + + ${lidar_min_range} + ${lidar_max_range} + 0.01 + + + gaussian + 0.0 + 0.01 + + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/materials.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/materials.xacro new file mode 100644 index 0000000..2832175 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/materials.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/pan_tilt.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/pan_tilt.xacro new file mode 100644 index 0000000..9c18651 --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/pan_tilt.xacro @@ -0,0 +1,141 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + gz_ros2_control/GazeboSimSystem + + + mock_components/GenericSystem + + + + + ${pan_min_rad} + ${pan_max_rad} + + + + + + + ${tilt_min_rad} + ${tilt_max_rad} + + + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2.urdf.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2.urdf.xacro new file mode 100644 index 0000000..f6a5c7a --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2.urdf.xacro @@ -0,0 +1,15 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2_params.urdf.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2_params.urdf.xacro new file mode 100644 index 0000000..879281a --- /dev/null +++ b/raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2_params.urdf.xacro @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/src/raspbot_v2/urdf/sonar.xacro b/raspbot_v2/src/raspbot_v2_description/urdf/sonar.xacro similarity index 66% rename from robot/src/raspbot_v2/urdf/sonar.xacro rename to raspbot_v2/src/raspbot_v2_description/urdf/sonar.xacro index 092c56f..9c33df1 100644 --- a/robot/src/raspbot_v2/urdf/sonar.xacro +++ b/raspbot_v2/src/raspbot_v2_description/urdf/sonar.xacro @@ -8,27 +8,58 @@ /ultrasonic/range. Mounted on the front face of the chassis, pointing forward. ───────────────────────────────────────────────────────────────────── --> + + + + + + + - - + + + + + + + + + + + + + + + + + + + + - - + + - + + + 0.001 + 1.0 + + + + + + + + + + ogre2 + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + 1000 + 0.9 + 0.01 + 0.001 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Warehouse + warehouse + + 0 0 -0.0986 0 0 0 + + + + diff --git a/robot/Dockerfile b/robot/Dockerfile deleted file mode 100644 index da98bfc..0000000 --- a/robot/Dockerfile +++ /dev/null @@ -1,50 +0,0 @@ -# syntax=docker/dockerfile:1 - -# ── Stage 1: build ──────────────────────────────────────────────────────────── -FROM ros:kilted AS builder - -SHELL ["/bin/bash", "-c"] - -RUN apt-get update && apt-get install -y --no-install-recommends \ - python3-colcon-common-extensions \ - ros-${ROS_DISTRO}-xacro \ - && rm -rf /var/lib/apt/lists/* - -WORKDIR /ws - -# Copy the ROS package into the standard colcon src/ layout and build it -COPY robot/src/raspbot_v2/ src/raspbot_v2/ - -RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \ - colcon build --packages-select raspbot_v2 - -# ── Stage 2: runtime ────────────────────────────────────────────────────────── -FROM ros:kilted-ros-core - -RUN apt-get update && apt-get install -y --no-install-recommends \ - ros-${ROS_DISTRO}-rclpy \ - ros-${ROS_DISTRO}-geometry-msgs \ - ros-${ROS_DISTRO}-std-msgs \ - ros-${ROS_DISTRO}-sensor-msgs \ - ros-${ROS_DISTRO}-launch-ros \ - ros-${ROS_DISTRO}-robot-state-publisher \ - ros-${ROS_DISTRO}-xacro \ - ros-${ROS_DISTRO}-tf2-ros \ - python3-smbus \ - && rm -rf /var/lib/apt/lists/* - -# Install the Raspbot hardware library directly into site-packages -COPY robot/raspbot_v2_interface/ /usr/local/lib/python3.12/dist-packages/raspbot_v2_interface/ - -# Bring across the built ROS overlay -COPY --from=builder /ws/install /ws/install - -# Source both ROS base and the workspace overlay on every shell/exec -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc && \ - echo "source /ws/install/setup.bash" >> /etc/bash.bashrc - -COPY docker-entrypoint.sh /docker-entrypoint.sh -RUN chmod +x /docker-entrypoint.sh - -ENTRYPOINT ["/docker-entrypoint.sh"] -CMD ["ros2", "launch", "raspbot_v2", "robot.launch.py"] diff --git a/robot/README.md b/robot/README.md deleted file mode 100644 index f27d313..0000000 --- a/robot/README.md +++ /dev/null @@ -1,302 +0,0 @@ -# Robot Controller - -ROS 2 nodes for the Yahboom Raspbot V2 — differential-drive motor control, pan/tilt camera orientation, and ultrasonic range sensing. - -All three nodes share the same I²C bus. The Linux kernel serialises individual transactions, so they run as separate processes without additional locking. - ---- - -## Motor controller - -``` - ┌───────────────────────────────────┐ - │ MotorControllerNode │ - │ │ - /cmd_vel ──────────>│ Twist → differential kinematics │ - (geometry_msgs/Twist)│ left = linear − (angular × wb/2)│ - │ right = linear + (angular × wb/2)│ - │ │ - /wheel_speeds ──────>│ Direct per-wheel override │ - (Float32MultiArray │ [FL, FR, RL, RR] │ - 4 × float32) │ │ - │ ▼ │ - │ raspbot_v2_interface │ - │ I²C bus 1, addr 0x2B │ - │ ▼ │ - │ /dev/i2c-1 ─────────> Motors │ - │ │ - /current_wheel_speeds│<─ telemetry @ 10 Hz │ - (Float32MultiArray) │ [FL, FR, RL, RR] │ - └───────────────────────────────────┘ -``` - -### Topics - -| Topic | Direction | Type | Description | -|---|---|---|---| -| `/cmd_vel` | Subscribed | `geometry_msgs/Twist` | Velocity command — `linear.x` (m/s) and `angular.z` (rad/s) | -| `/wheel_speeds` | Subscribed | `std_msgs/Float32MultiArray` | Direct per-wheel speed override `[FL, FR, RL, RR]` in library units (0–255) | -| `/current_wheel_speeds` | Published | `std_msgs/Float32MultiArray` | Current wheel speeds read from hardware, published at 10 Hz | - -### Parameters - -| Parameter | Default | Description | -|---|---|---| -| `wheel_base` | `0.3` | Distance between left and right wheels in metres | -| `max_speed` | `1.0` | Maximum motor speed in library units | - -### Sending velocity commands - -```bash -# Drive forward at 0.2 m/s -ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \ - "{linear: {x: 0.02}, angular: {z: 0.0}}" - -# Turn on the spot -ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \ - "{linear: {x: 0.0}, angular: {z: 0.5}}" - -# Stop -ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \ - "{linear: {x: 0.0}, angular: {z: 0.0}}" -``` - ---- - -## Camera orientation controller - -``` - ┌──────────────────────────────────────┐ - │ CameraOrientationNode │ - │ │ - /joint_command ────────>│ JointState (names: pan, tilt) │ - (sensor_msgs/ │ position in radians │ - JointState) │ │ - │ pan → servo 1 (0°–180°) │ - │ tilt → servo 2 (0°–110°) │ - │ │ - │ ▼ │ - │ raspbot_v2_interface │ - │ I²C bus 1, addr 0x2B │ - │ ▼ │ - │ /dev/i2c-1 ──────> Pan/tilt servos │ - │ │ - /joint_states <────────│ current angles @ 10 Hz │ - (sensor_msgs/ │ position in radians │ - JointState) │ │ - └──────────────────────────────────────┘ -``` - -### Topics - -| Topic | Direction | Type | Description | -|---|---|---|---| -| `/joint_command` | Subscribed | `sensor_msgs/JointState` | Commanded pan/tilt angles. Joint names `"pan"` and `"tilt"`, positions in **radians**. Unknown joint names are ignored. | -| `/joint_states` | Published | `sensor_msgs/JointState` | Current angles reflected from the last command, published at 10 Hz | - -### Parameters - -| Parameter | Default | Description | -|---|---|---| -| `pan_servo_id` | `1` | Raspbot servo channel for pan | -| `tilt_servo_id` | `2` | Raspbot servo channel for tilt | -| `pan_min_deg` | `0.0` | Pan lower limit (degrees) | -| `pan_max_deg` | `180.0` | Pan upper limit (degrees) | -| `tilt_min_deg` | `0.0` | Tilt lower limit (degrees) | -| `tilt_max_deg` | `110.0` | Tilt upper limit (degrees) — hardware cap | -| `pan_center_deg` | `90.0` | Startup and shutdown park position for pan | -| `tilt_center_deg` | `60.0` | Startup and shutdown park position for tilt | -| `state_rate_hz` | `10.0` | `~/joint_states` publish rate | - -### Hardware interface - -The node drives the pan and tilt servos over **I²C bus 1** (device address `0x2B`). The same `/dev/i2c-1` device used by the motor controller is sufficient — no additional device node is required. - -### Commanding the camera - -Pan to centre (90°) and tilt to 30°: - -```bash -ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \ - "{name: ['pan', 'tilt'], position: [1.5708, 0.5236]}" -``` - -A single axis can be commanded by omitting the other joint name: - -```bash -# Pan only -ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \ - "{name: ['pan'], position: [0.0]}" -``` - ---- - -## Ultrasonic range sensor - -``` - ┌──────────────────────────────────────┐ - │ UltrasonicNode │ - │ │ - │ Sensor off when no subscribers │ - │ Sensor on when subscribers > 0 │ - │ 1 s warm-up after power-on │ - │ │ - │ ▼ │ - │ raspbot_v2_interface │ - │ I²C bus 1, addr 0x2B │ - │ ▼ │ - │ /dev/i2c-1 ──────> HC-SR04 sensor │ - │ │ - /ultrasonic/range <────│ Range @ configurable rate │ - (sensor_msgs/Range) │ radiation_type = ULTRASOUND │ - │ range in metres (REP-117) │ - └──────────────────────────────────────┘ -``` - -### Topics - -| Topic | Direction | Type | Description | -|---|---|---|---| -| `/ultrasonic/range` | Published | `sensor_msgs/Range` | Distance in metres. `+inf` when beyond max range, `-inf` when closer than min range (REP-117). Only published while subscribers are connected. | - -### Parameters - -| Parameter | Default | Description | -|---|---|---| -| `publish_rate_hz` | `10.0` | Sensor poll and publish rate | -| `frame_id` | `'ultrasonic'` | `header.frame_id` on published messages | -| `min_range_m` | `0.02` | Minimum valid range in metres | -| `max_range_m` | `4.0` | Maximum valid range in metres | -| `field_of_view` | `0.2618` | Sensor cone width in radians (~15°) | -| `warmup_s` | `1.0` | Seconds to wait after powering the sensor on before publishing | - -### Verifying range readings - -```bash -ros2 topic echo /ultrasonic/range -``` - -The sensor activates automatically when a subscriber connects and deactivates when it disconnects. - ---- - -## Simulation (Gazebo) - -The simulation runs inside the **devcontainer** — not as a Docker Compose service. The devcontainer already has the display socket, GPU access, and host networking that Gazebo requires. Docker Compose is used only for deployment to the physical robot. - -Build the ROS workspace inside the devcontainer (once, or after source changes): - -```bash -cd /home/ws -colcon build --packages-select raspbot_v2 --symlink-install -source install/setup.bash -``` - -### Launching - -```bash -# Gazebo only — robot spawned, no SLAM or navigation -ros2 launch raspbot_v2 sim_launch.py - -# Gazebo + SLAM (building a map) + RViz -ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_rviz:=true - -# Full stack — SLAM + Nav2 autonomous navigation + RViz -ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true - -# Use a custom world file -ros2 launch raspbot_v2 sim_launch.py world:=/path/to/my_world.sdf -``` - -### Launch arguments - -| Argument | Default | Description | -|---|---|---| -| `world` | `worlds/empty_world.sdf` | Path to the Gazebo world SDF | -| `use_slam` | `false` | Run slam_toolbox in async mapping mode | -| `use_nav2` | `false` | Run the Nav2 navigation stack | -| `use_rviz` | `true` | Open RViz2 with the pre-configured layout | -| `wheel_base` | `0.14` | Wheel separation (m) — match the motor controller value | - -### Configuration files - -| File | Purpose | -|---|---| -| `config/slam_toolbox.yaml` | SLAM mode, map resolution, scan topic, frame IDs | -| `config/nav2_params.yaml` | BT navigator, DWB local planner, costmaps, AMCL, recovery behaviours | -| `config/controllers.yaml` | ros2_control — joint_state_broadcaster and pan_tilt_controller with PID gains | -| `config/rviz2_config.rviz` | Pre-configured displays: Map, RobotModel, LaserScan, Camera, TF, path plans | -| `worlds/empty_world.sdf` | Flat ground plane with lighting — add walls and furniture as needed | - -### Relationship to the hardware launch - -`sim_launch.py` and `robot.launch.py` use the same URDF and the same `raspbot_v2` package. The difference is what drives the joints: - -| | Simulation | Hardware | -|---|---|---| -| Drive | `gz-sim-diff-drive-system` plugin | `motor_controller_node` via I²C | -| Pan/tilt | `gz_ros2_control` + `pan_tilt_controller` | `camera_orientation_node` via I²C | -| Clock | Gazebo simulated time (`use_sim_time: true`) | System clock | - ---- - -## Hardware launch arguments - -Launch arguments can be appended when running the container manually: - -```bash -docker run --rm \ - --network=host \ - --device /dev/i2c-1 \ - --env ROS_DOMAIN_ID=0 \ - raspbot_v2:latest \ - ros2 launch raspbot_v2 robot.launch.py \ - wheel_base:=0.25 max_speed:=0.8 tilt_center_deg:=45.0 -``` - -| Argument | Default | Description | -|---|---|---| -| `wheel_base` | `0.3` | Distance between left and right wheels (m) | -| `max_speed` | `1.0` | Maximum motor speed in library units | -| `pan_center_deg` | `90.0` | Pan angle at startup and shutdown (degrees) | -| `tilt_center_deg` | `60.0` | Tilt angle at startup and shutdown (degrees) | -| `ultrasonic_rate_hz` | `10.0` | Ultrasonic sensor publish rate (Hz) | - ---- - -## Project layout - -``` -robot/ -├── Dockerfile # Two-stage build: colcon compile → clean runtime image -├── src/ -│ └── raspbot_v2/ -│ ├── package.xml -│ ├── setup.py -│ ├── launch/ -│ │ ├── robot.launch.py # Hardware — starts all nodes on the physical robot -│ │ └── sim_launch.py # Simulation — Gazebo + optional SLAM/Nav2/RViz -│ ├── urdf/ -│ │ ├── raspbot_v2.urdf.xacro # Top-level — includes all sub-files -│ │ ├── robot_base.xacro # Chassis, wheels, diff-drive plugin -│ │ ├── pan_tilt.xacro # Pan/tilt mount, joints, ros2_control block -│ │ ├── camera.xacro # Camera link, optical frame, Gazebo sensor -│ │ ├── lidar.xacro # Lidar link, Gazebo gpu_lidar sensor -│ │ ├── sonar.xacro # Ultrasonic link, Gazebo narrow-lidar approximation -│ │ └── raspbot_v2.ros2_control.xacro # gz_ros2_control plugin -│ ├── config/ -│ │ ├── controllers.yaml # ros2_control — joint_state_broadcaster + pan_tilt_controller -│ │ ├── slam_toolbox.yaml # SLAM mode, map resolution, scan topic, frame IDs -│ │ ├── nav2_params.yaml # BT navigator, DWB planner, costmaps, AMCL, recovery -│ │ └── rviz2_config.rviz # Pre-configured displays: Map, RobotModel, LaserScan, Camera -│ ├── worlds/ -│ │ └── empty_world.sdf # Flat ground plane with lighting -│ └── raspbot_v2/ -│ ├── motor_controller_node.py -│ ├── camera_orientation_node.py -│ ├── ultrasonic_node.py -│ └── led_node.py -└── raspbot_v2_interface/ # Vendored Yahboom hardware library - └── Raspbot_Lib/ - └── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B) -``` diff --git a/robot/raspbot_v2_interface/README.md b/robot/raspbot_v2_interface/README.md deleted file mode 100644 index 52a7df0..0000000 --- a/robot/raspbot_v2_interface/README.md +++ /dev/null @@ -1,5 +0,0 @@ -## Installation Steps (安装步骤) - -cd py_install - -sudo python3 setup.py install \ No newline at end of file diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py b/robot/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py deleted file mode 100644 index d568cab..0000000 --- a/robot/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py +++ /dev/null @@ -1,480 +0,0 @@ -#!/usr/bin/env python3 -# coding: utf-8 -import smbus -import time,random -import math - -PI5Car_I2CADDR = 0x2B -class Raspbot(): - - def get_i2c_device(self, address, i2c_bus): - self._addr = address - if i2c_bus is None: - return smbus.SMBus(1) - else: - return smbus.SMBus(i2c_bus) - - def __init__(self): - # Create I2C device. - self._device = self.get_i2c_device(PI5Car_I2CADDR, 0) - - #写数据 - def write_u8(self, reg, data): - try: - self._device.write_byte_data(self._addr, reg, data) - except: - print ('write_u8 I2C error') - - def write_reg(self, reg): - try: - self._device.write_byte(self._addr, reg) - except: - print ('write_u8 I2C error') - - def write_array(self, reg, data): - try: - # self._device.write_block_data(self._addr, reg, data) - self._device.write_i2c_block_data(self._addr, reg, data) - except: - print ('write_array I2C error') - - #读数据 - def read_data_byte(self): - try: - buf = self._device.write_byte(self._addr) - return buf - except: - print ('read_u8 I2C error') - - def read_data_array(self,reg,len): - try: - buf = self._device.read_i2c_block_data(self._addr,reg,len) - return buf - except: - print ('read_u8 I2C error') - - -#控制电机 - def Ctrl_Car(self, motor_id, motor_dir,motor_speed): - try: - if(motor_dir !=1)and(motor_dir != 0): #参数非法,方向默认前进 - motor_dir = 0 - if(motor_speed>255): - motor_speed = 255 - elif(motor_speed<0): - motor_speed = 0 - - reg = 0x01 - data = [motor_id, motor_dir, motor_speed] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制电机正反(-255~255) - def Ctrl_Muto(self, motor_id, motor_speed): - try: - - if(motor_speed>255): - motor_speed = 255 - if(motor_speed<-255): - motor_speed = -255 - if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 - motor_dir = 1 - else:motor_dir = 0 - reg = 0x01 - data = [motor_id, motor_dir, abs(motor_speed)] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制舵机 - def Ctrl_Servo(self, id, angle): - try: - reg = 0x02 - data = [id, angle] - if angle < 0: - angle = 0 - elif angle > 180: - angle = 180 - if(id==2 and angle > 100):angle = 100 - self.write_array(reg, data) - except: - print ('Ctrl_Servo I2C error') - -#控制灯珠(全部) - def Ctrl_WQ2812_ALL(self, state, color): - try: - reg = 0x03 - data = [state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独控制灯珠 - def Ctrl_WQ2812_Alone(self, number,state, color): - try: - reg = 0x04 - data = [number,state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制亮度(全部) - def Ctrl_WQ2812_brightness_ALL(self, R, G, B): - try: - reg = 0x08 - data = [R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独灯珠亮度 - def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): - try: - reg = 0x09 - data = [number,R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制红外遥控器开关 - def Ctrl_IR_Switch(self, state): - try: - reg = 0x05 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_IR_Switch I2C error') - -#控制蜂鸣器开关 - def Ctrl_BEEP_Switch(self, state): - try: - reg = 0x06 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_BEEP_Switch I2C error') - -#控制超声波测距开关 - def Ctrl_Ulatist_Switch(self, state): - try: - reg = 0x07 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_getDis_Switch I2C error') - - - - -#控制灯珠特效 -class LightShow: - - def __init__(self): - self.num_lights = 14 - self.last_val = 0 - self.MAX_TIME=999999 - self.bot=Raspbot() - self.running = True - - def execute_effect(self, effect_name,effect_duration,speed,current_color): - try: - if effect_name == 'river': - self.run_river_light(effect_duration,speed) - elif effect_name == 'breathing': - self.breathing_light(effect_duration,speed,current_color) - elif effect_name == 'gradient': - self.gradient_light(effect_duration,speed) - elif effect_name == 'random_running': - self.random_running_light(effect_duration,speed) - elif effect_name == 'starlight': - self.starlight_shimmer(effect_duration,speed) - else: - print("Unknown effect name.") - except KeyboardInterrupt: - self.turn_off_all_lights() - self.running = False - - def turn_off_all_lights(self): - self.bot.Ctrl_WQ2812_ALL(0, 0) - - def run_river_light(self,effect_duration,speed): - # speed = 0.01 - colors = [0, 1, 2, 3, 4, 5, 6] - color_index = 0 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights - 2): - self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) - time.sleep(speed) - self.bot.Ctrl_WQ2812_ALL(0, 0) - time.sleep(speed) - color_index = (color_index + 1) % len(colors) - self.turn_off_all_lights() - - def breathing_light(self, effect_duration,speed,current_color): - breath_direction = 0 - breath_count = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - - if current_color == 0: # Red - r, g, b = breath_count, 0, 0 - elif current_color == 1: # Green - r, g, b = 0, breath_count, 0 - elif current_color == 2: # Blue - r, g, b = 0, 0, breath_count - elif current_color == 3: # Yellow - r, g, b = breath_count, breath_count, 0 - elif current_color == 4: # Purple - r, g, b = breath_count, 0, breath_count - elif current_color == 5: # Cyan - r, g, b = 0, breath_count, breath_count - elif current_color == 6: # White - r, g, b = breath_count, breath_count, breath_count - else: - r, g, b = 0, 0, 0 # Default to black if invalid color code - - self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) - time.sleep(speed) - - if breath_direction == 0: - breath_count += 2 - if breath_count >= 255: - breath_count=255 - breath_direction = 1 - else: - breath_count -= 2 - if breath_count < 0: - breath_direction = 0 - breath_count = 0 - self.turn_off_all_lights() - - - def random_running_light(self,effect_duration,speed): - # on_time = 0.01 - # off_time = 0.01 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights): - color = random.randint(0, 6) - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - self.turn_off_all_lights() - - def starlight_shimmer(self,effect_duration,speed): - min_lights_on = 1 - max_lights_on = 7 - colors = [0, 1, 2, 3, 4, 5, 6] - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for color in colors: - start_time = time.time() - while time.time() - start_time < 1: - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) - for i in lights_on: - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - self.turn_off_all_lights() - - def gradient_light(self, effect_duration, speed): - grad_color = 0 - grad_index = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - if grad_color % 2 == 0: - gt_red = random.randint(0, 255) - gt_green = random.randint(0, 255) - gt_blue = random.randint(0, 255) - - gt_green = self.rgb_remix(gt_green) - gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) - grad_color += 1 - - if grad_color == 1: - if grad_index < 14: - number = (grad_index % 14) + 1 # Adjusting for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 2 - grad_index = 0 - - elif grad_color == 3: - if grad_index < 14: - number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 0 - grad_index = 0 - - time.sleep(speed) - - self.turn_off_all_lights() - - def rgb_remix(self, val): - if abs(val - self.last_val) < val % 30: - val = (val + self.last_val) % 255 - self.last_val = val % 255 - return self.last_val - - def rgb_remix_u8(self, r, g, b): - if r > 50 and g > 50 and b > 50: - index = random.randint(0, 2) - if index == 0: - r = 0 - elif index == 1: - g = 0 - elif index == 2: - b = 0 - return r, g, b - - def calculate_breath_color(self, color_code, breath_count): - max_brightness = 255 - if color_code == 0: # Red - return max_brightness, breath_count, 0 - elif color_code == 1: # Green - return max_brightness, 0, breath_count - elif color_code == 2: # Blue - return max_brightness, 0, 0, breath_count - elif color_code == 3: # Yellow - return max_brightness, breath_count, breath_count, 0 - elif color_code == 4: # Purple - return max_brightness, breath_count, 0, breath_count - elif color_code == 5: # Cyan - return max_brightness, 0, breath_count, breath_count - elif color_code == 6: # White - return max_brightness, breath_count, breath_count, breath_count - else: - return 0, 0, 0 # Default to black if invalid color code - - def stop(self): - self.running = False - -# test -#car = Raspbot() - -#读取巡线传感器地址 ,此值只有1位 -# track =car.read_data_array(0x0a,1) -# track = int(track[0]) -# x1 = (track>>3)&0x01 -# x2 = (track>>2)&0x01 -# x3 = (track>>1)&0x01 -# x4 = (track)&0x01 -# print(track,x1,x2,x3,x4) - - -# 读取超声传感器地址,此值只有2位 -# car.Ctrl_Ulatist_Switch(1)#open -# time.sleep(1) -# diss_H =car.read_data_array(0x1b,1)[0] -# diss_L =car.read_data_array(0x1a,1)[0] -# dis = diss_H<< 8 | diss_L -# print(dis+"mm") -# car.Ctrl_Ulatist_Switch(0)#close - -#读取红外遥控的值 -# car.Ctrl_IR_Switch(1) -# time.sleep(3) -# data =car.read_data_array(0x0c,1) -# print(data) -# car.Ctrl_IR_Switch(0) - - -#蜂鸣器测试 -# car.Ctrl_BEEP_Switch(1) -# time.sleep(1) -# car.Ctrl_BEEP_Switch(0) -# time.sleep(1) - - -#电机测试 -# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 -# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 -# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 -# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 -# time.sleep(1) -# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 -# time.sleep(1) -# car.Ctrl_Car(0,0,0) #L1电机 停止 -# car.Ctrl_Car(1,0,0) #L2电机 停止 -# car.Ctrl_Car(2,0,0) #R1电机 停止 -# car.Ctrl_Car(3,0,0) #R2电机 停止 - - -#舵机测试 -# car.Ctrl_Servo(1,0) #s1 0度 -# car.Ctrl_Servo(2,180) #s2 180度 -# time.sleep(1) -# car.Ctrl_Servo(1,180) #s1 180度 -# car.Ctrl_Servo(2,0) #s2 0度 -# time.sleep(1) - -#灯条测试 -# car.Ctrl_WQ2812_ALL(1,0)#红色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,1)#绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,2)#蓝色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,3)#黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#单个灯测试 -# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(14,1,2)#14号绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#控制亮度测试 全部 -# for i in range(255): -# car.Ctrl_WQ2812_brightness_ALL(i,0,0) -# time.sleep(0.01) - diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/NOTICE b/robot/raspbot_v2_interface/Raspbot_Lib/NOTICE deleted file mode 100644 index efbe591..0000000 --- a/robot/raspbot_v2_interface/Raspbot_Lib/NOTICE +++ /dev/null @@ -1,11 +0,0 @@ -This directory contains the Yahboom RASPBOT-V2 hardware interface library. - - Source: https://www.yahboom.net/study/RASPBOT-V2 - Copyright: Yahboom Technology Co., Ltd. - -The library was taken directly from the resources published by Yahboom for the -RASPBOT-V2 platform and is included here unmodified to provide I²C access to -the robot's motors and servos. - -No explicit open-source license has been provided by Yahboom for this material. -All intellectual property rights remain with Yahboom Technology Co., Ltd. diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py b/robot/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py deleted file mode 100644 index 3447937..0000000 --- a/robot/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py +++ /dev/null @@ -1,495 +0,0 @@ -#!/usr/bin/env python3 -# coding: utf-8 -# -# Source: Yahboom RASPBOT-V2 hardware library -# Origin: https://www.yahboom.net/study/RASPBOT-V2 -# Copyright: Yahboom Technology Co., Ltd. -# No explicit open-source license provided by the upstream source. -# Included here unmodified for hardware compatibility. -# -import smbus -import time,random -import math - -PI5Car_I2CADDR = 0x2B -class Raspbot(): - - def get_i2c_device(self, address, i2c_bus): - self._addr = address - if i2c_bus is None: - return smbus.SMBus(1) - else: - return smbus.SMBus(i2c_bus) - - def __init__(self): - # Create I2C device. - self._device = self.get_i2c_device(PI5Car_I2CADDR, 1) - - #写数据 - def write_u8(self, reg, data): - try: - self._device.write_byte_data(self._addr, reg, data) - except: - print ('write_u8 I2C error') - - def write_reg(self, reg): - try: - self._device.write_byte(self._addr, reg) - except: - print ('write_u8 I2C error') - - def write_array(self, reg, data): - try: - # self._device.write_block_data(self._addr, reg, data) - self._device.write_i2c_block_data(self._addr, reg, data) - except: - print ('write_array I2C error') - - #读数据 - def read_data_byte(self): - try: - buf = self._device.write_byte(self._addr) - return buf - except: - print ('read_u8 I2C error') - - def read_data_array(self,reg,len): - try: - buf = self._device.read_i2c_block_data(self._addr,reg,len) - return buf - except: - print ('read_u8 I2C error') - - -#控制电机 - def Ctrl_Car(self, motor_id, motor_dir,motor_speed): - try: - if(motor_dir !=1)and(motor_dir != 0): #参数非法,方向默认前进 - motor_dir = 0 - if(motor_speed>255): - motor_speed = 255 - elif(motor_speed<0): - motor_speed = 0 - - reg = 0x01 - data = [motor_id, motor_dir, motor_speed] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制电机正反(-255~255) - def Ctrl_Muto(self, motor_id, motor_speed): - try: - - if(motor_speed>255): - motor_speed = 255 - if(motor_speed<-255): - motor_speed = -255 - if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 - motor_dir = 1 - else:motor_dir = 0 - reg = 0x01 - data = [motor_id, motor_dir, abs(motor_speed)] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制舵机 - def Ctrl_Servo(self, id, angle): - try: - reg = 0x02 - data = [id, angle] - if angle < 0: - angle = 0 - elif angle > 180: - angle = 180 - if(id==2 and angle > 110):angle = 110 - self.write_array(reg, data) - except: - print ('Ctrl_Servo I2C error') - -#控制灯珠(全部) - def Ctrl_WQ2812_ALL(self, state, color): - try: - reg = 0x03 - data = [state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独控制灯珠 - def Ctrl_WQ2812_Alone(self, number,state, color): - try: - reg = 0x04 - data = [number,state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制亮度(全部) - def Ctrl_WQ2812_brightness_ALL(self, R, G, B): - try: - reg = 0x08 - data = [R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独灯珠亮度 - def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): - try: - reg = 0x09 - data = [number,R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制红外遥控器开关 - def Ctrl_IR_Switch(self, state): - try: - reg = 0x05 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_IR_Switch I2C error') - -#控制蜂鸣器开关 - def Ctrl_BEEP_Switch(self, state): - try: - reg = 0x06 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_BEEP_Switch I2C error') - -#控制超声波测距开关 - def Ctrl_Ulatist_Switch(self, state): - try: - reg = 0x07 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_getDis_Switch I2C error') - - def Read_Ultrasonic(self): - try: - diss_H = self.read_data_array(0x1b, 1)[0] - diss_L = self.read_data_array(0x1a, 1)[0] - dis = (diss_H << 8) | diss_L - return dis - except: - print('Read_Ultrasonic I2C error') - return None - - -#控制灯珠特效 -class LightShow: - - def __init__(self): - self.num_lights = 14 - self.last_val = 0 - self.MAX_TIME=999999 - self.bot=Raspbot() - self.running = True - - def execute_effect(self, effect_name,effect_duration,speed,current_color): - try: - if effect_name == 'river': - self.run_river_light(effect_duration,speed) - elif effect_name == 'breathing': - self.breathing_light(effect_duration,speed,current_color) - elif effect_name == 'gradient': - self.gradient_light(effect_duration,speed) - elif effect_name == 'random_running': - self.random_running_light(effect_duration,speed) - elif effect_name == 'starlight': - self.starlight_shimmer(effect_duration,speed) - else: - print("Unknown effect name.") - except KeyboardInterrupt: - self.turn_off_all_lights() - self.running = False - - def turn_off_all_lights(self): - self.bot.Ctrl_WQ2812_ALL(0, 0) - - def run_river_light(self,effect_duration,speed): - # speed = 0.01 - colors = [0, 1, 2, 3, 4, 5, 6] - color_index = 0 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights - 2): - self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) - time.sleep(speed) - self.bot.Ctrl_WQ2812_ALL(0, 0) - time.sleep(speed) - color_index = (color_index + 1) % len(colors) - self.turn_off_all_lights() - - def breathing_light(self, effect_duration,speed,current_color): - breath_direction = 0 - breath_count = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - - if current_color == 0: # Red - r, g, b = breath_count, 0, 0 - elif current_color == 1: # Green - r, g, b = 0, breath_count, 0 - elif current_color == 2: # Blue - r, g, b = 0, 0, breath_count - elif current_color == 3: # Yellow - r, g, b = breath_count, breath_count, 0 - elif current_color == 4: # Purple - r, g, b = breath_count, 0, breath_count - elif current_color == 5: # Cyan - r, g, b = 0, breath_count, breath_count - elif current_color == 6: # White - r, g, b = breath_count, breath_count, breath_count - else: - r, g, b = 0, 0, 0 # Default to black if invalid color code - - self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) - time.sleep(speed) - - if breath_direction == 0: - breath_count += 2 - if breath_count >= 255: - breath_count=255 - breath_direction = 1 - else: - breath_count -= 2 - if breath_count < 0: - breath_direction = 0 - breath_count = 0 - self.turn_off_all_lights() - - - def random_running_light(self,effect_duration,speed): - # on_time = 0.01 - # off_time = 0.01 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights): - color = random.randint(0, 6) - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - self.turn_off_all_lights() - - def starlight_shimmer(self,effect_duration,speed): - min_lights_on = 1 - max_lights_on = 7 - colors = [0, 1, 2, 3, 4, 5, 6] - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for color in colors: - start_time = time.time() - while time.time() - start_time < 1: - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) - for i in lights_on: - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - self.turn_off_all_lights() - - def gradient_light(self, effect_duration, speed): - grad_color = 0 - grad_index = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - if grad_color % 2 == 0: - gt_red = random.randint(0, 255) - gt_green = random.randint(0, 255) - gt_blue = random.randint(0, 255) - - gt_green = self.rgb_remix(gt_green) - gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) - grad_color += 1 - - if grad_color == 1: - if grad_index < 14: - number = (grad_index % 14) + 1 # Adjusting for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 2 - grad_index = 0 - - elif grad_color == 3: - if grad_index < 14: - number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 0 - grad_index = 0 - - time.sleep(speed) - - self.turn_off_all_lights() - - def rgb_remix(self, val): - if abs(val - self.last_val) < val % 30: - val = (val + self.last_val) % 255 - self.last_val = val % 255 - return self.last_val - - def rgb_remix_u8(self, r, g, b): - if r > 50 and g > 50 and b > 50: - index = random.randint(0, 2) - if index == 0: - r = 0 - elif index == 1: - g = 0 - elif index == 2: - b = 0 - return r, g, b - - def calculate_breath_color(self, color_code, breath_count): - max_brightness = 255 - if color_code == 0: # Red - return max_brightness, breath_count, 0 - elif color_code == 1: # Green - return max_brightness, 0, breath_count - elif color_code == 2: # Blue - return max_brightness, 0, 0, breath_count - elif color_code == 3: # Yellow - return max_brightness, breath_count, breath_count, 0 - elif color_code == 4: # Purple - return max_brightness, breath_count, 0, breath_count - elif color_code == 5: # Cyan - return max_brightness, 0, breath_count, breath_count - elif color_code == 6: # White - return max_brightness, breath_count, breath_count, breath_count - else: - return 0, 0, 0 # Default to black if invalid color code - - def stop(self): - self.running = False - -# test -#car = Raspbot() - -#读取巡线传感器地址 ,此值只有1位 -# track =car.read_data_array(0x0a,1) -# track = int(track[0]) -# x1 = (track>>3)&0x01 -# x2 = (track>>2)&0x01 -# x3 = (track>>1)&0x01 -# x4 = (track)&0x01 -# print(track,x1,x2,x3,x4) - - -# 读取超声传感器地址,此值只有2位 -# car.Ctrl_Ulatist_Switch(1)#open -# time.sleep(1) -# diss_H =car.read_data_array(0x1b,1)[0] -# diss_L =car.read_data_array(0x1a,1)[0] -# dis = diss_H<< 8 | diss_L -# print(dis+"mm") -# car.Ctrl_Ulatist_Switch(0)#close - -#读取红外遥控的值 -# car.Ctrl_IR_Switch(1) -# time.sleep(3) -# data =car.read_data_array(0x0c,1) -# print(data) -# car.Ctrl_IR_Switch(0) - - -#蜂鸣器测试 -# car.Ctrl_BEEP_Switch(1) -# time.sleep(1) -# car.Ctrl_BEEP_Switch(0) -# time.sleep(1) - - -#电机测试 -# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 -# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 -# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 -# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 -# time.sleep(1) -# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 -# time.sleep(1) -# car.Ctrl_Car(0,0,0) #L1电机 停止 -# car.Ctrl_Car(1,0,0) #L2电机 停止 -# car.Ctrl_Car(2,0,0) #R1电机 停止 -# car.Ctrl_Car(3,0,0) #R2电机 停止 - - -#舵机测试 -# car.Ctrl_Servo(1,0) #s1 0度 -# car.Ctrl_Servo(2,180) #s2 180度 -# time.sleep(1) -# car.Ctrl_Servo(1,180) #s1 180度 -# car.Ctrl_Servo(2,0) #s2 0度 -# time.sleep(1) - -#灯条测试 -# car.Ctrl_WQ2812_ALL(1,0)#红色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,1)#绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,2)#蓝色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,3)#黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#单个灯测试 -# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(10,1,2)#10号绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#控制亮度测试 全部 -# for i in range(255): -# car.Ctrl_WQ2812_brightness_ALL(i,0,0) -# time.sleep(0.01) - diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/__init__.py b/robot/raspbot_v2_interface/Raspbot_Lib/__init__.py deleted file mode 100644 index 3e40211..0000000 --- a/robot/raspbot_v2_interface/Raspbot_Lib/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .Raspbot_Lib import Raspbot,LightShow \ No newline at end of file diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc b/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc deleted file mode 100644 index 2b20dc4..0000000 Binary files a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc and /dev/null differ diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc b/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc deleted file mode 100644 index dc728c1..0000000 Binary files a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc and /dev/null differ diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc b/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc deleted file mode 100644 index 63bc85e..0000000 Binary files a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc and /dev/null differ diff --git a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc b/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc deleted file mode 100644 index c8e66c5..0000000 Binary files a/robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc and /dev/null differ diff --git a/robot/raspbot_v2_interface/__init__.py b/robot/raspbot_v2_interface/__init__.py deleted file mode 100644 index 8c72de4..0000000 --- a/robot/raspbot_v2_interface/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -from .motor_controller import MotorController - -__all__ = ["MotorController"] diff --git a/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py b/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py deleted file mode 100644 index 4b101ce..0000000 --- a/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py +++ /dev/null @@ -1,480 +0,0 @@ -#!/usr/bin/env python3 -# coding: utf-8 -import smbus -import time,random -import math - -PI5Car_I2CADDR = 0x2B -class Raspbot(): - - def get_i2c_device(self, address, i2c_bus): - self._addr = address - if i2c_bus is None: - return smbus.SMBus(1) - else: - return smbus.SMBus(i2c_bus) - - def __init__(self): - # Create I2C device. - self._device = self.get_i2c_device(PI5Car_I2CADDR, 1) - - #写数据 - def write_u8(self, reg, data): - try: - self._device.write_byte_data(self._addr, reg, data) - except: - print ('write_u8 I2C error') - - def write_reg(self, reg): - try: - self._device.write_byte(self._addr, reg) - except: - print ('write_u8 I2C error') - - def write_array(self, reg, data): - try: - # self._device.write_block_data(self._addr, reg, data) - self._device.write_i2c_block_data(self._addr, reg, data) - except: - print ('write_array I2C error') - - #读数据 - def read_data_byte(self): - try: - buf = self._device.write_byte(self._addr) - return buf - except: - print ('read_u8 I2C error') - - def read_data_array(self,reg,len): - try: - buf = self._device.read_i2c_block_data(self._addr,reg,len) - return buf - except: - print ('read_u8 I2C error') - - -#控制电机 - def Ctrl_Car(self, motor_id, motor_dir,motor_speed): - try: - if(motor_dir !=1)and(motor_dir != 0): #参数非法,方向默认前进 - motor_dir = 0 - if(motor_speed>255): - motor_speed = 255 - elif(motor_speed<0): - motor_speed = 0 - - reg = 0x01 - data = [motor_id, motor_dir, motor_speed] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制电机正反(-255~255) - def Ctrl_Muto(self, motor_id, motor_speed): - try: - - if(motor_speed>255): - motor_speed = 255 - if(motor_speed<-255): - motor_speed = -255 - if(motor_speed < 0 and motor_speed >= -255): #速度如果是负数则后退 - motor_dir = 1 - else:motor_dir = 0 - reg = 0x01 - data = [motor_id, motor_dir, abs(motor_speed)] - self.write_array(reg, data) - except: - print ('Ctrl_Car I2C error') - -#控制舵机 - def Ctrl_Servo(self, id, angle): - try: - reg = 0x02 - data = [id, angle] - if angle < 0: - angle = 0 - elif angle > 180: - angle = 180 - if(id==2 and angle > 110):angle = 110 - self.write_array(reg, data) - except: - print ('Ctrl_Servo I2C error') - -#控制灯珠(全部) - def Ctrl_WQ2812_ALL(self, state, color): - try: - reg = 0x03 - data = [state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独控制灯珠 - def Ctrl_WQ2812_Alone(self, number,state, color): - try: - reg = 0x04 - data = [number,state, color] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制亮度(全部) - def Ctrl_WQ2812_brightness_ALL(self, R, G, B): - try: - reg = 0x08 - data = [R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812 I2C error') - -#单独灯珠亮度 - def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B): - try: - reg = 0x09 - data = [number,R,G,B] - if R >255: - R =255 - if G > 255: - G = 255 - if B >255: - B=255 - self.write_array(reg, data) - except: - print ('Ctrl_WQ2812_Alone I2C error') - -#控制红外遥控器开关 - def Ctrl_IR_Switch(self, state): - try: - reg = 0x05 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_IR_Switch I2C error') - -#控制蜂鸣器开关 - def Ctrl_BEEP_Switch(self, state): - try: - reg = 0x06 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_BEEP_Switch I2C error') - -#控制超声波测距开关 - def Ctrl_Ulatist_Switch(self, state): - try: - reg = 0x07 - data = [state] - if state < 0: - state = 0 - elif state > 1: - state = 1 - self.write_array(reg, data) - except: - print ('Ctrl_getDis_Switch I2C error') - - - - -#控制灯珠特效 -class LightShow: - - def __init__(self): - self.num_lights = 14 - self.last_val = 0 - self.MAX_TIME=999999 - self.bot=Raspbot() - self.running = True - - def execute_effect(self, effect_name,effect_duration,speed,current_color): - try: - if effect_name == 'river': - self.run_river_light(effect_duration,speed) - elif effect_name == 'breathing': - self.breathing_light(effect_duration,speed,current_color) - elif effect_name == 'gradient': - self.gradient_light(effect_duration,speed) - elif effect_name == 'random_running': - self.random_running_light(effect_duration,speed) - elif effect_name == 'starlight': - self.starlight_shimmer(effect_duration,speed) - else: - print("Unknown effect name.") - except KeyboardInterrupt: - self.turn_off_all_lights() - self.running = False - - def turn_off_all_lights(self): - self.bot.Ctrl_WQ2812_ALL(0, 0) - - def run_river_light(self,effect_duration,speed): - # speed = 0.01 - colors = [0, 1, 2, 3, 4, 5, 6] - color_index = 0 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights - 2): - self.bot.Ctrl_WQ2812_Alone(i, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+1, 1, colors[color_index]) - self.bot.Ctrl_WQ2812_Alone(i+2, 1, colors[color_index]) - time.sleep(speed) - self.bot.Ctrl_WQ2812_ALL(0, 0) - time.sleep(speed) - color_index = (color_index + 1) % len(colors) - self.turn_off_all_lights() - - def breathing_light(self, effect_duration,speed,current_color): - breath_direction = 0 - breath_count = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - - if current_color == 0: # Red - r, g, b = breath_count, 0, 0 - elif current_color == 1: # Green - r, g, b = 0, breath_count, 0 - elif current_color == 2: # Blue - r, g, b = 0, 0, breath_count - elif current_color == 3: # Yellow - r, g, b = breath_count, breath_count, 0 - elif current_color == 4: # Purple - r, g, b = breath_count, 0, breath_count - elif current_color == 5: # Cyan - r, g, b = 0, breath_count, breath_count - elif current_color == 6: # White - r, g, b = breath_count, breath_count, breath_count - else: - r, g, b = 0, 0, 0 # Default to black if invalid color code - - self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b) - time.sleep(speed) - - if breath_direction == 0: - breath_count += 2 - if breath_count >= 255: - breath_count=255 - breath_direction = 1 - else: - breath_count -= 2 - if breath_count < 0: - breath_direction = 0 - breath_count = 0 - self.turn_off_all_lights() - - - def random_running_light(self,effect_duration,speed): - # on_time = 0.01 - # off_time = 0.01 - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for i in range(self.num_lights): - color = random.randint(0, 6) - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - self.turn_off_all_lights() - - def starlight_shimmer(self,effect_duration,speed): - min_lights_on = 1 - max_lights_on = 7 - colors = [0, 1, 2, 3, 4, 5, 6] - end_time = time.time() - while self.running and time.time() - end_time < effect_duration: - for color in colors: - start_time = time.time() - while time.time() - start_time < 1: - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - lights_on = random.sample(range(self.num_lights), k=random.randint(min_lights_on, max_lights_on)) - for i in lights_on: - self.bot.Ctrl_WQ2812_Alone(i, 1, color) - time.sleep(speed) - for i in range(self.num_lights): - self.bot.Ctrl_WQ2812_Alone(i, 0, 0) - self.turn_off_all_lights() - - def gradient_light(self, effect_duration, speed): - grad_color = 0 - grad_index = 0 - end_time = time.time() - - while self.running and time.time() - end_time < effect_duration: - if grad_color % 2 == 0: - gt_red = random.randint(0, 255) - gt_green = random.randint(0, 255) - gt_blue = random.randint(0, 255) - - gt_green = self.rgb_remix(gt_green) - gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, gt_blue) - grad_color += 1 - - if grad_color == 1: - if grad_index < 14: - number = (grad_index % 14) + 1 # Adjusting for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 2 - grad_index = 0 - - elif grad_color == 3: - if grad_index < 14: - number = ((14 - grad_index) % 14) # Reverse mapping, adjusted for 1-based indexing - self.bot.Ctrl_WQ2812_brightness_Alone(number, gt_red, gt_green, gt_blue) - grad_index += 1 - if grad_index >= 14: - grad_color = 0 - grad_index = 0 - - time.sleep(speed) - - self.turn_off_all_lights() - - def rgb_remix(self, val): - if abs(val - self.last_val) < val % 30: - val = (val + self.last_val) % 255 - self.last_val = val % 255 - return self.last_val - - def rgb_remix_u8(self, r, g, b): - if r > 50 and g > 50 and b > 50: - index = random.randint(0, 2) - if index == 0: - r = 0 - elif index == 1: - g = 0 - elif index == 2: - b = 0 - return r, g, b - - def calculate_breath_color(self, color_code, breath_count): - max_brightness = 255 - if color_code == 0: # Red - return max_brightness, breath_count, 0 - elif color_code == 1: # Green - return max_brightness, 0, breath_count - elif color_code == 2: # Blue - return max_brightness, 0, 0, breath_count - elif color_code == 3: # Yellow - return max_brightness, breath_count, breath_count, 0 - elif color_code == 4: # Purple - return max_brightness, breath_count, 0, breath_count - elif color_code == 5: # Cyan - return max_brightness, 0, breath_count, breath_count - elif color_code == 6: # White - return max_brightness, breath_count, breath_count, breath_count - else: - return 0, 0, 0 # Default to black if invalid color code - - def stop(self): - self.running = False - -# test -#car = Raspbot() - -#读取巡线传感器地址 ,此值只有1位 -# track =car.read_data_array(0x0a,1) -# track = int(track[0]) -# x1 = (track>>3)&0x01 -# x2 = (track>>2)&0x01 -# x3 = (track>>1)&0x01 -# x4 = (track)&0x01 -# print(track,x1,x2,x3,x4) - - -# 读取超声传感器地址,此值只有2位 -# car.Ctrl_Ulatist_Switch(1)#open -# time.sleep(1) -# diss_H =car.read_data_array(0x1b,1)[0] -# diss_L =car.read_data_array(0x1a,1)[0] -# dis = diss_H<< 8 | diss_L -# print(dis+"mm") -# car.Ctrl_Ulatist_Switch(0)#close - -#读取红外遥控的值 -# car.Ctrl_IR_Switch(1) -# time.sleep(3) -# data =car.read_data_array(0x0c,1) -# print(data) -# car.Ctrl_IR_Switch(0) - - -#蜂鸣器测试 -# car.Ctrl_BEEP_Switch(1) -# time.sleep(1) -# car.Ctrl_BEEP_Switch(0) -# time.sleep(1) - - -#电机测试 -# car.Ctrl_Car(0,0,150) #L1电机 前进 150速度 -# car.Ctrl_Car(1,0,150) #L2电机 前进 150速度 -# car.Ctrl_Car(2,0,150) #R1电机 前进 150速度 -# car.Ctrl_Car(3,0,150) #R2电机 前进 150速度 -# time.sleep(1) -# car.Ctrl_Car(0,1,50) #L1电机 后退 50速度 -# time.sleep(1) -# car.Ctrl_Car(0,0,0) #L1电机 停止 -# car.Ctrl_Car(1,0,0) #L2电机 停止 -# car.Ctrl_Car(2,0,0) #R1电机 停止 -# car.Ctrl_Car(3,0,0) #R2电机 停止 - - -#舵机测试 -# car.Ctrl_Servo(1,0) #s1 0度 -# car.Ctrl_Servo(2,180) #s2 180度 -# time.sleep(1) -# car.Ctrl_Servo(1,180) #s1 180度 -# car.Ctrl_Servo(2,0) #s2 0度 -# time.sleep(1) - -#灯条测试 -# car.Ctrl_WQ2812_ALL(1,0)#红色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,1)#绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,2)#蓝色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(1,3)#黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#单个灯测试 -# car.Ctrl_WQ2812_Alone(1,1,0)#1号红色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(2,1,3)#1号黄色 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(1,0,3)#1号关 -# time.sleep(1) -# car.Ctrl_WQ2812_Alone(10,1,2)#10号绿色 -# time.sleep(1) -# car.Ctrl_WQ2812_ALL(0,0) #关闭 - -#控制亮度测试 全部 -# for i in range(255): -# car.Ctrl_WQ2812_brightness_ALL(i,0,0) -# time.sleep(0.01) - diff --git a/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py b/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py deleted file mode 100644 index 3e40211..0000000 --- a/robot/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .Raspbot_Lib import Raspbot,LightShow \ No newline at end of file diff --git a/robot/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg b/robot/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg deleted file mode 100644 index f28ecfd..0000000 Binary files a/robot/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg and /dev/null differ diff --git a/robot/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg b/robot/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg deleted file mode 100644 index f51ef8b..0000000 Binary files a/robot/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg and /dev/null differ diff --git a/robot/raspbot_v2_interface/setup.py b/robot/raspbot_v2_interface/setup.py deleted file mode 100644 index d4275b1..0000000 --- a/robot/raspbot_v2_interface/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -from setuptools import find_packages, setup - -setup( - name='Raspbot_Lib', - version='0.0.2', - py_modules = ['Raspbot_Lib'], - author='Yahboom Team', - url='www.yahboom.com', - packages=find_packages(), - description='Raspbot drvier V0.0.2' -) \ No newline at end of file diff --git a/robot/src/raspbot_v2/config/controllers.yaml b/robot/src/raspbot_v2/config/controllers.yaml deleted file mode 100644 index f113830..0000000 --- a/robot/src/raspbot_v2/config/controllers.yaml +++ /dev/null @@ -1,55 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 50 # Hz — must be >= the fastest controller's publish rate - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - pan_tilt_controller: - type: joint_trajectory_controller/JointTrajectoryController - -# ─── pan_tilt_controller ───────────────────────────────────────────────────── -# Accepts goals on: -# /pan_tilt_controller/follow_joint_trajectory (action, JointTrajectory) -# -# allow_partial_joints_goal: true lets you command only pan or only tilt, -# matching the behaviour of the real camera_orientation_node which handles -# the joints independently. -pan_tilt_controller: - ros__parameters: - joints: - - pan - - tilt - command_interfaces: - - position - state_interfaces: - - position - - velocity - allow_partial_joints_goal: true - open_loop_control: false - goal_time: 5.0 # seconds — trajectory must complete within this window - stopped_velocity_tolerance: 0.01 # rad/s - constraints: - goal_time: 0.5 # seconds — tolerance after goal_time expires - stopped_velocity_tolerance: 0.01 - pan: - goal: 0.05 # rad — acceptable position error at goal - tilt: - goal: 0.05 # rad - # PID gains — active when command_interfaces includes velocity or effort, - # or when the hardware plugin uses them for closed-loop position control. - # Tune on the real robot: raise p until oscillation, then halve it; add d - # to damp, and a small i to remove steady-state error. - gains: - pan: - p: 10.0 - i: 0.01 - d: 1.0 - i_clamp: 1.0 - ff_velocity_scale: 1.0 - tilt: - p: 10.0 - i: 0.01 - d: 1.0 - i_clamp: 1.0 - ff_velocity_scale: 1.0 diff --git a/robot/src/raspbot_v2/config/nav2_params.yaml b/robot/src/raspbot_v2/config/nav2_params.yaml deleted file mode 100644 index 9300c56..0000000 --- a/robot/src/raspbot_v2/config/nav2_params.yaml +++ /dev/null @@ -1,314 +0,0 @@ -# Nav2 parameters for the Raspbot V2 -# Small differential-drive indoor robot: 20 × 15 cm chassis, max 0.2 m/s -# -# Tuning starting points — adjust after observing behaviour in simulation: -# robot_radius → measure the actual half-diagonal -# inflation_radius → must clear the robot + a comfortable margin -# max_vel_x → limited by motor speed and I²C latency -# DWB critic scales → path alignment vs obstacle clearance trade-off - -# ── AMCL (particle-filter localisation against a saved map) ───────────────── -# Not used when slam_toolbox runs in mapping mode — slam_toolbox publishes -# the map → odom transform directly. Switch to this when using a pre-built -# map with slam_toolbox in localization mode. -amcl: - ros__parameters: - use_sim_time: true - alpha1: 0.002 # rotation noise from rotation - alpha2: 0.002 # rotation noise from translation - alpha3: 0.002 # translation noise from translation - alpha4: 0.002 # translation noise from rotation - alpha5: 0.002 # (strafe — 0 for differential drive) - base_frame_id: base_footprint - global_frame_id: map - odom_frame_id: odom - robot_model_type: nav2_amcl::DifferentialMotionModel - scan_topic: /scan - laser_model_type: likelihood_field - laser_max_range: 12.0 - laser_min_range: 0.15 - max_beams: 60 - max_particles: 2000 - min_particles: 500 - pf_err: 0.05 - pf_z: 0.99 - resample_interval: 1 - sigma_hit: 0.2 - z_hit: 0.5 - z_rand: 0.5 - z_max: 0.05 - z_short: 0.05 - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - update_min_d: 0.25 # m — minimum travel before filter update - update_min_a: 0.2 # rad — minimum rotation before filter update - transform_tolerance: 1.0 - tf_broadcast: true - save_pose_rate: 0.5 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - do_beamskip: false - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - -# ── BT Navigator ───────────────────────────────────────────────────────────── -bt_navigator: - ros__parameters: - use_sim_time: true - global_frame: map - robot_base_frame: base_footprint - odom_topic: /odom - bt_loop_duration: 10 # ms - default_server_timeout: 20 # ms - navigators: - - navigate_to_pose - - navigate_through_poses - navigate_to_pose: - plugin: nav2_bt_navigator::NavigateToPoseNavigator - navigate_through_poses: - plugin: nav2_bt_navigator::NavigateThroughPosesNavigator - error_code_names: - - compute_path_error_code - - follow_path_error_code - -# ── Controller server (local planner — DWB) ────────────────────────────────── -controller_server: - ros__parameters: - use_sim_time: true - controller_frequency: 20.0 # Hz - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - speed_limit_topic: speed_limit - progress_checker_plugins: [progress_checker] - goal_checker_plugins: [general_goal_checker] - controller_plugins: [FollowPath] - - progress_checker: - plugin: nav2_controller::SimpleProgressChecker - required_movement_radius: 0.5 - movement_time_allowance: 10.0 - - general_goal_checker: - plugin: nav2_controller::SimpleGoalChecker - stateful: true - xy_goal_tolerance: 0.25 # m - yaw_goal_tolerance: 0.25 # rad - - FollowPath: - plugin: dwb_core::DWBLocalPlanner - debug_trajectory_details: true - - # Velocity limits — conservative for a small indoor robot - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.20 # m/s - max_vel_y: 0.0 # differential drive - max_vel_theta: 1.0 # rad/s - min_speed_xy: 0.0 - max_speed_xy: 0.20 - min_speed_theta: 0.0 - - # Acceleration limits - acc_lim_x: 0.5 # m/s² - acc_lim_y: 0.0 - acc_lim_theta: 3.2 # rad/s² - decel_lim_x: -1.0 - decel_lim_y: 0.0 - decel_lim_theta: -3.2 - - # Trajectory sampling - vx_samples: 20 - vy_samples: 1 # 1 = no lateral sampling for diff drive - vtheta_samples: 20 - sim_time: 1.7 # seconds to simulate each candidate trajectory - linear_granularity: 0.05 - angular_granularity: 0.025 - - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: true - stateful: true - - # Critics and their weights - critics: - - RotateToGoal - - Oscillation - - BaseObstacle - - GoalAlign - - PathAlign - - PathDist - - GoalDist - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - -# ── Planner server (global planner — NavFn/A*) ─────────────────────────────── -planner_server: - ros__parameters: - use_sim_time: true - planner_plugins: [GridBased] - GridBased: - plugin: nav2_navfn_planner::NavfnPlanner - tolerance: 0.5 - use_astar: false # false = Dijkstra; true = A* (faster but less optimal) - allow_unknown: true - -# ── Behaviour server (recovery actions) ────────────────────────────────────── -behavior_server: - ros__parameters: - use_sim_time: true - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: - - spin - - backup - - drive_on_heading - - wait - - assisted_teleop - spin: - plugin: nav2_behaviors::Spin - backup: - plugin: nav2_behaviors::BackUp - drive_on_heading: - plugin: nav2_behaviors::DriveOnHeading - wait: - plugin: nav2_behaviors::Wait - assisted_teleop: - plugin: nav2_behaviors::AssistedTeleop - # Conservative limits for recovery motions - max_rotational_vel: 1.0 # rad/s - min_rotational_vel: 0.4 # rad/s - rotational_acc_lim: 3.2 # rad/s² - simulate_ahead_time: 2.0 # seconds - -# ── Velocity smoother ──────────────────────────────────────────────────────── -velocity_smoother: - ros__parameters: - use_sim_time: true - smoothing_frequency: 20.0 - scale_velocities: false - feedback: OPEN_LOOP - max_velocity: [0.20, 0.0, 1.0] # [x m/s, y m/s, theta rad/s] - min_velocity: [-0.20, 0.0, -1.0] - max_accel: [0.5, 0.0, 3.2] - max_decel: [-1.0, 0.0, -3.2] - odom_topic: odom - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - -# ── Waypoint follower ───────────────────────────────────────────────────────── -waypoint_follower: - ros__parameters: - use_sim_time: true - loop_rate: 20 - stop_on_failure: false - action_server_result_timeout: 900.0 - waypoint_task_executor_plugin: wait_at_waypoint - wait_at_waypoint: - plugin: nav2_waypoint_follower::WaitAtWaypoint - enabled: true - waypoint_pause_duration: 200 # ms - -# ── Local costmap ───────────────────────────────────────────────────────────── -local_costmap: - local_costmap: - ros__parameters: - use_sim_time: true - update_frequency: 5.0 # Hz - publish_frequency: 2.0 # Hz - global_frame: odom - robot_base_frame: base_footprint - rolling_window: true - width: 3 # m — window size - height: 3 # m - resolution: 0.05 # m/cell - # Footprint of the actual chassis (in metres, relative to base_footprint) - # Replace robot_radius with footprint for a more accurate bounding shape. - robot_radius: 0.15 - plugins: - - obstacle_layer - - inflation_layer - obstacle_layer: - plugin: nav2_costmap_2d::ObstacleLayer - enabled: true - observation_sources: scan - scan: - topic: /scan - data_type: LaserScan - marking: true - clearing: true - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - max_obstacle_height: 2.0 - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - inflation_radius: 0.30 # m — must be > robot_radius - cost_scaling_factor: 3.0 - always_send_full_costmap: true - -# ── Global costmap ──────────────────────────────────────────────────────────── -global_costmap: - global_costmap: - ros__parameters: - use_sim_time: true - update_frequency: 1.0 - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_footprint - robot_radius: 0.15 - resolution: 0.05 - track_unknown_space: true - plugins: - - static_layer - - obstacle_layer - - inflation_layer - static_layer: - plugin: nav2_costmap_2d::StaticLayer - map_subscribe_transient_local: true - obstacle_layer: - plugin: nav2_costmap_2d::ObstacleLayer - enabled: true - observation_sources: scan - scan: - topic: /scan - data_type: LaserScan - marking: true - clearing: true - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 - raytrace_max_range: 3.0 - raytrace_min_range: 0.0 - max_obstacle_height: 2.0 - inflation_layer: - plugin: nav2_costmap_2d::InflationLayer - inflation_radius: 0.30 - cost_scaling_factor: 3.0 - -# ── Map server / saver ──────────────────────────────────────────────────────── -map_server: - ros__parameters: - use_sim_time: true - yaml_filename: "" # set at launch when loading a pre-built map - -map_saver: - ros__parameters: - use_sim_time: true - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: true diff --git a/robot/src/raspbot_v2/config/rviz2_config.rviz b/robot/src/raspbot_v2/config/rviz2_config.rviz deleted file mode 100644 index 5fb857e..0000000 --- a/robot/src/raspbot_v2/config/rviz2_config.rviz +++ /dev/null @@ -1,274 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /RobotModel1 - - /LaserScan1 - - /Map1 - Splitter Ratio: 0.5 - Tree Height: 600 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /2D Pose Estimate1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - -Visualization Manager: - Class: "" - Displays: - - - Class: rviz_default_plugins/Grid - Name: Grid - Value: true - Alpha: 0.5 - Cell Size: 1 - Color: 160; 160; 164 - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - - - Class: rviz_default_plugins/RobotModel - Name: RobotModel - Value: true - Alpha: 1 - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Links: - All Links Enabled: true - TF Prefix: "" - Update Interval: 0 - Visual Enabled: true - Collision Enabled: false - - - Class: rviz_default_plugins/TF - Name: TF - Value: true - Frame Timeout: 15 - Frames: - All Enabled: false - Marker Scale: 0.3 - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - - - Class: rviz_default_plugins/Map - Name: Map - Value: true - Alpha: 0.7 - Color Scheme: map - Draw Behind: false - Topic: - Depth: 1 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - - - Class: rviz_default_plugins/LaserScan - Name: LaserScan - Value: true - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Color: 255; 50; 0 - Color Transformer: FlatColor - Decay Time: 0 - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Position Transformer: XYZ - Size (m): 0.05 - Size (Pixels): 3 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /scan - Use Fixed Frame: true - Use rainbow: true - - - Class: rviz_default_plugins/Image - Name: Camera - Value: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /camera/image_raw - - - Class: rviz_default_plugins/Path - Name: Global Plan - Value: true - Alpha: 1 - Buffer Length: 1 - Color: 0; 128; 0 - Line Style: Lines - Line Width: 0.029999999329447746 - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /plan - - - Class: rviz_default_plugins/Path - Name: Local Plan - Value: true - Alpha: 0.5 - Buffer Length: 1 - Color: 0; 0; 255 - Line Style: Lines - Line Width: 0.029999999329447746 - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_plan - - - Class: nav2_rviz_plugins/ParticleCloud - Name: Particle Cloud - Value: true - Color: 50; 200; 50 - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /particle_cloud - - Tools: - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - - Views: - Current: - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 100 - Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 900 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000357fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f00700065007200740069006500730200000000ffffffff0000009100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500670065007400730100000000ffffffff0000000000000000fb0000000c00430061006d0065007261010000018d0000010a00000000000000000000000100000200000003a5fc0200000002fb0000000a0049006d006100670065010000014f0000016a0000000000000000fb00000010006400690073007000600061007900730100000000000003a50000005000fffffffb000000100044006900730070006c006100790073010000003b000002b40000002800ffffff000000010000010f000003a5fc0200000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000000003a5000000f500fffffffb000000120053006500610072006300680020003100000000ffffffff0000000000000000fb0000000c00430061006d006500720061000000000000000000000000000000000000010f0000035700000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1400 - X: 50 - Y: 50 diff --git a/robot/src/raspbot_v2/config/slam_toolbox.yaml b/robot/src/raspbot_v2/config/slam_toolbox.yaml deleted file mode 100644 index e0806fd..0000000 --- a/robot/src/raspbot_v2/config/slam_toolbox.yaml +++ /dev/null @@ -1,64 +0,0 @@ -slam_toolbox: - ros__parameters: - - use_sim_time: true - - # ── Solver ────────────────────────────────────────────────────────────── - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ── Frames ────────────────────────────────────────────────────────────── - odom_frame: odom - map_frame: map - base_frame: base_footprint - scan_topic: /scan - - # ── Mode ──────────────────────────────────────────────────────────────── - # mapping — build a new map; publishes /map and /map_metadata - # localization — localise in a previously-saved map (load with map_server) - # lifelong — continuous mapping with loop closure; memory-intensive - mode: mapping - - # ── Map parameters ────────────────────────────────────────────────────── - resolution: 0.05 # metres per pixel — 5 cm is good for indoor navigation - max_laser_range: 12.0 # m — matches RPLIDAR A1 spec; trim to actual room size - map_update_interval: 5.0 # seconds between map publishes - - # ── Timing / TF ───────────────────────────────────────────────────────── - transform_publish_period: 0.02 # seconds (50 Hz) — map → odom TF publish rate - transform_timeout: 0.2 # seconds — how long to wait for a TF lookup - tf_buffer_duration: 30.0 # seconds — TF history kept in memory - throttle_scans: 1 # process every Nth scan (1 = all) - minimum_time_interval: 0.5 # seconds — minimum gap between processed scans - - # ── Scan matching ──────────────────────────────────────────────────────── - use_scan_matching: true - use_scan_barycenter: true - - # How far the robot must move/turn before adding a new node to the graph - minimum_travel_distance: 0.5 # m - minimum_travel_heading: 0.5 # rad (~29°) - - # Scan buffer — larger = more context for loop closure but more memory - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 # m - - # Matching quality thresholds - link_match_minimum_response_fine: 0.1 - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - - # Angular search parameters - fine_search_angle_offset: 0.00349 # rad (~0.2°) - coarse_search_angle_offset: 0.349 # rad (~20°) - coarse_angle_resolution: 0.0349 # rad (~2°) - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true - - debug_logging: false - stack_size_to_use: 40000000 # bytes — increase if you see stack overflows diff --git a/robot/src/raspbot_v2/launch/house_sim_launch.py b/robot/src/raspbot_v2/launch/house_sim_launch.py deleted file mode 100644 index 41d68d8..0000000 --- a/robot/src/raspbot_v2/launch/house_sim_launch.py +++ /dev/null @@ -1,42 +0,0 @@ -""" -house_sim_launch.py — Gazebo simulation using the Nav2 warehouse world - -Thin wrapper around sim_launch.py that sets the world to the warehouse -environment from nav2_minimal_tb4_sim. All other arguments (use_slam, -use_nav2, use_rviz, wheel_base) are forwarded as-is. - -Note: the warehouse world pulls models from Gazebo Fuel on first launch. -They are cached in ~/.gz/fuel after that and work offline. - -Usage: - ros2 launch raspbot_v2 house_sim_launch.py - ros2 launch raspbot_v2 house_sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true -""" - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - - -def generate_launch_description(): - world = os.path.join( - get_package_share_directory('nav2_minimal_tb4_sim'), - 'worlds', - 'warehouse.sdf', - ) - - sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('raspbot_v2'), - 'launch', - 'sim_launch.py', - ) - ), - launch_arguments={'world': world}.items(), - ) - - return LaunchDescription([sim]) diff --git a/robot/src/raspbot_v2/launch/robot.launch.py b/robot/src/raspbot_v2/launch/robot.launch.py deleted file mode 100644 index cc27412..0000000 --- a/robot/src/raspbot_v2/launch/robot.launch.py +++ /dev/null @@ -1,116 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, Command -from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue - - -def generate_launch_description(): - urdf_file = os.path.join( - get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro' - ) - controllers_config = os.path.join( - get_package_share_directory('raspbot_v2'), 'config', 'controllers.yaml' - ) - - robot_description = ParameterValue( - Command([ - 'xacro ', urdf_file, - ' wheel_separation:=', LaunchConfiguration('wheel_base'), - ' controllers_config:=', controllers_config, - ]), - value_type=str, - ) - - return LaunchDescription([ - - # ── Motor controller arguments ──────────────────────────────────── - DeclareLaunchArgument('wheel_base', default_value='0.3', - description='Distance between left and right wheels (m)'), - DeclareLaunchArgument('max_speed', default_value='1.0', - description='Maximum motor speed in library units'), - - # ── Ultrasonic sensor arguments ─────────────────────────────────── - DeclareLaunchArgument('ultrasonic_rate_hz', default_value='10.0', - description='Ultrasonic sensor publish rate (Hz)'), - - # ── Camera orientation arguments ────────────────────────────────── - DeclareLaunchArgument('pan_center_deg', default_value='0.0', - description='Pan angle at startup and shutdown (degrees, ROS convention)'), - DeclareLaunchArgument('tilt_center_deg', default_value='-15.0', - description='Tilt angle at startup and shutdown (degrees, ROS convention)'), - - # ── TF / robot description ──────────────────────────────────────── - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - parameters=[{ - 'robot_description': robot_description, - 'use_sim_time': False, - }], - output='screen', - ), - - # Static: map → odom (identity — replace with SLAM/AMCL when ready) - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='map_to_odom', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'], - output='screen', - ), - - # Static: odom → base_footprint (placeholder until odometry is added) - Node( - package='tf2_ros', - executable='static_transform_publisher', - name='odom_to_base', - arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'], - output='screen', - ), - - # ── Nodes ───────────────────────────────────────────────────────── - Node( - package='raspbot_v2', - executable='motor_controller', - name='motor_controller', - parameters=[{ - 'wheel_base': LaunchConfiguration('wheel_base'), - 'max_speed': LaunchConfiguration('max_speed'), - }], - output='screen', - ), - - Node( - package='raspbot_v2', - executable='camera_orientation', - name='camera_orientation', - parameters=[{ - 'pan_center_deg': LaunchConfiguration('pan_center_deg'), - 'tilt_center_deg': LaunchConfiguration('tilt_center_deg'), - }], - output='screen', - ), - - Node( - package='raspbot_v2', - executable='ultrasonic', - name='ultrasonic', - parameters=[{ - 'publish_rate_hz': LaunchConfiguration('ultrasonic_rate_hz'), - }], - output='screen', - ), - - Node( - package='raspbot_v2', - executable='led_controller', - name='led_controller', - output='screen', - ), - - ]) diff --git a/robot/src/raspbot_v2/launch/sim_launch.py b/robot/src/raspbot_v2/launch/sim_launch.py deleted file mode 100644 index fc311f3..0000000 --- a/robot/src/raspbot_v2/launch/sim_launch.py +++ /dev/null @@ -1,259 +0,0 @@ -""" -sim_launch.py — Gazebo Harmonic simulation for the Raspbot V2 - -Brings up: - • gz_sim — Gazebo with the configured world - • robot_state_publisher — publishes robot_description and TF from URDF - • ros_gz_sim/create — spawns the robot into the Gazebo world - • ros_gz_bridge — bridges gz ↔ ROS 2 topics - • controller_manager — loaded by gz_ros2_control plugin in URDF - • joint_state_broadcaster + pan_tilt_controller (spawned after gz starts) - • slam_toolbox — online async SLAM (optional, --slam) - • nav2 — navigation stack (optional, --nav2, requires --slam or map) - • rviz2 — pre-configured visualiser (optional, --rviz) - -Usage: - ros2 launch raspbot_v2 sim_launch.py - ros2 launch raspbot_v2 sim_launch.py use_slam:=true use_nav2:=true use_rviz:=true - ros2 launch raspbot_v2 sim_launch.py world:=/path/to/my_world.sdf -""" - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - TimerAction, -) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - pkg = get_package_share_directory('raspbot_v2') - - # ── Paths ────────────────────────────────────────────────────────────── - urdf_file = os.path.join(pkg, 'urdf', 'raspbot_v2.urdf.xacro') - controllers_yaml = os.path.join(pkg, 'config', 'controllers.yaml') - slam_yaml = os.path.join(pkg, 'config', 'slam_toolbox.yaml') - nav2_yaml = os.path.join(pkg, 'config', 'nav2_params.yaml') - rviz_config = os.path.join(pkg, 'config', 'rviz2_config.rviz') - default_world = os.path.join(pkg, 'worlds', 'empty_world.sdf') - - # ── Launch arguments ─────────────────────────────────────────────────── - world_arg = DeclareLaunchArgument('world', default_value=default_world, - description='Path to the Gazebo world SDF file') - slam_arg = DeclareLaunchArgument('use_slam', default_value='false', - description='Launch slam_toolbox in async mapping mode') - nav2_arg = DeclareLaunchArgument('use_nav2', default_value='false', - description='Launch the Nav2 navigation stack') - rviz_arg = DeclareLaunchArgument('use_rviz', default_value='true', - description='Launch RViz2 with the pre-configured layout') - wheel_base_arg = DeclareLaunchArgument('wheel_base', default_value='0.14', - description='Wheel separation (m) — must match motor controller') - - # ── Robot description (xacro → URDF string) ──────────────────────────── - robot_description = ParameterValue( - Command([ - 'xacro ', urdf_file, - ' wheel_separation:=', LaunchConfiguration('wheel_base'), - ' controllers_config:=', controllers_yaml, - ]), - value_type=str, - ) - - # ── robot_state_publisher ────────────────────────────────────────────── - rsp = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - parameters=[{ - 'robot_description': robot_description, - 'use_sim_time': True, - }], - output='screen', - ) - - # ── Gazebo ──────────────────────────────────────────────────────────── - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('ros_gz_sim'), - 'launch', 'gz_sim.launch.py', - ) - ), - launch_arguments={ - 'gz_args': [LaunchConfiguration('world'), ' -r'], - }.items(), - ) - - # ── Spawn robot model into Gazebo ───────────────────────────────────── - # Reads robot_description from the RSP node's topic and spawns the entity. - spawn_robot = Node( - package='ros_gz_sim', - executable='create', - arguments=[ - '-name', 'raspbot_v2', - '-topic', 'robot_description', - '-x', '0.0', '-y', '0.0', '-z', '0.05', - '-R', '0.0', '-P', '0.0', '-Y', '0.0', - ], - output='screen', - ) - - # ── ros_gz_bridge — topic bridging between gz and ROS 2 ────────────── - # Format: /ros_topic@ros_msg_type[gz_msg_type (gz→ROS) - # /ros_topic@ros_msg_type]gz_msg_type (ROS→gz) - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - name='ros_gz_bridge', - arguments=[ - # Drive commands: ROS → gz - '/cmd_vel@geometry_msgs/msg/Twist]gz.msgs.Twist', - # Odometry: gz → ROS - '/odom@nav_msgs/msg/Odometry[gz.msgs.Odometry', - # TF from diff-drive plugin: gz → ROS - '/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V', - # Lidar scan: gz → ROS - '/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan', - # Sonar (simulated as narrow lidar): gz → ROS - '/ultrasonic/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan', - # Camera image: gz → ROS - '/camera/image@sensor_msgs/msg/Image[gz.msgs.Image', - # Camera info: gz → ROS - '/camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo', - # Wheel joint states from JointStatePublisher plugin: gz → ROS - '/gz/joint_states@sensor_msgs/msg/JointState[gz.msgs.Model', - # Clock: gz → ROS (required for use_sim_time) - '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', - ], - remappings=[ - # Remap camera topic to match what image_transport / webrtc_streamer expects - ('/camera/image', '/camera/image_raw'), - ], - parameters=[{'use_sim_time': True}], - output='screen', - ) - - # ── ros2_control — spawn controllers ─────────────────────────────────── - # Controllers are started by the gz_ros2_control plugin inside Gazebo. - # We use TimerAction to wait for the controller_manager to be ready - # before spawning controllers (gz takes a few seconds to initialise). - spawn_jsb = TimerAction( - period=5.0, - actions=[ - Node( - package='controller_manager', - executable='spawner', - arguments=[ - 'joint_state_broadcaster', - '--controller-manager', '/controller_manager', - ], - output='screen', - ), - ], - ) - - spawn_pan_tilt = TimerAction( - period=6.0, # after joint_state_broadcaster is active - actions=[ - Node( - package='controller_manager', - executable='spawner', - arguments=[ - 'pan_tilt_controller', - '--controller-manager', '/controller_manager', - ], - output='screen', - ), - ], - ) - - # ── Joint state relay — merge gz wheel states with ros2_control states ─ - # robot_state_publisher needs all joint states on one /joint_states topic. - # joint_state_broadcaster publishes pan/tilt; /gz/joint_states has wheels. - # joint_state_publisher merges both sources automatically if configured. - joint_state_relay = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_merger', - parameters=[{ - 'use_sim_time': True, - 'source_list': ['/joint_states', '/gz/joint_states'], - 'rate': 50, - }], - output='screen', - ) - - # ── slam_toolbox — online async SLAM ────────────────────────────────── - slam = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('slam_toolbox'), - 'launch', 'online_async_launch.py', - ]) - ), - launch_arguments={ - 'slam_params_file': slam_yaml, - 'use_sim_time': 'true', - }.items(), - condition=IfCondition(LaunchConfiguration('use_slam')), - ) - - # ── Nav2 navigation stack ───────────────────────────────────────────── - nav2 = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('nav2_bringup'), - 'launch', 'navigation_launch.py', - ]) - ), - launch_arguments={ - 'params_file': nav2_yaml, - 'use_sim_time': 'true', - }.items(), - condition=IfCondition(LaunchConfiguration('use_nav2')), - ) - - # ── RViz2 ───────────────────────────────────────────────────────────── - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', rviz_config], - parameters=[{'use_sim_time': True}], - condition=IfCondition(LaunchConfiguration('use_rviz')), - output='screen', - ) - - return LaunchDescription([ - # Arguments - world_arg, - slam_arg, - nav2_arg, - rviz_arg, - wheel_base_arg, - - # Core simulation - gz_sim, - rsp, - spawn_robot, - bridge, - - # ros2_control controllers (delayed — wait for gz to start) - spawn_jsb, - spawn_pan_tilt, - joint_state_relay, - - # Optional stacks - slam, - nav2, - rviz, - ]) diff --git a/robot/src/raspbot_v2/package.xml b/robot/src/raspbot_v2/package.xml deleted file mode 100644 index 47afd26..0000000 --- a/robot/src/raspbot_v2/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - raspbot_v2 - 0.0.1 - Yahboom Raspbot V2 motor and camera orientation controller - Your Name - Apache-2.0 - - rclpy - geometry_msgs - std_msgs - sensor_msgs - launch - launch_ros - - ament_python - - - ament_python - - ament_copyright - ament_flake8 - ament_pep257 - \ No newline at end of file diff --git a/robot/src/raspbot_v2/setup.cfg b/robot/src/raspbot_v2/setup.cfg deleted file mode 100644 index bc7c3ec..0000000 --- a/robot/src/raspbot_v2/setup.cfg +++ /dev/null @@ -1,13 +0,0 @@ -[metadata] -name = raspbot_v2 -version = 0.0.1 - -[options] -packages = find: -install_requires = - setuptools - -[develop] -script_dir=$base/lib/raspbot_v2 -[install] -install_scripts=$base/lib/raspbot_v2 diff --git a/robot/src/raspbot_v2/setup.py b/robot/src/raspbot_v2/setup.py deleted file mode 100644 index 75ff63c..0000000 --- a/robot/src/raspbot_v2/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -import glob - -from setuptools import setup - -package_name = 'raspbot_v2' - -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', glob.glob('launch/*.py')), - ('share/' + package_name + '/urdf', glob.glob('urdf/*.xacro')), - ('share/' + package_name + '/config', glob.glob('config/*')), - ('share/' + package_name + '/worlds', glob.glob('worlds/*.sdf')), - ], - install_requires=['setuptools'], - zip_safe=True, - entry_points={ - 'console_scripts': [ - 'motor_controller = raspbot_v2.motor_controller_node:main', - 'camera_orientation = raspbot_v2.camera_orientation_node:main', - 'ultrasonic = raspbot_v2.ultrasonic_node:main', - 'led_controller = raspbot_v2.led_node:main', - ], - }, -) \ No newline at end of file diff --git a/robot/src/raspbot_v2/urdf/camera.xacro b/robot/src/raspbot_v2/urdf/camera.xacro deleted file mode 100644 index c8eca3a..0000000 --- a/robot/src/raspbot_v2/urdf/camera.xacro +++ /dev/null @@ -1,83 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 30 - true - - ${camera_fov} - - ${camera_width} - ${camera_height} - R8G8B8 - - - 0.05 - 50.0 - - - gaussian - 0.0 - 0.007 - - - - /camera/image - - - - diff --git a/robot/src/raspbot_v2/urdf/lidar.xacro b/robot/src/raspbot_v2/urdf/lidar.xacro deleted file mode 100644 index 69e32f9..0000000 --- a/robot/src/raspbot_v2/urdf/lidar.xacro +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1 - 10 - true - /scan - laser - - - - 360 - 1 - -3.14159265 - 3.14159265 - - - - ${lidar_min_range} - ${lidar_max_range} - 0.01 - - - gaussian - 0.0 - 0.01 - - - - - - diff --git a/robot/src/raspbot_v2/urdf/pan_tilt.xacro b/robot/src/raspbot_v2/urdf/pan_tilt.xacro deleted file mode 100644 index 636f00b..0000000 --- a/robot/src/raspbot_v2/urdf/pan_tilt.xacro +++ /dev/null @@ -1,134 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - - - - - gz_ros2_control/GazeboSimSystem - - - - - ${pan_min_rad} - ${pan_max_rad} - - - - - - - ${tilt_min_rad} - ${tilt_max_rad} - - - - - - - diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro deleted file mode 100644 index 633c738..0000000 --- a/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - robot_description - robot_state_publisher - - - ${controllers_config} - - - - - diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro deleted file mode 100644 index 69bb155..0000000 --- a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robot/src/raspbot_v2/urdf/robot_base.xacro b/robot/src/raspbot_v2/urdf/robot_base.xacro deleted file mode 100644 index 12ee135..0000000 --- a/robot/src/raspbot_v2/urdf/robot_base.xacro +++ /dev/null @@ -1,155 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.0 - 0.5 - 1e6 - 1.0 - - - - - - - - - - - - wheel_front_left_joint - wheel_rear_left_joint - wheel_front_right_joint - wheel_rear_right_joint - ${wheel_separation} - ${wheel_radius} - 10 - /cmd_vel - /odom - /tf - odom - base_footprint - - - - - - - /gz/joint_states - wheel_front_left_joint - wheel_front_right_joint - wheel_rear_left_joint - wheel_rear_right_joint - - - - diff --git a/webui/backend/ros_node.py b/webui/backend/ros_node.py index 19f9865..d0ee394 100644 --- a/webui/backend/ros_node.py +++ b/webui/backend/ros_node.py @@ -2,7 +2,7 @@ import threading import xml.etree.ElementTree as ET import rclpy -from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import JointState, Range @@ -39,7 +39,7 @@ class RobotBridgeNode(Node): def __init__(self): super().__init__('webui_bridge') - self._cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) + self._cmd_vel_pub = self.create_publisher(TwistStamped, '/cmd_vel', 10) self._joint_cmd_pub = self.create_publisher(JointState, '/joint_command', 10) self._led_color_pub = self.create_publisher(ColorRGBA, '/led/color', 10) self._led_effect_pub = self.create_publisher(String, '/led/effect', 10) @@ -59,9 +59,10 @@ class RobotBridgeNode(Node): # ------------------------------------------------------------------ def publish_cmd_vel(self, linear_x: float, angular_z: float) -> None: - msg = Twist() - msg.linear.x = float(linear_x) - msg.angular.z = float(angular_z) + msg = TwistStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.twist.linear.x = float(linear_x) + msg.twist.angular.z = float(angular_z) self._cmd_vel_pub.publish(msg) def publish_joint_command(self, pan: float, tilt: float) -> None: