Migrate to a new physical robot
Uses the mecanum controller properly across physical and virtual There is a timing issue with i2c which is why the control update is limited to 10hz The sonar and LED's arent yet working, this will come soon.
This commit is contained in:
@@ -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
|
||||
|
||||
# ********************************************************
|
||||
|
||||
@@ -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/"
|
||||
}
|
||||
@@ -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
|
||||
|
||||
+9
-2
@@ -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:
|
||||
|
||||
@@ -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"]
|
||||
@@ -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
|
||||
```
|
||||
@@ -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(<dependency> 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()
|
||||
@@ -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.
|
||||
@@ -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,
|
||||
])
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_bringup</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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(<dependency> 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()
|
||||
@@ -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.
|
||||
@@ -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
|
||||
@@ -0,0 +1,3 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
@@ -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,
|
||||
])
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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()
|
||||
@@ -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.
|
||||
@@ -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}],
|
||||
),
|
||||
])
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,176 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||
xmlns:gz="http://gazebosim.org/schema"
|
||||
>
|
||||
<!-- ── base_footprint ───────────────────────────────────────────────────── -->
|
||||
<!-- Virtual ground-plane reference link. Gz Sim requires at least a
|
||||
minimal inertial so it survives the URDF→SDF conversion. -->
|
||||
<link name="base_footprint">
|
||||
<inertial>
|
||||
<mass value="0.001" />
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0"
|
||||
iyy="1e-6" iyz="0"
|
||||
izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint" />
|
||||
<child link="base_link" />
|
||||
<origin xyz="0 0 ${chassis_z_from_ground}" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<!-- ── base_link ────────────────────────────────────────────────────────── -->
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}" />
|
||||
</geometry>
|
||||
<material name="blue" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}" />
|
||||
</geometry>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="${base_mass}" />
|
||||
<!-- Solid-box inertia tensor -->
|
||||
<inertia
|
||||
ixx="${base_mass/12*(base_width**2 + base_height**2)}"
|
||||
ixy="0" ixz="0"
|
||||
iyy="${base_mass/12*(base_length**2 + base_height**2)}"
|
||||
iyz="0"
|
||||
izz="${base_mass/12*(base_length**2 + base_width**2)}" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- ── wheel macro ──────────────────────────────────────────────────────── -->
|
||||
<!-- fdir1 y-sign: for O-type mecanum the traction diagonal is -(x*y),
|
||||
where x=±1 (front/rear) and y=±1 (left/right in URDF frame). -->
|
||||
<xacro:macro name="wheel" params="name x y">
|
||||
<link name="wheel_${name}">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}" />
|
||||
</geometry>
|
||||
<material name="arrow" />
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}" />
|
||||
</geometry>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="${wheel_mass}" />
|
||||
<!-- Cylinder spinning around y-axis (joint axis="0 1 0"):
|
||||
Iyy = m·r²/2 (spin axis)
|
||||
Ixx = Izz = m·(3r²+h²)/12 (transverse axes) -->
|
||||
<inertia
|
||||
ixx="${wheel_mass*(3*wheel_radius**2 + wheel_width**2)/12}"
|
||||
ixy="0" ixz="0"
|
||||
iyy="${wheel_mass*wheel_radius**2/2}"
|
||||
iyz="0"
|
||||
izz="${wheel_mass*(3*wheel_radius**2 + wheel_width**2)/12}" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint_wheel_${name}" type="continuous">
|
||||
<parent link="base_link" />
|
||||
<child link="wheel_${name}" />
|
||||
<origin xyz="${wx*x} ${wy*y} ${wz}" rpy="0 0 0" />
|
||||
<axis xyz="0 1 0" />
|
||||
<dynamics damping="0.1" friction="0.0" />
|
||||
</joint>
|
||||
<!-- Anisotropic friction: mu = traction direction (high grip, perpendicular to
|
||||
roller), mu2 = roller direction (low resistance, allows roller to spin).
|
||||
fdir1 is the traction direction in the chassis frame. For O-type mecanum
|
||||
the y-sign of fdir1 is -(x*y): front-left/rear-right get -sin, and
|
||||
front-right/rear-left get +sin, matching the two roller diagonals.
|
||||
gz:expressed_in keeps fdir1 fixed in the chassis frame as the hub spins.
|
||||
In gz-sim9 (Harmonic) mu/mu2/fdir1 must be nested under
|
||||
collision/surface/friction/ode — the shorthand directly under <gazebo
|
||||
reference> is silently ignored. -->
|
||||
<gazebo reference="wheel_${name}">
|
||||
<collision>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>${wheel_roller_friction}</mu2>
|
||||
<fdir1 gz:expressed_in="base_footprint">${cos(roller_angle)} ${-x*y*sin(roller_angle)} 0</fdir1>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>100000.0</kp>
|
||||
<kd>1.0</kd>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:wheel name="front_left" x="1" y="1" />
|
||||
<xacro:wheel name="front_right" x="1" y="-1" />
|
||||
<xacro:wheel name="rear_left" x="-1" y="1" />
|
||||
<xacro:wheel name="rear_right" x="-1" y="-1" />
|
||||
|
||||
<!-- ── ros2_control: drive wheels ──────────────────────────────────────── -->
|
||||
<ros2_control name="mecanum_drive" type="system">
|
||||
<hardware>
|
||||
<xacro:if value="$(arg use_sim_time)">
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</xacro:if>
|
||||
<xacro:unless value="$(arg use_sim_time)">
|
||||
<xacro:if value="$(arg use_hardware)">
|
||||
<plugin>raspbot_v2_hardware_interface/RaspbotHardwareInterface</plugin>
|
||||
</xacro:if>
|
||||
<xacro:unless value="$(arg use_hardware)">
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
</xacro:unless>
|
||||
</xacro:unless>
|
||||
</hardware>
|
||||
<joint name="joint_wheel_front_left">
|
||||
<command_interface name="velocity" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
<joint name="joint_wheel_front_right">
|
||||
<command_interface name="velocity" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
<joint name="joint_wheel_rear_left">
|
||||
<command_interface name="velocity" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
<joint name="joint_wheel_rear_right">
|
||||
<command_interface name="velocity" />
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
<!-- ── Gazebo: gz_ros2_control plugin ───────────────────────────────────── -->
|
||||
<!-- Replaces the native MecanumDrive + JointStatePublisher plugins.
|
||||
The plugin hosts the controller_manager and loads controllers.yaml. -->
|
||||
<gazebo>
|
||||
<plugin filename="gz_ros2_control-system"
|
||||
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||
<parameters>$(find raspbot_v2_control)/config/controllers.yaml</parameters>
|
||||
<parameters>$(find raspbot_v2_control)/config/sim_overrides.yaml</parameters>
|
||||
<ros>
|
||||
<remapping>mecanum_drive_controller/reference:=/cmd_vel</remapping>
|
||||
<remapping>mecanum_drive_controller/odometry:=/odom</remapping>
|
||||
<remapping>mecanum_drive_controller/tf_odometry:=/tf</remapping>
|
||||
</ros>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,87 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Camera module
|
||||
|
||||
Mounted on camera_tilt_link (output of the tilt joint in pan_tilt.xacro).
|
||||
|
||||
Two frames are defined:
|
||||
camera_link — physical body of the camera module
|
||||
camera_optical_frame — REP-103 optical frame (Z forward, X right, Y down)
|
||||
used as the sensor origin in Gazebo and ROS image topics
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<!-- Physical camera body — visual only, small box representing the module -->
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.025 0.06 0.02" />
|
||||
</geometry>
|
||||
<material name="blue" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.025 0.06 0.02" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.03" />
|
||||
<inertia ixx="0.000005" ixy="0" ixz="0"
|
||||
iyy="0.000005" iyz="0" izz="0.000005" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent link="camera_tilt_link" />
|
||||
<child link="camera_link" />
|
||||
<origin xyz="0.01 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<!-- REP-103 optical frame: Z forward, X right, Y down.
|
||||
The rpy="-pi/2 0 -pi/2" rotation maps the body frame (X-fwd, Y-left, Z-up)
|
||||
to the optical frame convention expected by image_geometry and cv_bridge. -->
|
||||
<link name="camera_optical_frame" />
|
||||
|
||||
<joint name="camera_optical_joint" type="fixed">
|
||||
<parent link="camera_link" />
|
||||
<child link="camera_optical_frame" />
|
||||
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo camera sensor (Gazebo Harmonic / gz-sim)
|
||||
|
||||
Image data is published on gz topics and bridged to ROS 2 via
|
||||
ros_gz_bridge:
|
||||
/camera/image (gz) → /camera/image_raw (ROS 2)
|
||||
/camera/info (gz) → /camera/camera_info (ROS 2)
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo reference="camera_link">
|
||||
<sensor name="camera" type="camera">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<camera>
|
||||
<horizontal_fov>${camera_fov}</horizontal_fov>
|
||||
<image>
|
||||
<width>${camera_width}</width>
|
||||
<height>${camera_height}</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.05</near>
|
||||
<far>50.0</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<!-- Publishes to gz topic /camera/image -->
|
||||
<topic>/camera/image</topic>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,42 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
HC-SR04 ultrasonic range sensor
|
||||
|
||||
frame_id "ultrasonic" matches what ultrasonic_node publishes on
|
||||
/ultrasonic/range. Mounted on the front face of the chassis,
|
||||
pointing forward.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<xacro:property name="c_width" default="0.085" />
|
||||
<xacro:property name="c_depth" default="0.08" />
|
||||
<xacro:property name="c_height" default="0.05" />
|
||||
|
||||
|
||||
<link name="cpu_housing">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${c_width} ${c_depth} ${c_height}" />
|
||||
</geometry>
|
||||
<material name="green" />
|
||||
</visual>
|
||||
<!-- To simpllify physics for now, just model the base of the robot
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.05 0.06 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||
iyy="0.000001" iyz="0" izz="0.000001" />
|
||||
</inertial> -->
|
||||
</link>
|
||||
|
||||
<joint name="cpu_housing_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="cpu_housing" />
|
||||
<!-- Front-centre of the chassis, mid-height -->
|
||||
<origin xyz="-0.02 0.0 ${(base_height + c_height)/2}" rpy="0 0 0" />
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -0,0 +1,77 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
RPLIDAR A1 — 2D laser scanner
|
||||
|
||||
frame_id "laser" matches what sllidar_ros2 publishes on /scan.
|
||||
Adjust xyz to the physical mounting position on the chassis.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<link name="laser">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="0.04" length="0.04" />
|
||||
</geometry>
|
||||
<material name="light_grey" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="0.04" length="0.04" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.17" />
|
||||
<!-- Solid cylinder -->
|
||||
<inertia ixx="0.000087" ixy="0" ixz="0"
|
||||
iyy="0.000087" iyz="0"
|
||||
izz="0.000136" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<parent link="base_link" />
|
||||
<child link="laser" />
|
||||
<!-- x/y/z: position of lidar centre relative to base_link centre.
|
||||
x negative = rear-mounted; adjust to your physical layout. -->
|
||||
<origin xyz="${lidar_x} ${lidar_y} ${lidar_z}" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo gpu_lidar sensor (Gazebo Harmonic / gz-sim)
|
||||
|
||||
Scan data is published on gz topic /scan and bridged to ROS 2 via
|
||||
ros_gz_bridge:
|
||||
/scan (gz) → /scan (ROS 2, sensor_msgs/LaserScan)
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo reference="laser">
|
||||
<sensor name="lidar" type="gpu_lidar">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>10</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>/scan</topic>
|
||||
<gz_frame_id>laser</gz_frame_id>
|
||||
<lidar>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.14159265</min_angle>
|
||||
<max_angle> 3.14159265</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>${lidar_min_range}</min>
|
||||
<max>${lidar_max_range}</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</lidar>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1" />
|
||||
</material>
|
||||
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1" />
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 1 1" />
|
||||
</material>
|
||||
|
||||
<material name="arrow">
|
||||
<color rgba="0 1 0 1" />
|
||||
</material>
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
|
||||
<material name="light_grey">
|
||||
<color rgba="0.8 0.8 0.8 1" />
|
||||
</material>
|
||||
|
||||
<material name="dark_grey">
|
||||
<color rgba="0.2 0.2 0.2 1" />
|
||||
</material>
|
||||
</robot>
|
||||
@@ -0,0 +1,141 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Camera pan / tilt mechanism
|
||||
|
||||
Joint names (pan, tilt) must match the names published by
|
||||
camera_orientation_node on /joint_states so that robot_state_publisher
|
||||
drives the TF tree correctly from live hardware.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<xacro:include filename="raspbot_v2_params.urdf.xacro" />
|
||||
|
||||
|
||||
<!-- Mount bracket -->
|
||||
<link name="camera_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.03 0.04 0.03" />
|
||||
</geometry>
|
||||
<material name="dark_grey" />
|
||||
</visual>
|
||||
<!-- To simpllify physics, just model the base of the robot
|
||||
<inertial>
|
||||
<mass value="0.05" />
|
||||
<inertia ixx="0.00001" ixy="0" ixz="0"
|
||||
iyy="0.00001" iyz="0" izz="0.00001" />
|
||||
</inertial> -->
|
||||
</link>
|
||||
|
||||
<joint name="camera_base_joint" type="fixed">
|
||||
<parent link="ultrasonic" />
|
||||
<child link="camera_base" />
|
||||
<!-- Adjust xyz to match the physical servo mount position -->
|
||||
<origin xyz="0 0.0 0.02" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<!-- Pan link — rotates around Z (yaw) -->
|
||||
<link name="camera_pan_link">
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||
iyy="0.000001" iyz="0" izz="0.000001" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="pan" type="revolute">
|
||||
<parent link="camera_base" />
|
||||
<child link="camera_pan_link" />
|
||||
<axis xyz="0 0 1" />
|
||||
<origin xyz="0 0 0.02" rpy="0 0 0" />
|
||||
<!-- Physical range: 0°–180°, centred at 90° → ROS range: ±90° -->
|
||||
<limit lower="${pan_min_rad}" upper="${pan_max_rad}"
|
||||
effort="1.0" velocity="1.5" />
|
||||
<dynamics damping="0.1" friction="0.05" />
|
||||
</joint>
|
||||
|
||||
<!-- Tilt link — rotates around Y (pitch) -->
|
||||
<link name="camera_tilt_link">
|
||||
<inertial>
|
||||
<mass value="0.02" />
|
||||
<inertia ixx="0.000002" ixy="0" ixz="0"
|
||||
iyy="0.000002" iyz="0" izz="0.000002" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tilt" type="revolute">
|
||||
<parent link="camera_pan_link" />
|
||||
<child link="camera_tilt_link" />
|
||||
<axis xyz="0 1 0" />
|
||||
<origin xyz="0.02 0 0" rpy="0 0 0" />
|
||||
<!-- Physical range: 0°–110°, centred at 60° → ROS range: -60° to +50° -->
|
||||
<limit lower="${tilt_min_rad}" upper="${tilt_max_rad}"
|
||||
effort="1.0" velocity="1.5" />
|
||||
<dynamics damping="0.1" friction="0.05" />
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Transmissions (for ros2_control)
|
||||
|
||||
These describe the mapping between joint commands and actuators.
|
||||
Required when using gz_ros2_control for Gazebo simulation.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<transmission name="pan_transmission">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="pan">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="pan_actuator">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="tilt_transmission">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="tilt">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="tilt_actuator">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
ros2_control block
|
||||
|
||||
For Gazebo simulation, set the hardware plugin to:
|
||||
gz_ros2_control/GazeboSimSystem
|
||||
For the real robot, replace with the custom I²C servo hardware plugin.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<ros2_control name="pan_tilt_servos" type="system">
|
||||
<hardware>
|
||||
<xacro:if value="$(arg use_sim_time)">
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</xacro:if>
|
||||
<xacro:unless value="$(arg use_sim_time)">
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
</xacro:unless>
|
||||
</hardware>
|
||||
<joint name="pan">
|
||||
<command_interface name="position">
|
||||
<param name="min">${pan_min_rad}</param>
|
||||
<param name="max">${pan_max_rad}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
<joint name="tilt">
|
||||
<command_interface name="position">
|
||||
<param name="min">${tilt_min_rad}</param>
|
||||
<param name="max">${tilt_max_rad}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position" />
|
||||
<state_interface name="velocity" />
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
|
||||
xmlns:gz="http://gazebosim.org/schema"
|
||||
name="raspbot_v2">
|
||||
<xacro:include filename="raspbot_v2_params.urdf.xacro" />
|
||||
<xacro:include filename="materials.xacro" />
|
||||
|
||||
<xacro:include filename="base.xacro" />
|
||||
<xacro:include filename="sonar.xacro" />
|
||||
<xacro:include filename="pan_tilt.xacro" />
|
||||
<xacro:include filename="camera.xacro" />
|
||||
<xacro:include filename="cpu_housing.xacro" />
|
||||
<xacro:include filename="lidar.xacro" />
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,122 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
||||
<!-- ================================================================
|
||||
Top-level parameters
|
||||
All key dimensions are declared as xacro:arg so they can be
|
||||
overridden from a launch file or command line:
|
||||
|
||||
xacro mecanum_robot.urdf.xacro base_length:=0.6 wheel_radius:=0.1
|
||||
|
||||
Derived quantities (wheel offsets, inertia values) are computed
|
||||
automatically from these arguments.
|
||||
================================================================ -->
|
||||
|
||||
<xacro:arg name="use_sim_time" default="false" />
|
||||
<xacro:property name="use_sim_time" value="$(arg use_sim_time)" />
|
||||
<!-- Set true when running on real hardware (selects the I2C hardware interface plugin).
|
||||
Ignored when use_sim_time is true. Default false keeps mock_components for RViz-only use. -->
|
||||
<xacro:arg name="use_hardware" default="false" />
|
||||
|
||||
<!-- Base chassis dimensions (metres) -->
|
||||
<xacro:arg name="base_length" default="0.18" /> <!-- X: front-to-back -->
|
||||
<xacro:arg name="base_width" default="0.09" /> <!-- Y: side-to-side -->
|
||||
<xacro:arg name="base_height" default="0.03" /> <!-- Z: vertical depth -->
|
||||
<xacro:arg name="base_mass" default="05" /> <!-- kg -->
|
||||
|
||||
<!-- Wheel dimensions (metres) -->
|
||||
<xacro:arg name="wheel_radius" default="0.03" />
|
||||
<xacro:arg name="wheel_width" default="0.03" />
|
||||
<xacro:arg name="wheel_mass" default="0.1" /> <!-- kg, each wheel -->
|
||||
|
||||
<!-- Roller friction (mu2): near-zero lets rollers spin freely; increase to
|
||||
resist post-stop sliding. 0.0 = no resistance, 1.0 = full grip. -->
|
||||
<xacro:arg name="wheel_roller_friction" default="0.5" />
|
||||
|
||||
<!-- Roller angle (radians): angle of the traction direction relative to the
|
||||
wheel's forward axis. Standard mecanum wheels use pi/4 (45 degrees).
|
||||
Smaller values lean the force more forward; larger values more lateral. -->
|
||||
<xacro:arg name="roller_angle" default="0.7854" />
|
||||
|
||||
<!-- Sonar -->
|
||||
<xacro:arg name="sonar_min_range" default="0.02" /> <!-- m (HC-SR04 spec) -->
|
||||
<xacro:arg name="sonar_max_range" default="4.0" /> <!-- m (HC-SR04 spec) -->
|
||||
|
||||
<!-- Pan / tilt limits (radians, ROS convention: 0 = centre) -->
|
||||
<!-- Pan: physical 0°–180°, centred at 90° → ±π/2 -->
|
||||
<!-- Tilt: physical 0°–110°, centred at 60° → −60° (−1.047) to +50° (+0.873) -->
|
||||
<xacro:arg name="pan_min_rad" default="${-pi/2}" />
|
||||
<xacro:arg name="pan_max_rad" default="${ pi/2}" />
|
||||
<xacro:arg name="tilt_min_rad" default="${-pi/3}" /> <!-- −60° -->
|
||||
<xacro:arg name="tilt_max_rad" default="${5*pi/18}" /> <!-- +50° -->
|
||||
|
||||
<!-- Camera-->
|
||||
<xacro:arg name="camera_fov" default="1.047" /> <!-- rad (~60°) -->
|
||||
<xacro:arg name="camera_width" default="640" />
|
||||
<xacro:arg name="camera_height" default="480" />
|
||||
|
||||
|
||||
<!-- Lidar -->
|
||||
<!-- Mounting position relative to base_link centre -->
|
||||
<xacro:arg name="lidar_x" default="-0.03" /> <!-- m, slightly rearward -->
|
||||
<xacro:arg name="lidar_y" default="0.0" />
|
||||
<xacro:arg name="lidar_z" default="0.09" /> <!-- m above base_link centre -->
|
||||
<xacro:arg name="lidar_min_range" default="0.15" /> <!-- m (RPLIDAR A1 spec) -->
|
||||
<xacro:arg name="lidar_max_range" default="12.0" /> <!-- m (RPLIDAR A1 spec) -->
|
||||
|
||||
<!-- Pull args into properties so they can be used in math expressions -->
|
||||
<xacro:property name="base_length" value="$(arg base_length)" />
|
||||
<xacro:property name="base_width" value="$(arg base_width)" />
|
||||
<xacro:property name="base_height" value="$(arg base_height)" />
|
||||
<xacro:property name="base_mass" value="$(arg base_mass)" />
|
||||
|
||||
<xacro:property name="wheel_radius" value="$(arg wheel_radius)" />
|
||||
<xacro:property name="wheel_width" value="$(arg wheel_width)" />
|
||||
<xacro:property name="wheel_mass" value="$(arg wheel_mass)" />
|
||||
<xacro:property name="wheel_roller_friction" value="$(arg wheel_roller_friction)" />
|
||||
<xacro:property name="roller_angle" value="$(arg roller_angle)" />
|
||||
|
||||
<xacro:property name="sonar_min_range" value="$(arg sonar_min_range)" />
|
||||
<xacro:property name="sonar_max_range" value="$(arg sonar_max_range)" />
|
||||
|
||||
<xacro:property name="pan_min_rad" value="$(arg pan_min_rad)" />
|
||||
<xacro:property name="pan_max_rad" value="$(arg pan_max_rad)" />
|
||||
<xacro:property name="tilt_min_rad" value="$(arg tilt_min_rad)" />
|
||||
<xacro:property name="tilt_max_rad" value="$(arg tilt_max_rad)" />
|
||||
|
||||
<xacro:property name="camera_fov" value="$(arg camera_fov)" />
|
||||
<xacro:property name="camera_width" value="$(arg camera_width)" />
|
||||
<xacro:property name="camera_height" value="$(arg camera_height)" />
|
||||
|
||||
<xacro:property name="lidar_x" value="$(arg lidar_x)" />
|
||||
<xacro:property name="lidar_y" value="$(arg lidar_y)" />
|
||||
<xacro:property name="lidar_z" value="$(arg lidar_z)" />
|
||||
<xacro:property name="lidar_min_range" value="$(arg lidar_min_range)" />
|
||||
<xacro:property name="lidar_max_range" value="$(arg lidar_max_range)" />
|
||||
|
||||
<!-- <xacro:include filename="$(find raspbot_v2)/urdf/gazebo.xacro"/> -->
|
||||
|
||||
<!-- Derived geometry:
|
||||
base_footprint sits on the ground plane (z = 0).
|
||||
base_link origin is placed at height = wheel_radius + base_height/2
|
||||
so the bottom of the chassis clears the wheel axle line and the
|
||||
wheel centres end up at exactly wheel_radius above the ground.
|
||||
|
||||
Wheel x offset: pull the axle slightly inward from the chassis edge
|
||||
so the wheel hub looks natural.
|
||||
Wheel y offset: wheel centre is half-width outside the chassis side.
|
||||
-->
|
||||
<xacro:property name="chassis_z_from_ground"
|
||||
value="${wheel_radius}" />
|
||||
|
||||
<!-- Wheel x offset from base_link origin (along the robot's forward axis) -->
|
||||
<xacro:property name="wx" value="${base_length/2 - wheel_radius}" />
|
||||
|
||||
<!-- Wheel y offset: place wheel flush outside chassis side wall -->
|
||||
<xacro:property name="wy" value="${base_width/2 + wheel_width/2}" />
|
||||
|
||||
<!-- Wheel z offset relative to base_link origin.
|
||||
base_link is at (wheel_radius + base_height/2) above the ground.
|
||||
The wheel centre on the raspbot is aligned with the origin of base_link
|
||||
so the value is 0 -->
|
||||
<xacro:property name="wz" value="0" />
|
||||
</robot>
|
||||
+41
-10
@@ -8,27 +8,58 @@
|
||||
/ultrasonic/range. Mounted on the front face of the chassis,
|
||||
pointing forward.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<xacro:property name="u_base_length" default="0.05" /> <!-- x-->
|
||||
<xacro:property name="u_base_width" default="0.06" /> <!-- y -->
|
||||
<xacro:property name="u_base_height" default="0.03" /> <!-- z -->
|
||||
<xacro:property name="u_sensor_radius" default="0.008" />
|
||||
<xacro:property name="u_sensor_length" default="0.006" />
|
||||
<xacro:property name="u_sensor_separation" default="0.026" />
|
||||
|
||||
|
||||
<link name="ultrasonic">
|
||||
<visual>
|
||||
<geometry><box size="0.02 0.04 0.02"/></geometry>
|
||||
<material name="light_grey"/>
|
||||
<geometry>
|
||||
<box size="${u_base_length} ${u_base_width} ${u_base_height}" />
|
||||
</geometry>
|
||||
<material name="green" />
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${u_sensor_radius}" length="${u_sensor_length}" />
|
||||
</geometry>
|
||||
<material name="white" />
|
||||
<origin
|
||||
xyz="${u_base_length/2 + u_sensor_length/2} ${u_sensor_separation/2} 0"
|
||||
rpy="0 ${pi/2} 0" />
|
||||
</visual>
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${u_sensor_radius}" length="${u_sensor_length}" />
|
||||
</geometry>
|
||||
<material name="white" />
|
||||
<origin
|
||||
xyz="${u_base_length/2 + u_sensor_length/2} -${u_sensor_separation/2} 0"
|
||||
rpy="0 ${pi/2} 0" />
|
||||
</visual>
|
||||
|
||||
<!-- To simpllify physics for now, just model the base of the robot
|
||||
<collision>
|
||||
<geometry><box size="0.02 0.04 0.02"/></geometry>
|
||||
<geometry>
|
||||
<box size="0.05 0.06 0.03" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<mass value="0.01" />
|
||||
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||
iyy="0.000001" iyz="0" izz="0.000001"/>
|
||||
</inertial>
|
||||
iyy="0.000001" iyz="0" izz="0.000001" />
|
||||
</inertial> -->
|
||||
</link>
|
||||
|
||||
<joint name="ultrasonic_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="ultrasonic"/>
|
||||
<parent link="base_link" />
|
||||
<child link="ultrasonic" />
|
||||
<!-- Front-centre of the chassis, mid-height -->
|
||||
<origin xyz="${base_length/2 + 0.01} 0.0 0.0" rpy="0 0 0"/>
|
||||
<origin xyz="${((base_length/2) + 0.05 + 0.017)/2} 0.0 ${base_height}" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
@@ -78,4 +109,4 @@
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
</robot>
|
||||
@@ -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.
|
||||
@@ -0,0 +1,55 @@
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
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(
|
||||
'enable_camera', default_value='true',
|
||||
description='Enable the camera orientation controller node.'
|
||||
),
|
||||
]
|
||||
|
||||
# ros2_control stack (controller_manager + spawners) with real hardware plugin
|
||||
control_stack = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(
|
||||
get_package_share_directory('raspbot_v2_control'),
|
||||
'launch', 'controller_manager.launch.py'
|
||||
)
|
||||
),
|
||||
launch_arguments={
|
||||
'use_sim_time': 'false',
|
||||
'use_hardware': 'true',
|
||||
'namespace': namespace,
|
||||
'enable_camera': enable_camera,
|
||||
}.items(),
|
||||
)
|
||||
|
||||
camera_orientation_controller = Node(
|
||||
package='raspbot_v2_hardware',
|
||||
executable='camera_orientation_controller',
|
||||
name='camera_orientation_controller',
|
||||
output='screen',
|
||||
condition=IfCondition(enable_camera),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
*declared_args,
|
||||
control_stack,
|
||||
camera_orientation_controller,
|
||||
])
|
||||
@@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_hardware</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_mypy</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>ament_xmllint</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
+1
-1
@@ -85,7 +85,7 @@ class CameraOrientationNode(Node):
|
||||
|
||||
# --- Hardware ---
|
||||
# Import here so the node can be unit-tested without hardware present
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot
|
||||
from raspbot_v2_hardware.raspbot_lib import Raspbot
|
||||
self._bot = Raspbot()
|
||||
|
||||
# --- State (degrees, internal representation) ---
|
||||
+6
-2
@@ -23,11 +23,15 @@ effect_color int default 0 – colour index for breathing effect
|
||||
"""
|
||||
|
||||
import threading
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import ColorRGBA, String
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from raspbot_v2_hardware.raspbot_lib import LightShow
|
||||
|
||||
VALID_EFFECTS = {'river', 'breathing', 'gradient', 'random_running', 'starlight', 'off'}
|
||||
|
||||
|
||||
@@ -39,12 +43,12 @@ class LedNode(Node):
|
||||
self.declare_parameter('effect_speed', 0.05)
|
||||
self.declare_parameter('effect_color', 0)
|
||||
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot, LightShow
|
||||
from raspbot_v2_hardware.raspbot_lib import Raspbot, LightShow
|
||||
self._Raspbot = Raspbot
|
||||
self._LightShow = LightShow
|
||||
self._bot = Raspbot()
|
||||
|
||||
self._light_show: object | None = None
|
||||
self._light_show: 'LightShow | None' = None
|
||||
self._effect_thread: threading.Thread | None = None
|
||||
|
||||
self.create_subscription(ColorRGBA, 'led/color', self._color_cb, 10)
|
||||
+3
-3
@@ -1,4 +1,4 @@
|
||||
from .Raspbot_Lib import Raspbot
|
||||
from .raspbot_lib import Raspbot
|
||||
|
||||
# Motor ID assignments (from Raspbot_Lib comments)
|
||||
_MOTOR_FL = 0 # L1 — front left
|
||||
@@ -11,7 +11,7 @@ _DRIVER_MAX = 255
|
||||
|
||||
class MotorController:
|
||||
"""
|
||||
Thin adapter between the ROS 2 motor controller node and Raspbot_Lib.
|
||||
Thin adapter between the ROS 2 motor controller node and raspbot_lib.
|
||||
|
||||
Speed convention: the node passes values in the range
|
||||
[-max_speed, max_speed] (default max_speed = 1.0). This class scales
|
||||
@@ -22,7 +22,7 @@ class MotorController:
|
||||
def __init__(self, max_speed: float = 1.0):
|
||||
self._max_speed = max_speed
|
||||
self._bot: Raspbot | None = None
|
||||
# Shadow register — Raspbot_Lib has no read-back, so we track state.
|
||||
# Shadow register — Raspbot has no read-back, so we track state.
|
||||
self._speeds = [0.0, 0.0, 0.0, 0.0] # [FL, FR, RL, RR]
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
+2
-2
@@ -4,8 +4,8 @@ from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
# Import your motor library
|
||||
from raspbot_v2_interface import MotorController
|
||||
from .motor_controller import MotorController
|
||||
|
||||
|
||||
class MotorControllerNode(Node):
|
||||
def __init__(self):
|
||||
@@ -0,0 +1,307 @@
|
||||
#!/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
|
||||
import 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):
|
||||
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 Exception:
|
||||
print('write_u8 I2C error')
|
||||
|
||||
def write_reg(self, reg):
|
||||
try:
|
||||
self._device.write_byte(self._addr, reg)
|
||||
except Exception:
|
||||
print('write_u8 I2C error')
|
||||
|
||||
def write_array(self, reg, data):
|
||||
try:
|
||||
self._device.write_i2c_block_data(self._addr, reg, data)
|
||||
except Exception:
|
||||
print('write_array I2C error')
|
||||
|
||||
def read_data_byte(self):
|
||||
try:
|
||||
buf = self._device.write_byte(self._addr)
|
||||
return buf
|
||||
except Exception:
|
||||
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 Exception:
|
||||
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
|
||||
self.write_array(0x01, [motor_id, motor_dir, motor_speed])
|
||||
except Exception:
|
||||
print('Ctrl_Car I2C error')
|
||||
|
||||
def Ctrl_Muto(self, motor_id, motor_speed):
|
||||
try:
|
||||
if motor_speed > 255:
|
||||
motor_speed = 255
|
||||
if motor_speed < -255:
|
||||
motor_speed = -255
|
||||
motor_dir = 1 if motor_speed < 0 else 0
|
||||
self.write_array(0x01, [motor_id, motor_dir, abs(motor_speed)])
|
||||
except Exception:
|
||||
print('Ctrl_Car I2C error')
|
||||
|
||||
def Ctrl_Servo(self, id, angle):
|
||||
try:
|
||||
if angle < 0:
|
||||
angle = 0
|
||||
elif angle > 180:
|
||||
angle = 180
|
||||
if id == 2 and angle > 110:
|
||||
angle = 110
|
||||
self.write_array(0x02, [id, angle])
|
||||
except Exception:
|
||||
print('Ctrl_Servo I2C error')
|
||||
|
||||
def Ctrl_WQ2812_ALL(self, state, color):
|
||||
try:
|
||||
state = max(0, min(1, state))
|
||||
self.write_array(0x03, [state, color])
|
||||
except Exception:
|
||||
print('Ctrl_WQ2812 I2C error')
|
||||
|
||||
def Ctrl_WQ2812_Alone(self, number, state, color):
|
||||
try:
|
||||
state = max(0, min(1, state))
|
||||
self.write_array(0x04, [number, state, color])
|
||||
except Exception:
|
||||
print('Ctrl_WQ2812_Alone I2C error')
|
||||
|
||||
def Ctrl_WQ2812_brightness_ALL(self, R, G, B):
|
||||
try:
|
||||
self.write_array(0x08, [min(R, 255), min(G, 255), min(B, 255)])
|
||||
except Exception:
|
||||
print('Ctrl_WQ2812 I2C error')
|
||||
|
||||
def Ctrl_WQ2812_brightness_Alone(self, number, R, G, B):
|
||||
try:
|
||||
self.write_array(0x09, [number, min(R, 255), min(G, 255), min(B, 255)])
|
||||
except Exception:
|
||||
print('Ctrl_WQ2812_Alone I2C error')
|
||||
|
||||
def Ctrl_IR_Switch(self, state):
|
||||
try:
|
||||
state = max(0, min(1, state))
|
||||
self.write_array(0x05, [state])
|
||||
except Exception:
|
||||
print('Ctrl_IR_Switch I2C error')
|
||||
|
||||
def Ctrl_BEEP_Switch(self, state):
|
||||
try:
|
||||
state = max(0, min(1, state))
|
||||
self.write_array(0x06, [state])
|
||||
except Exception:
|
||||
print('Ctrl_BEEP_Switch I2C error')
|
||||
|
||||
def Ctrl_Ulatist_Switch(self, state):
|
||||
try:
|
||||
state = max(0, min(1, state))
|
||||
self.write_array(0x07, [state])
|
||||
except Exception:
|
||||
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]
|
||||
return (diss_H << 8) | diss_L
|
||||
except Exception:
|
||||
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):
|
||||
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:
|
||||
color_map = {
|
||||
0: (breath_count, 0, 0),
|
||||
1: (0, breath_count, 0),
|
||||
2: (0, 0, breath_count),
|
||||
3: (breath_count, breath_count, 0),
|
||||
4: (breath_count, 0, breath_count),
|
||||
5: (0, breath_count, breath_count),
|
||||
6: (breath_count, breath_count, breath_count),
|
||||
}
|
||||
r, g, b = color_map.get(current_color, (0, 0, 0))
|
||||
self.bot.Ctrl_WQ2812_brightness_ALL(r, g, b)
|
||||
time.sleep(speed)
|
||||
if breath_direction == 0:
|
||||
breath_count = min(breath_count + 2, 255)
|
||||
if breath_count >= 255:
|
||||
breath_direction = 1
|
||||
else:
|
||||
breath_count = max(breath_count - 2, 0)
|
||||
if breath_count <= 0:
|
||||
breath_direction = 0
|
||||
self.turn_off_all_lights()
|
||||
|
||||
def random_running_light(self, effect_duration, speed):
|
||||
end_time = time.time()
|
||||
while self.running and time.time() - end_time < effect_duration:
|
||||
for i in range(self.num_lights):
|
||||
self.bot.Ctrl_WQ2812_Alone(i, 1, random.randint(0, 6))
|
||||
time.sleep(speed)
|
||||
self.turn_off_all_lights()
|
||||
|
||||
def starlight_shimmer(self, effect_duration, speed):
|
||||
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(1, 7),
|
||||
)
|
||||
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
|
||||
gt_red = gt_green = gt_blue = 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 = self.rgb_remix(random.randint(0, 255))
|
||||
gt_red, gt_green, gt_blue = self.rgb_remix_u8(gt_red, gt_green, random.randint(0, 255))
|
||||
grad_color += 1
|
||||
|
||||
if grad_color == 1:
|
||||
if grad_index < 14:
|
||||
self.bot.Ctrl_WQ2812_brightness_Alone(
|
||||
(grad_index % 14) + 1, 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:
|
||||
self.bot.Ctrl_WQ2812_brightness_Alone(
|
||||
(14 - grad_index) % 14, 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
|
||||
else:
|
||||
b = 0
|
||||
return r, g, b
|
||||
|
||||
def stop(self):
|
||||
self.running = False
|
||||
+1
-1
@@ -54,7 +54,7 @@ class UltrasonicNode(Node):
|
||||
rate_hz = self.get_parameter('publish_rate_hz').value
|
||||
|
||||
# --- Hardware ---
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot
|
||||
from raspbot_v2_hardware.raspbot_lib import Raspbot
|
||||
self._bot = Raspbot()
|
||||
|
||||
# --- Sensor state ---
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/raspbot_v2_hardware
|
||||
[install]
|
||||
install_scripts=$base/lib/raspbot_v2_hardware
|
||||
@@ -0,0 +1,34 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'raspbot_v2_hardware'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch', ['launch/hardware.launch.py']),
|
||||
],
|
||||
package_data={'': ['py.typed']},
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='Matt Spencer',
|
||||
maintainer_email='matthew@thespencers.me.uk',
|
||||
description='TODO: Package description',
|
||||
license='MIT',
|
||||
extras_require={
|
||||
'test': [
|
||||
'pytest',
|
||||
],
|
||||
},
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'camera_orientation_controller = raspbot_v2_hardware.camera_orientation_node:main',
|
||||
'led_controller = raspbot_v2_hardware.led_node:main',
|
||||
'ultrasonic_controller = raspbot_v2_hardware.ultrasonic_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright() -> None:
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8() -> None:
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2025 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_mypy.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.mypy
|
||||
@pytest.mark.linter
|
||||
def test_mypy() -> None:
|
||||
rc = main(argv=[])
|
||||
assert rc == 0, 'Found type errors!'
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257() -> None:
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_xmllint.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.xmllint
|
||||
def test_xmllint() -> None:
|
||||
rc = main(argv=[])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
@@ -0,0 +1,54 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(raspbot_v2_hardware_interface)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/raspbot_hardware_interface.cpp
|
||||
)
|
||||
|
||||
target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
|
||||
|
||||
target_include_directories(${PROJECT_NAME} PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME} PUBLIC
|
||||
hardware_interface::hardware_interface
|
||||
hardware_interface::mock_components
|
||||
pluginlib::pluginlib
|
||||
rclcpp::rclcpp
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(
|
||||
hardware_interface raspbot_v2_hardware_interface_plugin.xml
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include/${PROJECT_NAME}
|
||||
)
|
||||
install(
|
||||
TARGETS ${PROJECT_NAME}
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
install(
|
||||
FILES raspbot_v2_hardware_interface_plugin.xml
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
|
||||
ament_export_dependencies(hardware_interface pluginlib rclcpp)
|
||||
|
||||
ament_package()
|
||||
+74
@@ -0,0 +1,74 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
|
||||
namespace raspbot_v2_hardware_interface
|
||||
{
|
||||
|
||||
class RaspbotHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
|
||||
|
||||
CallbackReturn on_init(
|
||||
const hardware_interface::HardwareComponentInterfaceParams & params) override;
|
||||
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
|
||||
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
|
||||
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time & time, const rclcpp::Duration & period) override;
|
||||
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time & time, const rclcpp::Duration & period) override;
|
||||
|
||||
private:
|
||||
int i2c_fd_{-1};
|
||||
int i2c_bus_{1};
|
||||
double max_wheel_vel_{10.0}; // rad/s that maps to driver value 255
|
||||
|
||||
// Accessed only from the RT control-loop thread — no lock needed.
|
||||
std::array<double, 4> wheel_positions_{};
|
||||
std::array<double, 4> wheel_velocities_{};
|
||||
std::array<double, 4> wheel_commands_{};
|
||||
|
||||
// The Yahboom STM32 motor controller clock-stretches the I2C bus for ~80 ms
|
||||
// per bulk write while it applies PWM. A blocking write() in the RT control
|
||||
// loop therefore overruns the 20 ms budget at 50 Hz.
|
||||
//
|
||||
// This background thread decouples I2C from the RT loop: write() deposits
|
||||
// the latest commands under a brief mutex hold and returns immediately; the
|
||||
// thread drains the buffer at whatever rate the hardware allows (~12 Hz).
|
||||
std::thread i2c_thread_;
|
||||
std::mutex cmd_mutex_;
|
||||
std::condition_variable cmd_cv_;
|
||||
std::array<double, 4> desired_commands_{}; // protected by cmd_mutex_
|
||||
bool cmd_pending_{false}; // protected by cmd_mutex_
|
||||
bool i2c_thread_running_{false}; // protected by cmd_mutex_
|
||||
|
||||
void i2c_write_loop();
|
||||
void stop_i2c_thread();
|
||||
|
||||
// Joint order: [FL, FR, RL, RR]
|
||||
static constexpr int MOTOR_FL = 0;
|
||||
static constexpr int MOTOR_RL = 1;
|
||||
static constexpr int MOTOR_FR = 2;
|
||||
static constexpr int MOTOR_RR = 3;
|
||||
static constexpr int I2C_DEVICE_ADDR = 0x2B;
|
||||
|
||||
bool ctrl_muto(int motor_id, int speed);
|
||||
int to_motor_speed(double rad_s) const;
|
||||
void stop_all_motors();
|
||||
};
|
||||
|
||||
} // namespace raspbot_v2_hardware_interface
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_hardware_interface</name>
|
||||
<version>0.0.1</version>
|
||||
<description>ros2_control hardware interface plugin for Raspbot V2 mecanum drive</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,12 @@
|
||||
<library path="raspbot_v2_hardware_interface">
|
||||
<class
|
||||
name="raspbot_v2_hardware_interface/RaspbotHardwareInterface"
|
||||
type="raspbot_v2_hardware_interface::RaspbotHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ros2_control hardware interface for the Raspbot V2 mecanum drive.
|
||||
Translates joint velocity commands from MecanumDriveController into
|
||||
I2C motor commands via the Yahboom MCU (register 0x01, address 0x2B).
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -0,0 +1,166 @@
|
||||
#include "raspbot_v2_hardware_interface/raspbot_hardware_interface.hpp"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <linux/i2c-dev.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace raspbot_v2_hardware_interface
|
||||
{
|
||||
|
||||
hardware_interface::CallbackReturn RaspbotHardwareInterface::on_init(
|
||||
const hardware_interface::HardwareComponentInterfaceParams & params)
|
||||
{
|
||||
if (SystemInterface::on_init(params) != CallbackReturn::SUCCESS) {
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (info_.hardware_parameters.count("max_wheel_velocity_rad_s")) {
|
||||
max_wheel_vel_ = std::stod(info_.hardware_parameters.at("max_wheel_velocity_rad_s"));
|
||||
}
|
||||
if (info_.hardware_parameters.count("i2c_bus")) {
|
||||
i2c_bus_ = std::stoi(info_.hardware_parameters.at("i2c_bus"));
|
||||
}
|
||||
|
||||
wheel_positions_.fill(0.0);
|
||||
wheel_velocities_.fill(0.0);
|
||||
wheel_commands_.fill(0.0);
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RaspbotHardwareInterface::on_configure(
|
||||
const rclcpp_lifecycle::State &)
|
||||
{
|
||||
std::string dev = "/dev/i2c-" + std::to_string(i2c_bus_);
|
||||
i2c_fd_ = ::open(dev.c_str(), O_RDWR);
|
||||
if (i2c_fd_ < 0) {
|
||||
RCLCPP_ERROR(get_logger(), "Failed to open %s: %s", dev.c_str(), strerror(errno));
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
if (::ioctl(i2c_fd_, I2C_SLAVE, I2C_DEVICE_ADDR) < 0) {
|
||||
RCLCPP_ERROR(
|
||||
get_logger(), "Failed to set I2C slave 0x%02X: %s", I2C_DEVICE_ADDR, strerror(errno));
|
||||
::close(i2c_fd_);
|
||||
i2c_fd_ = -1;
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
get_logger(), "I2C opened on %s, slave 0x%02X, max vel %.1f rad/s",
|
||||
dev.c_str(), I2C_DEVICE_ADDR, max_wheel_vel_);
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RaspbotHardwareInterface::on_activate(
|
||||
const rclcpp_lifecycle::State &)
|
||||
{
|
||||
stop_all_motors();
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RaspbotHardwareInterface::on_deactivate(
|
||||
const rclcpp_lifecycle::State &)
|
||||
{
|
||||
stop_all_motors();
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RaspbotHardwareInterface::on_cleanup(
|
||||
const rclcpp_lifecycle::State &)
|
||||
{
|
||||
if (i2c_fd_ >= 0) {
|
||||
::close(i2c_fd_);
|
||||
i2c_fd_ = -1;
|
||||
}
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RaspbotHardwareInterface::read(
|
||||
const rclcpp::Time &, const rclcpp::Duration & period)
|
||||
{
|
||||
// No encoder read-back: mirror the last command as velocity and integrate position.
|
||||
const double dt = period.seconds();
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
wheel_velocities_[i] = wheel_commands_[i];
|
||||
wheel_positions_[i] += wheel_velocities_[i] * dt;
|
||||
}
|
||||
|
||||
set_state("joint_wheel_front_left/position", wheel_positions_[0]);
|
||||
set_state("joint_wheel_front_right/position", wheel_positions_[1]);
|
||||
set_state("joint_wheel_rear_left/position", wheel_positions_[2]);
|
||||
set_state("joint_wheel_rear_right/position", wheel_positions_[3]);
|
||||
|
||||
set_state("joint_wheel_front_left/velocity", wheel_velocities_[0]);
|
||||
set_state("joint_wheel_front_right/velocity", wheel_velocities_[1]);
|
||||
set_state("joint_wheel_rear_left/velocity", wheel_velocities_[2]);
|
||||
set_state("joint_wheel_rear_right/velocity", wheel_velocities_[3]);
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RaspbotHardwareInterface::write(
|
||||
const rclcpp::Time &, const rclcpp::Duration &)
|
||||
{
|
||||
wheel_commands_[0] = get_command<double>("joint_wheel_front_left/velocity");
|
||||
wheel_commands_[1] = get_command<double>("joint_wheel_front_right/velocity");
|
||||
wheel_commands_[2] = get_command<double>("joint_wheel_rear_left/velocity");
|
||||
wheel_commands_[3] = get_command<double>("joint_wheel_rear_right/velocity");
|
||||
|
||||
// Sanitise NaN (controller not yet active)
|
||||
for (auto & v : wheel_commands_) {
|
||||
if (std::isnan(v)) {v = 0.0;}
|
||||
}
|
||||
|
||||
ctrl_muto(MOTOR_FL, to_motor_speed(wheel_commands_[0]));
|
||||
ctrl_muto(MOTOR_FR, to_motor_speed(wheel_commands_[1]));
|
||||
ctrl_muto(MOTOR_RL, to_motor_speed(wheel_commands_[2]));
|
||||
ctrl_muto(MOTOR_RR, to_motor_speed(wheel_commands_[3]));
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Private helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool RaspbotHardwareInterface::ctrl_muto(int motor_id, int speed)
|
||||
{
|
||||
const int clamped = std::clamp(speed, -255, 255);
|
||||
const uint8_t dir = (clamped < 0) ? 1 : 0;
|
||||
const uint8_t abs_speed = static_cast<uint8_t>(std::abs(clamped));
|
||||
// Register 0x01: [motor_id, direction, speed] — mirrors Raspbot_Lib Ctrl_Muto
|
||||
uint8_t buf[] = {0x01, static_cast<uint8_t>(motor_id), dir, abs_speed};
|
||||
return ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
|
||||
}
|
||||
|
||||
int RaspbotHardwareInterface::to_motor_speed(double rad_s) const
|
||||
{
|
||||
if (max_wheel_vel_ <= 0.0) {return 0;}
|
||||
const double scaled = (rad_s / max_wheel_vel_) * 255.0;
|
||||
return static_cast<int>(std::clamp(scaled, -255.0, 255.0));
|
||||
}
|
||||
|
||||
void RaspbotHardwareInterface::stop_all_motors()
|
||||
{
|
||||
for (int id : {MOTOR_FL, MOTOR_RL, MOTOR_FR, MOTOR_RR}) {
|
||||
ctrl_muto(id, 0);
|
||||
}
|
||||
wheel_commands_.fill(0.0);
|
||||
}
|
||||
|
||||
} // namespace raspbot_v2_hardware_interface
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
raspbot_v2_hardware_interface::RaspbotHardwareInterface,
|
||||
hardware_interface::SystemInterface)
|
||||
@@ -0,0 +1,32 @@
|
||||
cmake_minimum_required(VERSION 3.20)
|
||||
project(raspbot_v2_sim)
|
||||
|
||||
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(<dependency> REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch rviz worlds
|
||||
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()
|
||||
@@ -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.
|
||||
@@ -0,0 +1,103 @@
|
||||
import os
|
||||
|
||||
from ament_index_python import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
def generate_launch_description():
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
rviz = LaunchConfiguration('rviz')
|
||||
|
||||
declared_args = [
|
||||
DeclareLaunchArgument(
|
||||
'namespace', default_value='',
|
||||
description='ROS namespace for all robot nodes (e.g. "robot1").'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'rviz', default_value='false',
|
||||
description='If true, launch RViz.'
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
pkg_share = get_package_share_directory('raspbot_v2_sim')
|
||||
rviz_config_dir = os.path.join(pkg_share, 'rviz', 'raspbot_v2.rviz')
|
||||
world_file = os.path.join(pkg_share, 'worlds', 'warehouse_world')
|
||||
|
||||
bringup_robot = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('raspbot_v2_bringup'), 'launch', 'raspbot_v2_bringup.launch.py'
|
||||
])
|
||||
]),
|
||||
launch_arguments={
|
||||
'namespace': namespace,
|
||||
'use_sim_time': 'true',
|
||||
}.items()
|
||||
)
|
||||
|
||||
gazebo_node = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'
|
||||
])
|
||||
]),
|
||||
launch_arguments={
|
||||
'gz_args': ['-r ', world_file],
|
||||
'on_exit_shutdown': 'true',
|
||||
}.items(),
|
||||
)
|
||||
|
||||
|
||||
# Spawn the robot entity into Gazebo after the world has loaded.
|
||||
# This triggers gz_ros2_control::GazeboSimROS2ControlPlugin, which hosts
|
||||
# the controller_manager. Without this the /controller_manager service
|
||||
# never appears, even though Gazebo is running.
|
||||
spawn_robot = TimerAction(
|
||||
period=3.0,
|
||||
actions=[Node(
|
||||
package='ros_gz_sim',
|
||||
executable='create',
|
||||
parameters=[{
|
||||
'topic': '/robot_description',
|
||||
'name': 'raspbot_v2',
|
||||
'x': 0.0, 'y': 0.0, 'z': 0.05,
|
||||
}],
|
||||
output='screen',
|
||||
)],
|
||||
)
|
||||
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=['-d', rviz_config_dir],
|
||||
output='screen',
|
||||
condition=IfCondition(rviz),
|
||||
)
|
||||
|
||||
|
||||
clock_bridge = Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
|
||||
'/camera/image@sensor_msgs/msg/Image[gz.msgs.Image',
|
||||
'/camera/camera_info@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo',
|
||||
'/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan',
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
*declared_args,
|
||||
gazebo_node,
|
||||
clock_bridge,
|
||||
bringup_robot,
|
||||
spawn_robot,
|
||||
rviz_node,
|
||||
])
|
||||
@@ -0,0 +1,49 @@
|
||||
import os
|
||||
|
||||
from ament_index_python import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, TimerAction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
def generate_launch_description():
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
|
||||
declared_args = [
|
||||
DeclareLaunchArgument(
|
||||
'namespace', default_value='',
|
||||
description='ROS namespace for all robot nodes (e.g. "robot1").'
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
rviz_config_dir = os.path.join(
|
||||
get_package_share_directory('raspbot_v2_sim'),
|
||||
'rviz', 'raspbot_v2.rviz')
|
||||
|
||||
bringup_robot = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('raspbot_v2_bringup'), 'launch', 'raspbot_v2_bringup.launch.py'
|
||||
])
|
||||
]),
|
||||
launch_arguments={
|
||||
'namespace': namespace,
|
||||
'use_sim_time': 'false',
|
||||
}.items()
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
*declared_args,
|
||||
bringup_robot,
|
||||
Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
arguments=['-d', rviz_config_dir],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
])
|
||||
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2_sim</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,275 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 555
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Filter (blacklist): ""
|
||||
Filter (whitelist): ""
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
wheel_front_left:
|
||||
Value: true
|
||||
wheel_front_right:
|
||||
Value: true
|
||||
wheel_rear_left:
|
||||
Value: true
|
||||
wheel_rear_right:
|
||||
Value: true
|
||||
Marker Scale: 0.20000000298023224
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
base_footprint:
|
||||
base_link:
|
||||
wheel_front_left:
|
||||
{}
|
||||
wheel_front_right:
|
||||
{}
|
||||
wheel_rear_left:
|
||||
{}
|
||||
wheel_rear_right:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheel_front_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheel_front_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheel_rear_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
wheel_rear_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz_default_plugins/LaserScan
|
||||
Color: 0; 255; 0
|
||||
Color Transformer: FlatColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /scan
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Camera
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /camera/image
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- 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
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 0.6006462574005127
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 66
|
||||
Y: 60
|
||||
@@ -0,0 +1,89 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.9">
|
||||
<world name="warehouse">
|
||||
|
||||
<!-- ── Physics ─────────────────────────────────────────────────────── -->
|
||||
<physics name="1ms" type="ignored">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
|
||||
<!-- ── Required gz-sim system plugins ───────────────────────────────── -->
|
||||
<plugin filename="gz-sim-physics-system"
|
||||
name="gz::sim::systems::Physics"/>
|
||||
<plugin filename="gz-sim-user-commands-system"
|
||||
name="gz::sim::systems::UserCommands"/>
|
||||
<plugin filename="gz-sim-scene-broadcaster-system"
|
||||
name="gz::sim::systems::SceneBroadcaster"/>
|
||||
|
||||
<!-- Sensors system — required for lidar, camera, and IMU sensors -->
|
||||
<plugin filename="gz-sim-sensors-system"
|
||||
name="gz::sim::systems::Sensors">
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
|
||||
<!-- Contact sensor system — required for collision detection -->
|
||||
<plugin filename="gz-sim-contact-system"
|
||||
name="gz::sim::systems::Contact"/>
|
||||
|
||||
<!-- ── Lighting ─────────────────────────────────────────────────────── -->
|
||||
<light name="sun" type="directional">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
</light>
|
||||
|
||||
<!-- ── Ground plane ─────────────────────────────────────────────────── -->
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100</mu>
|
||||
<mu2>50</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- ── Small Warehouse (OpenRobotics / Gazebo Fuel) ─────────────────── -->
|
||||
<include>
|
||||
<uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Warehouse</uri>
|
||||
<name>warehouse</name>
|
||||
<!-- Offset so the mesh floor (native z≈0.0986) sits at world z=0 -->
|
||||
<pose>0 0 -0.0986 0 0 0</pose>
|
||||
</include>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
@@ -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"]
|
||||
-302
@@ -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)
|
||||
```
|
||||
@@ -1,5 +0,0 @@
|
||||
## Installation Steps (安装步骤)
|
||||
|
||||
cd py_install
|
||||
|
||||
sudo python3 setup.py install
|
||||
@@ -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)
|
||||
|
||||
@@ -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.
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
from .Raspbot_Lib import Raspbot,LightShow
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,3 +0,0 @@
|
||||
from .motor_controller import MotorController
|
||||
|
||||
__all__ = ["MotorController"]
|
||||
@@ -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)
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
from .Raspbot_Lib import Raspbot,LightShow
|
||||
Binary file not shown.
Binary file not shown.
@@ -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'
|
||||
)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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: <Fixed 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: <Fixed 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
|
||||
@@ -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
|
||||
@@ -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])
|
||||
@@ -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',
|
||||
),
|
||||
|
||||
])
|
||||
@@ -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,
|
||||
])
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Yahboom Raspbot V2 motor and camera orientation controller</description>
|
||||
<maintainer email="you@example.com">Your Name</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
</package>
|
||||
@@ -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
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,83 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Camera module
|
||||
|
||||
Mounted on camera_tilt_link (output of the tilt joint in pan_tilt.xacro).
|
||||
|
||||
Two frames are defined:
|
||||
camera_link — physical body of the camera module
|
||||
camera_optical_frame — REP-103 optical frame (Z forward, X right, Y down)
|
||||
used as the sensor origin in Gazebo and ROS image topics
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<!-- Physical camera body — visual only, small box representing the module -->
|
||||
<link name="camera_link">
|
||||
<visual>
|
||||
<geometry><box size="0.025 0.06 0.02"/></geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry><box size="0.025 0.06 0.02"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.03"/>
|
||||
<inertia ixx="0.000005" ixy="0" ixz="0"
|
||||
iyy="0.000005" iyz="0" izz="0.000005"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="camera_joint" type="fixed">
|
||||
<parent link="camera_tilt_link"/>
|
||||
<child link="camera_link"/>
|
||||
<origin xyz="0.01 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- REP-103 optical frame: Z forward, X right, Y down.
|
||||
The rpy="-pi/2 0 -pi/2" rotation maps the body frame (X-fwd, Y-left, Z-up)
|
||||
to the optical frame convention expected by image_geometry and cv_bridge. -->
|
||||
<link name="camera_optical_frame"/>
|
||||
|
||||
<joint name="camera_optical_joint" type="fixed">
|
||||
<parent link="camera_link"/>
|
||||
<child link="camera_optical_frame"/>
|
||||
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo camera sensor (Gazebo Harmonic / gz-sim)
|
||||
|
||||
Image data is published on gz topics and bridged to ROS 2 via
|
||||
ros_gz_bridge:
|
||||
/camera/image (gz) → /camera/image_raw (ROS 2)
|
||||
/camera/info (gz) → /camera/camera_info (ROS 2)
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo reference="camera_link">
|
||||
<sensor name="camera" type="camera">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>30</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<camera>
|
||||
<horizontal_fov>${camera_fov}</horizontal_fov>
|
||||
<image>
|
||||
<width>${camera_width}</width>
|
||||
<height>${camera_height}</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.05</near>
|
||||
<far>50.0</far>
|
||||
</clip>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.007</stddev>
|
||||
</noise>
|
||||
</camera>
|
||||
<!-- Publishes to gz topic /camera/image -->
|
||||
<topic>/camera/image</topic>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -1,77 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
RPLIDAR A1 — 2D laser scanner
|
||||
|
||||
frame_id "laser" matches what sllidar_ros2 publishes on /scan.
|
||||
Adjust xyz to the physical mounting position on the chassis.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<link name="laser">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="0.04" length="0.04"/>
|
||||
</geometry>
|
||||
<material name="light_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="0.04" length="0.04"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.17"/>
|
||||
<!-- Solid cylinder -->
|
||||
<inertia ixx="0.000087" ixy="0" ixz="0"
|
||||
iyy="0.000087" iyz="0"
|
||||
izz="0.000136"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="laser_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="laser"/>
|
||||
<!-- x/y/z: position of lidar centre relative to base_link centre.
|
||||
x negative = rear-mounted; adjust to your physical layout. -->
|
||||
<origin xyz="${lidar_x} ${lidar_y} ${lidar_z}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo gpu_lidar sensor (Gazebo Harmonic / gz-sim)
|
||||
|
||||
Scan data is published on gz topic /scan and bridged to ROS 2 via
|
||||
ros_gz_bridge:
|
||||
/scan (gz) → /scan (ROS 2, sensor_msgs/LaserScan)
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo reference="laser">
|
||||
<sensor name="lidar" type="gpu_lidar">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>10</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>/scan</topic>
|
||||
<gz_frame_id>laser</gz_frame_id>
|
||||
<lidar>
|
||||
<scan>
|
||||
<horizontal>
|
||||
<samples>360</samples>
|
||||
<resolution>1</resolution>
|
||||
<min_angle>-3.14159265</min_angle>
|
||||
<max_angle> 3.14159265</max_angle>
|
||||
</horizontal>
|
||||
</scan>
|
||||
<range>
|
||||
<min>${lidar_min_range}</min>
|
||||
<max>${lidar_max_range}</max>
|
||||
<resolution>0.01</resolution>
|
||||
</range>
|
||||
<noise>
|
||||
<type>gaussian</type>
|
||||
<mean>0.0</mean>
|
||||
<stddev>0.01</stddev>
|
||||
</noise>
|
||||
</lidar>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -1,134 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Camera pan / tilt mechanism
|
||||
|
||||
Joint names (pan, tilt) must match the names published by
|
||||
camera_orientation_node on /joint_states so that robot_state_publisher
|
||||
drives the TF tree correctly from live hardware.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<!-- Mount bracket — fixed to the top-front of the chassis -->
|
||||
<link name="camera_base">
|
||||
<visual>
|
||||
<geometry><box size="0.03 0.04 0.03"/></geometry>
|
||||
<material name="dark_grey"/>
|
||||
</visual>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="0.00001" ixy="0" ixz="0"
|
||||
iyy="0.00001" iyz="0" izz="0.00001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="camera_base_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="camera_base"/>
|
||||
<!-- Adjust xyz to match the physical servo mount position -->
|
||||
<origin xyz="${base_length/2 - 0.02} 0.0 ${base_height/2 + 0.015}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- Pan link — rotates around Z (yaw) -->
|
||||
<link name="camera_pan_link">
|
||||
<inertial>
|
||||
<mass value="0.01"/>
|
||||
<inertia ixx="0.000001" ixy="0" ixz="0"
|
||||
iyy="0.000001" iyz="0" izz="0.000001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="pan" type="revolute">
|
||||
<parent link="camera_base"/>
|
||||
<child link="camera_pan_link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.02" rpy="0 0 0"/>
|
||||
<!-- Physical range: 0°–180°, centred at 90° → ROS range: ±90° -->
|
||||
<limit lower="${pan_min_rad}" upper="${pan_max_rad}"
|
||||
effort="1.0" velocity="1.5"/>
|
||||
<dynamics damping="0.1" friction="0.05"/>
|
||||
</joint>
|
||||
|
||||
<!-- Tilt link — rotates around Y (pitch) -->
|
||||
<link name="camera_tilt_link">
|
||||
<inertial>
|
||||
<mass value="0.02"/>
|
||||
<inertia ixx="0.000002" ixy="0" ixz="0"
|
||||
iyy="0.000002" iyz="0" izz="0.000002"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tilt" type="revolute">
|
||||
<parent link="camera_pan_link"/>
|
||||
<child link="camera_tilt_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<origin xyz="0.02 0 0" rpy="0 0 0"/>
|
||||
<!-- Physical range: 0°–110°, centred at 60° → ROS range: -60° to +50° -->
|
||||
<limit lower="${tilt_min_rad}" upper="${tilt_max_rad}"
|
||||
effort="1.0" velocity="1.5"/>
|
||||
<dynamics damping="0.1" friction="0.05"/>
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Transmissions (for ros2_control)
|
||||
|
||||
These describe the mapping between joint commands and actuators.
|
||||
Required when using gz_ros2_control for Gazebo simulation.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<transmission name="pan_transmission">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="pan">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="pan_actuator">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="tilt_transmission">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="tilt">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="tilt_actuator">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
ros2_control block
|
||||
|
||||
For Gazebo simulation, set the hardware plugin to:
|
||||
gz_ros2_control/GazeboSimSystem
|
||||
For the real robot, replace with the custom I²C servo hardware plugin.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<ros2_control name="pan_tilt_servos" type="system">
|
||||
<hardware>
|
||||
<!-- Gazebo simulation: -->
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
<!-- Real robot (replace with actual plugin when available):
|
||||
<plugin>raspbot_v2_hardware/ServoHardware</plugin>
|
||||
-->
|
||||
</hardware>
|
||||
<joint name="pan">
|
||||
<command_interface name="position">
|
||||
<param name="min">${pan_min_rad}</param>
|
||||
<param name="max">${pan_max_rad}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="tilt">
|
||||
<command_interface name="position">
|
||||
<param name="min">${tilt_min_rad}</param>
|
||||
<param name="max">${tilt_max_rad}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
</robot>
|
||||
@@ -1,33 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||
gz_ros2_control — Gazebo Harmonic system plugin
|
||||
|
||||
Wires the <ros2_control> hardware block in pan_tilt.xacro to gz-sim
|
||||
so that the controller_manager can load and run controllers against
|
||||
simulated pan and tilt joints.
|
||||
|
||||
Lifecycle in simulation:
|
||||
1. gz-sim loads this plugin when the robot is spawned
|
||||
2. The plugin reads robot_description from robot_state_publisher
|
||||
3. controller_manager starts; it loads controllers.yaml
|
||||
4. Spawning joint_state_broadcaster + pan_tilt_controller (via launch)
|
||||
drives /joint_states and accepts trajectory commands
|
||||
═══════════════════════════════════════════════════════════════════════ -->
|
||||
|
||||
<gazebo>
|
||||
<plugin filename="gz_ros2_control-system"
|
||||
name="gz_ros2_control::GazeboSimROS2ControlPlugin">
|
||||
<robot_param>robot_description</robot_param>
|
||||
<robot_param_node>robot_state_publisher</robot_param_node>
|
||||
<!-- Path injected at xacro-processing time by the launch file:
|
||||
xacro ... controllers_config:=<path>/controllers.yaml
|
||||
Empty by default so the file can be previewed without a built workspace. -->
|
||||
<xacro:if value="${controllers_config != ''}">
|
||||
<parameters>${controllers_config}</parameters>
|
||||
</xacro:if>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -1,107 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="raspbot_v2">
|
||||
|
||||
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||
raspbot_v2.urdf.xacro — top-level composition file
|
||||
|
||||
All physical parameters are declared here as xacro:arg so they can
|
||||
be overridden at build time:
|
||||
xacro raspbot_v2.urdf.xacro wheel_separation:=0.14
|
||||
|
||||
Sub-files reference these properties directly; they are not intended
|
||||
to be included in isolation.
|
||||
═══════════════════════════════════════════════════════════════════════ -->
|
||||
|
||||
<!-- ── Chassis ────────────────────────────────────────────────────────── -->
|
||||
<xacro:arg name="base_length" default="0.20"/> <!-- m, front-to-rear -->
|
||||
<xacro:arg name="base_width" default="0.15"/> <!-- m, left-to-right -->
|
||||
<xacro:arg name="base_height" default="0.06"/> <!-- m -->
|
||||
<xacro:arg name="base_mass" default="1.0"/> <!-- kg -->
|
||||
|
||||
<!-- ── Wheels ─────────────────────────────────────────────────────────── -->
|
||||
<!-- wheel_separation: centre-to-centre distance between left and right wheels.
|
||||
Should match the wheel_base parameter in the motor controller node. -->
|
||||
<xacro:arg name="wheel_separation" default="0.14"/> <!-- m -->
|
||||
<xacro:arg name="wheel_radius" default="0.033"/> <!-- m -->
|
||||
<xacro:arg name="wheel_width" default="0.02"/> <!-- m -->
|
||||
<xacro:arg name="wheel_mass" default="0.1"/> <!-- kg -->
|
||||
<!-- Front-to-rear distance between wheel pairs (half-value used per side) -->
|
||||
<xacro:arg name="wheel_base_half" default="0.07"/> <!-- m -->
|
||||
|
||||
<!-- ── Pan / tilt limits (radians, ROS convention: 0 = centre) ────────── -->
|
||||
<!-- Pan: physical 0°–180°, centred at 90° → ±π/2 -->
|
||||
<!-- Tilt: physical 0°–110°, centred at 60° → −60° (−1.047) to +50° (+0.873) -->
|
||||
<xacro:arg name="pan_min_rad" default="${-pi/2}"/>
|
||||
<xacro:arg name="pan_max_rad" default="${ pi/2}"/>
|
||||
<xacro:arg name="tilt_min_rad" default="${-pi/3}"/> <!-- −60° -->
|
||||
<xacro:arg name="tilt_max_rad" default="${5*pi/18}"/> <!-- +50° -->
|
||||
|
||||
<!-- ── Camera ─────────────────────────────────────────────────────────── -->
|
||||
<xacro:arg name="camera_fov" default="1.047"/> <!-- rad (~60°) -->
|
||||
<xacro:arg name="camera_width" default="640"/>
|
||||
<xacro:arg name="camera_height" default="480"/>
|
||||
|
||||
<!-- ── Lidar ──────────────────────────────────────────────────────────── -->
|
||||
<!-- Mounting position relative to base_link centre -->
|
||||
<xacro:arg name="lidar_x" default="-0.03"/> <!-- m, slightly rearward -->
|
||||
<xacro:arg name="lidar_y" default="0.0"/>
|
||||
<xacro:arg name="lidar_z" default="0.09"/> <!-- m above base_link centre -->
|
||||
<xacro:arg name="lidar_min_range" default="0.15"/> <!-- m (RPLIDAR A1 spec) -->
|
||||
<xacro:arg name="lidar_max_range" default="12.0"/> <!-- m (RPLIDAR A1 spec) -->
|
||||
|
||||
<!-- ── Sonar ──────────────────────────────────────────────────────────── -->
|
||||
<xacro:arg name="sonar_min_range" default="0.02"/> <!-- m (HC-SR04 spec) -->
|
||||
<xacro:arg name="sonar_max_range" default="4.0"/> <!-- m (HC-SR04 spec) -->
|
||||
|
||||
<!-- ── ros2_control ────────────────────────────────────────────────────── -->
|
||||
<!-- Passed by the launch file as an absolute path to controllers.yaml.
|
||||
Empty by default so xacro can process the file without a built workspace. -->
|
||||
<xacro:arg name="controllers_config" default=""/>
|
||||
|
||||
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||
Resolve args → properties so sub-files can use ${name} syntax
|
||||
═══════════════════════════════════════════════════════════════════════ -->
|
||||
|
||||
<xacro:property name="base_length" value="$(arg base_length)"/>
|
||||
<xacro:property name="base_width" value="$(arg base_width)"/>
|
||||
<xacro:property name="base_height" value="$(arg base_height)"/>
|
||||
<xacro:property name="base_mass" value="$(arg base_mass)"/>
|
||||
|
||||
<xacro:property name="wheel_separation" value="$(arg wheel_separation)"/>
|
||||
<xacro:property name="wheel_radius" value="$(arg wheel_radius)"/>
|
||||
<xacro:property name="wheel_width" value="$(arg wheel_width)"/>
|
||||
<xacro:property name="wheel_mass" value="$(arg wheel_mass)"/>
|
||||
<xacro:property name="wheel_base_half" value="$(arg wheel_base_half)"/>
|
||||
|
||||
<xacro:property name="pan_min_rad" value="$(arg pan_min_rad)"/>
|
||||
<xacro:property name="pan_max_rad" value="$(arg pan_max_rad)"/>
|
||||
<xacro:property name="tilt_min_rad" value="$(arg tilt_min_rad)"/>
|
||||
<xacro:property name="tilt_max_rad" value="$(arg tilt_max_rad)"/>
|
||||
|
||||
<xacro:property name="camera_fov" value="$(arg camera_fov)"/>
|
||||
<xacro:property name="camera_width" value="$(arg camera_width)"/>
|
||||
<xacro:property name="camera_height" value="$(arg camera_height)"/>
|
||||
|
||||
<xacro:property name="lidar_x" value="$(arg lidar_x)"/>
|
||||
<xacro:property name="lidar_y" value="$(arg lidar_y)"/>
|
||||
<xacro:property name="lidar_z" value="$(arg lidar_z)"/>
|
||||
<xacro:property name="lidar_min_range" value="$(arg lidar_min_range)"/>
|
||||
<xacro:property name="lidar_max_range" value="$(arg lidar_max_range)"/>
|
||||
|
||||
<xacro:property name="sonar_min_range" value="$(arg sonar_min_range)"/>
|
||||
<xacro:property name="sonar_max_range" value="$(arg sonar_max_range)"/>
|
||||
|
||||
<xacro:property name="controllers_config" value="$(arg controllers_config)"/>
|
||||
|
||||
<!-- ═══════════════════════════════════════════════════════════════════════
|
||||
Sub-file includes (order matters — base must come first)
|
||||
═══════════════════════════════════════════════════════════════════════ -->
|
||||
|
||||
<xacro:include filename="robot_base.xacro"/>
|
||||
<xacro:include filename="pan_tilt.xacro"/>
|
||||
<xacro:include filename="camera.xacro"/>
|
||||
<xacro:include filename="lidar.xacro"/>
|
||||
<xacro:include filename="sonar.xacro"/>
|
||||
<xacro:include filename="raspbot_v2.ros2_control.xacro"/>
|
||||
|
||||
</robot>
|
||||
@@ -1,155 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Inertia helpers
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<xacro:macro name="box_inertia" params="m x y z">
|
||||
<inertial>
|
||||
<mass value="${m}"/>
|
||||
<inertia ixx="${m*(y*y+z*z)/12}" ixy="0" ixz="0"
|
||||
iyy="${m*(x*x+z*z)/12}" iyz="0"
|
||||
izz="${m*(x*x+y*y)/12}"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- Cylinder axis along Z -->
|
||||
<xacro:macro name="cylinder_inertia" params="m r h">
|
||||
<inertial>
|
||||
<mass value="${m}"/>
|
||||
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
|
||||
iyy="${m*(3*r*r+h*h)/12}" iyz="0"
|
||||
izz="${m*r*r/2}"/>
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Materials
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<material name="dark_grey"> <color rgba="0.2 0.2 0.2 1.0"/></material>
|
||||
<material name="light_grey"> <color rgba="0.7 0.7 0.7 1.0"/></material>
|
||||
<material name="blue"> <color rgba="0.0 0.4 0.8 1.0"/></material>
|
||||
<material name="black"> <color rgba="0.05 0.05 0.05 1.0"/></material>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Base chassis
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||
</geometry>
|
||||
<material name="dark_grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${base_length} ${base_width} ${base_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:box_inertia m="${base_mass}"
|
||||
x="${base_length}" y="${base_width}" z="${base_height}"/>
|
||||
</link>
|
||||
|
||||
<!-- base_footprint is the ground-projected origin; base_link is the chassis body -->
|
||||
<joint name="base_footprint_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Wheels (FL / FR / RL / RR)
|
||||
Cylinders rotated 90° so their axis lies along Y (roll axis).
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
|
||||
<xacro:macro name="wheel" params="name x_sign y_sign">
|
||||
<link name="wheel_${name}">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder radius="${wheel_radius}" length="${wheel_width}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
|
||||
</collision>
|
||||
<xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>
|
||||
</link>
|
||||
|
||||
<joint name="wheel_${name}_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="wheel_${name}"/>
|
||||
<!-- x_sign: +1=front, -1=rear | y_sign: +1=left, -1=right -->
|
||||
<origin xyz="${x_sign * wheel_base_half} ${y_sign * wheel_separation/2} ${-base_height/2 + wheel_radius}"
|
||||
rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- Friction and contact properties for Gazebo -->
|
||||
<gazebo reference="wheel_${name}">
|
||||
<mu1>1.0</mu1>
|
||||
<mu2>0.5</mu2>
|
||||
<kp>1e6</kp>
|
||||
<kd>1.0</kd>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:wheel name="front_left" x_sign=" 1" y_sign=" 1"/>
|
||||
<xacro:wheel name="front_right" x_sign=" 1" y_sign="-1"/>
|
||||
<xacro:wheel name="rear_left" x_sign="-1" y_sign=" 1"/>
|
||||
<xacro:wheel name="rear_right" x_sign="-1" y_sign="-1"/>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo — differential drive plugin
|
||||
Subscribes to /cmd_vel (gz topic), publishes /odom and the
|
||||
odom → base_footprint TF. Bridge gz topics to ROS 2 via ros_gz_bridge:
|
||||
/cmd_vel (ROS 2) → /cmd_vel (gz)
|
||||
/odom (gz) → /odom (ROS 2, nav_msgs/Odometry)
|
||||
/tf (gz) → /tf (ROS 2, tf2_msgs/TFMessage)
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo>
|
||||
<plugin filename="gz-sim-diff-drive-system"
|
||||
name="gz::sim::systems::DiffDrive">
|
||||
<left_joint>wheel_front_left_joint</left_joint>
|
||||
<left_joint>wheel_rear_left_joint</left_joint>
|
||||
<right_joint>wheel_front_right_joint</right_joint>
|
||||
<right_joint>wheel_rear_right_joint</right_joint>
|
||||
<wheel_separation>${wheel_separation}</wheel_separation>
|
||||
<wheel_radius>${wheel_radius}</wheel_radius>
|
||||
<odom_publish_frequency>10</odom_publish_frequency>
|
||||
<topic>/cmd_vel</topic>
|
||||
<odom_topic>/odom</odom_topic>
|
||||
<tf_topic>/tf</tf_topic>
|
||||
<frame_id>odom</frame_id>
|
||||
<child_frame_id>base_footprint</child_frame_id>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- ─────────────────────────────────────────────────────────────────────
|
||||
Gazebo — joint state publisher for wheel joints
|
||||
Publishes wheel positions on /gz/joint_states so robot_state_publisher
|
||||
can update the wheel TF links. Bridge to ROS 2 via ros_gz_bridge:
|
||||
/gz/joint_states (gz) → /gz/joint_states (ROS 2, sensor_msgs/JointState)
|
||||
Then merge with the ros2_control /joint_states in the launch file.
|
||||
───────────────────────────────────────────────────────────────────── -->
|
||||
<gazebo>
|
||||
<plugin filename="gz-sim-joint-state-publisher-system"
|
||||
name="gz::sim::systems::JointStatePublisher">
|
||||
<topic>/gz/joint_states</topic>
|
||||
<joint_name>wheel_front_left_joint</joint_name>
|
||||
<joint_name>wheel_front_right_joint</joint_name>
|
||||
<joint_name>wheel_rear_left_joint</joint_name>
|
||||
<joint_name>wheel_rear_right_joint</joint_name>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user