m5p3nc3r/ros-raspbot-v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Migrate to a new physical robot

Browse Source 
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:
Matt Spencer
2026-05-27 13:25:20 +00:00
parent c1319a6357
commit ec554bcf2c
96 changed files with 2874 additions and 3730 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.devcontainer/Dockerfile
+9 -52
View File
@@ -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
# ********************************************************
.devcontainer/devcontainer.json
+10 -12
View File
@@ -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=/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/"
}
ansible/setup_robot.yml
+33
View File
@@ -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
docker-compose.yml
+9 -2
View File
@@ -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:
raspbot_v2/Dockerfile
+83
View File
@@ -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"]
raspbot_v2/README.md
+46
View File
@@ -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
```
raspbot_v2/src/raspbot_v2_bringup/CMakeLists.txt
+31
View File
@@ -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()
robot/LICENSE → raspbot_v2/src/raspbot_v2_bringup/LICENSE
+6 -21
View File
@@ -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.
raspbot_v2/src/raspbot_v2_bringup/launch/raspbot_v2_bringup.launch.py
+80
View File
@@ -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,
])
raspbot_v2/src/raspbot_v2_bringup/package.xml
+18
View File
@@ -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>
raspbot_v2/src/raspbot_v2_control/CMakeLists.txt
+31
View File
@@ -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()
raspbot_v2/src/raspbot_v2_control/LICENSE
+17
View File
@@ -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.
raspbot_v2/src/raspbot_v2_control/config/controllers.yaml
+46
View File
@@ -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
raspbot_v2/src/raspbot_v2_control/config/sim_overrides.yaml
+3
View File
@@ -0,0 +1,3 @@
controller_manager:
ros__parameters:
use_sim_time: true
raspbot_v2/src/raspbot_v2_control/launch/controller_manager.launch.py
+119
View File
@@ -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,
])
raspbot_v2/src/raspbot_v2_control/package.xml
+18
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/CMakeLists.txt
+29
View File
@@ -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()
raspbot_v2/src/raspbot_v2_description/LICENSE
+17
View File
@@ -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.
raspbot_v2/src/raspbot_v2_description/launch/raspbot_v2_display.launch.py
+36
View File
@@ -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}],
),
])
robot/src/raspbot_v2/raspbot_v2/__init__.py → raspbot_v2/src/raspbot_v2_description/meshes/empty
View File
raspbot_v2/src/raspbot_v2_description/package.xml
+18
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/base.xacro
+176
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/camera.xacro
+87
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/cpu_housing.xacro
+42
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/lidar.xacro
+77
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/materials.xacro
+30
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/pan_tilt.xacro
+141
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2.urdf.xacro
+15
View File
@@ -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>
raspbot_v2/src/raspbot_v2_description/urdf/raspbot_v2_params.urdf.xacro
+122
View File
@@ -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>
robot/src/raspbot_v2/urdf/sonar.xacro → raspbot_v2/src/raspbot_v2_description/urdf/sonar.xacro
+40 -9
View File
@@ -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>
<!-- ─────────────────────────────────────────────────────────────────────
raspbot_v2/src/raspbot_v2_hardware/LICENSE
+17
View File
@@ -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.
raspbot_v2/src/raspbot_v2_hardware/launch/hardware.launch.py
+55
View File
@@ -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,
])
raspbot_v2/src/raspbot_v2_hardware/package.xml
+20
View File
@@ -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>
robot/src/raspbot_v2/resource/raspbot_v2 → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/__init__.py
View File
robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/camera_orientation_node.py
+1 -1
View File
@@ -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) ---
robot/src/raspbot_v2/raspbot_v2/led_node.py → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/led_node.py
+6 -2
View File
@@ -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)
robot/raspbot_v2_interface/motor_controller.py → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/motor_controller.py
+3 -3
View File
@@ -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]
# ------------------------------------------------------------------
robot/src/raspbot_v2/raspbot_v2/motor_controller_node.py → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/motor_controller_node.py
+2 -2
View File
@@ -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):
raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/py.typed
View File
raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/raspbot_lib.py
+307
View File
@@ -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
robot/src/raspbot_v2/raspbot_v2/ultrasonic_node.py → raspbot_v2/src/raspbot_v2_hardware/raspbot_v2_hardware/ultrasonic_node.py
+1 -1
View File
@@ -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 ---
raspbot_v2/src/raspbot_v2_hardware/resource/raspbot_v2_hardware
View File
raspbot_v2/src/raspbot_v2_hardware/setup.cfg
+4
View File
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/raspbot_v2_hardware
[install]
install_scripts=$base/lib/raspbot_v2_hardware
raspbot_v2/src/raspbot_v2_hardware/setup.py
+34
View File
@@ -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',
],
},
)
raspbot_v2/src/raspbot_v2_hardware/test/test_copyright.py
+25
View File
@@ -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'
raspbot_v2/src/raspbot_v2_hardware/test/test_flake8.py
+25
View File
@@ -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)
raspbot_v2/src/raspbot_v2_hardware/test/test_mypy.py
+23
View File
@@ -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!'
raspbot_v2/src/raspbot_v2_hardware/test/test_pep257.py
+23
View File
@@ -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'
raspbot_v2/src/raspbot_v2_hardware/test/test_xmllint.py
+23
View File
@@ -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'
raspbot_v2/src/raspbot_v2_hardware_interface/CMakeLists.txt
+54
View File
@@ -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()
raspbot_v2/src/raspbot_v2_hardware_interface/include/raspbot_v2_hardware_interface/raspbot_hardware_interface.hpp
+74
View File
@@ -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
raspbot_v2/src/raspbot_v2_hardware_interface/package.xml
+19
View File
@@ -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>
raspbot_v2/src/raspbot_v2_hardware_interface/raspbot_v2_hardware_interface_plugin.xml
+12
View File
@@ -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>
raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp
+166
View File
@@ -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)
raspbot_v2/src/raspbot_v2_sim/CMakeLists.txt
+32
View File
@@ -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()
raspbot_v2/src/raspbot_v2_sim/LICENSE
+17
View File
@@ -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.
raspbot_v2/src/raspbot_v2_sim/launch/gazebo.launch.py
+103
View File
@@ -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,
])
raspbot_v2/src/raspbot_v2_sim/launch/rviz2.launch.py
+49
View File
@@ -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',
)
])
raspbot_v2/src/raspbot_v2_sim/package.xml
+18
View File
@@ -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>
raspbot_v2/src/raspbot_v2_sim/rviz/raspbot_v2.rviz
+275
View File
@@ -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
robot/src/raspbot_v2/worlds/empty_world.sdf → raspbot_v2/src/raspbot_v2_sim/worlds/empty_world
View File
raspbot_v2/src/raspbot_v2_sim/worlds/warehouse_world
+89
View File
@@ -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>
robot/Dockerfile
-50
View File
@@ -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"]
robot/README.md
-302
View File
@@ -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)
```
robot/raspbot_v2_interface/README.md
-5
View File
@@ -1,5 +0,0 @@
## Installation Steps (安装步骤)
cd py_install
sudo python3 setup.py install
robot/raspbot_v2_interface/Raspbot_Lib/.ipynb_checkpoints/Raspbot_Lib-checkpoint.py
-480
View File
@@ -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)
robot/raspbot_v2_interface/Raspbot_Lib/NOTICE
-11
View File
@@ -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.
robot/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py
-495
View File
@@ -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)
robot/raspbot_v2_interface/Raspbot_Lib/__init__.py
-1
View File
@@ -1 +0,0 @@
from .Raspbot_Lib import Raspbot,LightShow
robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock.cpython-311.pyc
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspblock_Lib.cpython-311.pyc
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/Raspbot_Lib.cpython-311.pyc
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/Raspbot_Lib/__pycache__/__init__.cpython-311.pyc
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/__init__.py
-3
View File
@@ -1,3 +0,0 @@
from .motor_controller import MotorController
__all__ = ["MotorController"]
robot/raspbot_v2_interface/build/lib/Raspbot_Lib/Raspbot_Lib.py
-480
View File
@@ -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)
robot/raspbot_v2_interface/build/lib/Raspbot_Lib/__init__.py
-1
View File
@@ -1 +0,0 @@
from .Raspbot_Lib import Raspbot,LightShow
robot/raspbot_v2_interface/dist/Raspblock_Lib-0.0.2-py3.11.egg
Vendored
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/dist/Raspbot_Lib-0.0.2-py3.11.egg
Vendored
BIN
View File
Binary file not shown.
robot/raspbot_v2_interface/setup.py
-11
View File
@@ -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'
)
robot/src/raspbot_v2/config/controllers.yaml
-55
View File
@@ -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
robot/src/raspbot_v2/config/nav2_params.yaml
-314
View File
@@ -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
robot/src/raspbot_v2/config/rviz2_config.rviz
-274
View File
@@ -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
robot/src/raspbot_v2/config/slam_toolbox.yaml
-64
View File
@@ -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
robot/src/raspbot_v2/launch/house_sim_launch.py
-42
View File
@@ -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])
robot/src/raspbot_v2/launch/robot.launch.py
-116
View File
@@ -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',
),
])
robot/src/raspbot_v2/launch/sim_launch.py
-259
View File
@@ -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,
])
robot/src/raspbot_v2/package.xml
-24
View File
@@ -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>
robot/src/raspbot_v2/setup.cfg
-13
View File
@@ -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
robot/src/raspbot_v2/setup.py
-29
View File
@@ -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',
],
},
)
robot/src/raspbot_v2/urdf/camera.xacro
-83
View File
@@ -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>
robot/src/raspbot_v2/urdf/lidar.xacro
-77
View File
@@ -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>
robot/src/raspbot_v2/urdf/pan_tilt.xacro
-134
View File
@@ -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>
robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro
-33
View File
@@ -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>
robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
-107
View File
@@ -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>
robot/src/raspbot_v2/urdf/robot_base.xacro
-155
View File
@@ -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>
webui/backend/ros_node.py
+6 -5
View File
@@ -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: