diff --git a/.gitignore b/.gitignore index 7b853979..3cc44ee4 100644 --- a/.gitignore +++ b/.gitignore @@ -60,3 +60,7 @@ build-*/ *.gch /.project .DS_Store +.vscode/ +log/ +build/ +install/ diff --git a/README.md b/README.md index dbdc612a..49fb522a 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # TurtleBot3 - + + +This is my Turtlebot, using a Raspberry Pi 5 and with an added camera, also equipped with an OLED display that shows on boot. - Active Branches: humble, jazzy, main(rolling) - Legacy Branches: *-devel, noetic diff --git a/media/20260206_132256.jpg b/media/20260206_132256.jpg new file mode 100644 index 00000000..63cb0581 Binary files /dev/null and b/media/20260206_132256.jpg differ diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index 0bbcee1f..c4629309 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -121,4 +121,11 @@ def generate_launch_description(): {'namespace': namespace}], arguments=['-i', usb_port], output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(get_package_share_directory('turtlebot3_utils'), 'launch'), + '/oled_display.launch.py' + ]) + ), ]) diff --git a/turtlebot3_bringup/launch/view_model.launch.py b/turtlebot3_bringup/launch/view_model.launch.py new file mode 100644 index 00000000..d8a9bd04 --- /dev/null +++ b/turtlebot3_bringup/launch/view_model.launch.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import Command +from launch_ros.actions import Node + +def generate_launch_description(): + + TURTLEBOT3_MODEL = os.environ.get('TURTLEBOT3_MODEL', 'burger') + + urdf_file = os.path.join( + get_package_share_directory('turtlebot3_description'), + 'urdf', + f'turtlebot3_{TURTLEBOT3_MODEL}.urdf' + ) + + rviz_config_file = os.path.join( + get_package_share_directory('turtlebot3_description'), + 'rviz', + 'model.rviz' + ) + + return LaunchDescription([ + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': Command(['xacro ', urdf_file]) + }] + ), + + Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + output='screen' + ), + + Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file], + output='screen' + ), + ]) \ No newline at end of file diff --git a/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_server.py b/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_server.py index 1ded3034..8ababa4e 100644 --- a/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_server.py +++ b/turtlebot3_example/turtlebot3_example/turtlebot3_patrol/turtlebot3_patrol_server.py @@ -20,8 +20,7 @@ import threading import time -from geometry_msgs.msg import Point -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, TwistStamped from nav_msgs.msg import Odometry import rclpy from rclpy.action import ActionServer @@ -50,25 +49,22 @@ def __init__(self): goal_callback=self.goal_callback) self.goal_msg = Patrol.Goal() - self.twist = Twist() + self.twist = TwistStamped() self.odom = Odometry() - self.position = Point() - self.rotation = 0.0 - self.linear_x = 1.0 - self.angular_z = 4.0 + self.linear_x = 0.2 + self.angular_z = 1.5 qos = QoSProfile(depth=10) - self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos) + self.cmd_vel_pub = self.create_publisher(TwistStamped, 'cmd_vel', qos) self.odom_sub = self.create_subscription( - Odometry, 'odom', self.odom_callback, qos - ) + Odometry, 'odom', self.odom_callback, qos) def init_twist(self): - self.twist.linear.x = 0.0 - self.twist.angular.z = 0.0 + self.twist.twist.linear.x = 0.0 + self.twist.twist.angular.z = 0.0 self.cmd_vel_pub.publish(self.twist) def odom_callback(self, msg): @@ -80,45 +76,85 @@ def get_yaw(self): cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) return math.atan2(siny, cosy) - def go_front(self, position, length): + def go_front(self, length): + start_x = self.odom.pose.pose.position.x + start_y = self.odom.pose.pose.position.y + target_heading = self.get_yaw() # hold this heading + while True: - position += self.twist.linear.x - if position >= length: + current_x = self.odom.pose.pose.position.x + current_y = self.odom.pose.pose.position.y + dist = math.sqrt( + (current_x - start_x) ** 2 + + (current_y - start_y) ** 2 + ) + + if dist >= length: break - self.twist.linear.x = self.linear_x - self.twist.angular.z = 0.0 + + # Small correction to hold heading + heading_error = math.atan2( + math.sin(target_heading - self.get_yaw()), + math.cos(target_heading - self.get_yaw()) + ) + + self.twist.twist.linear.x = self.linear_x + self.twist.twist.angular.z = heading_error * 0.5 # proportional correction self.cmd_vel_pub.publish(self.twist) + time.sleep(0.05) - time.sleep(1) self.init_twist() + time.sleep(0.3) def turn(self, target_angle): initial_yaw = self.get_yaw() target_yaw = initial_yaw + (target_angle * math.pi / 180.0) + # Normalize target_yaw to [-pi, pi] + target_yaw = math.atan2(math.sin(target_yaw), math.cos(target_yaw)) + + self.get_logger().info( + f'Turn start — initial_yaw: {math.degrees(initial_yaw):.1f}° ' + f'target_yaw: {math.degrees(target_yaw):.1f}°' + ) + + loop_count = 0 while True: - rclpy.spin_once(self, timeout_sec=0.1) + time.sleep(0.1) + loop_count += 1 current_yaw = self.get_yaw() - yaw_diff = abs( - math.atan2( - math.sin(target_yaw - current_yaw), - math.cos(target_yaw - current_yaw) - ) + + # Signed error — tells us direction AND magnitude + error = math.atan2( + math.sin(target_yaw - current_yaw), + math.cos(target_yaw - current_yaw) + ) + + self.get_logger().info( + f' loop {loop_count}: current_yaw={math.degrees(current_yaw):.1f}° ' + f'error={math.degrees(error):.1f}°' ) - if yaw_diff < 0.01: + if abs(error) < 0.05: + self.get_logger().info(f' Turn complete after {loop_count} loops') + break + + if loop_count > 200: + self.get_logger().warn('Turn timeout — forcing exit') break - self.twist.linear.x = 0.0 - self.twist.angular.z = self.angular_z + # Proportional speed, direction controlled by sign of error + speed = max(0.5, min(1.0, abs(error) * 1.2)) + self.twist.twist.linear.x = 0.0 + self.twist.twist.angular.z = math.copysign(speed, error) self.cmd_vel_pub.publish(self.twist) self.init_twist() + time.sleep(0.3) def goal_callback(self, goal_request): self.goal_msg = goal_request - return GoalResponse.ACCEPT def execute_callback(self, goal_handle): @@ -128,17 +164,14 @@ def execute_callback(self, goal_handle): length = self.goal_msg.goal.y iteration = int(self.goal_msg.goal.z) - while True: - if self.goal_msg.goal.x == 1: - for count in range(iteration): - self.square(feedback_msg, goal_handle, length) - feedback_msg.state = 'square patrol complete!!' - break - elif self.goal_msg.goal.x == 2: - for count in range(iteration): - self.triangle(feedback_msg, goal_handle, length) - feedback_msg.state = 'triangle patrol complete!!' - break + if self.goal_msg.goal.x == 1: + for count in range(iteration): + self.square(feedback_msg, goal_handle, length) + feedback_msg.state = 'square patrol complete!!' + elif self.goal_msg.goal.x == 2: + for count in range(iteration): + self.triangle(feedback_msg, goal_handle, length) + feedback_msg.state = 'triangle patrol complete!!' goal_handle.succeed() result = Patrol.Result() @@ -152,37 +185,24 @@ def execute_callback(self, goal_handle): def square(self, feedback_msg, goal_handle, length): self.linear_x = 0.2 - self.angular_z = 13 * (90.0 / 180.0) * math.pi / 100.0 for i in range(4): - self.position.x = 0.0 - self.angle = 0.0 - - self.go_front(self.position.x, length) + self.go_front(length) self.turn(90.0) feedback_msg.state = 'line ' + str(i + 1) goal_handle.publish_feedback(feedback_msg) - time.sleep(0.1) - - self.init_twist() def triangle(self, feedback_msg, goal_handle, length): self.linear_x = 0.2 - self.angular_z = 8 * (120.0 / 180.0) * math.pi / 100.0 + self.angular_z = 1.5 for i in range(3): - self.position.x = 0.0 - self.angle = 0.0 - - self.go_front(self.position.x, length) + self.go_front(length) self.turn(120.0) feedback_msg.state = 'line ' + str(i + 1) goal_handle.publish_feedback(feedback_msg) - time.sleep(1) - - self.init_twist() def main(args=None): @@ -190,8 +210,10 @@ def main(args=None): turtlebot3_patrol_server = Turtlebot3PatrolServer() - rclpy.spin(turtlebot3_patrol_server) + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(turtlebot3_patrol_server) + executor.spin() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/turtlebot3_teleop/config/joy_teleop.yaml b/turtlebot3_teleop/config/joy_teleop.yaml new file mode 100644 index 00000000..7c955700 --- /dev/null +++ b/turtlebot3_teleop/config/joy_teleop.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + axis_linear: + x: 1 + axis_angular: + yaw: 3 + publish_stamped_twist: true + scale_linear: + x: 0.5 + scale_angular: + yaw: 0.5 \ No newline at end of file diff --git a/turtlebot3_teleop/launch/joystick_teleop.launch.py b/turtlebot3_teleop/launch/joystick_teleop.launch.py new file mode 100644 index 00000000..56a46867 --- /dev/null +++ b/turtlebot3_teleop/launch/joystick_teleop.launch.py @@ -0,0 +1,45 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # Include the robot bringup launch file + robot_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('turtlebot3_bringup'), + 'launch', + 'robot.launch.py' + ]) + ]) + ) + + # Joy node to read joystick + joy_node = Node( + package='joy', + executable='joy_node', + name='joy_node' + ) + + # Teleop node to convert joy to cmd_vel + teleop_node = Node( + package='teleop_twist_joy', + executable='teleop_node', + name='teleop_twist_joy_node', + parameters=[PathJoinSubstitution([ + FindPackageShare('turtlebot3_teleop'), + 'config', + 'joy_teleop.yaml' + ])] + ) + + return LaunchDescription([ + robot_bringup, + joy_node, + teleop_node + ]) \ No newline at end of file diff --git a/turtlebot3_teleop/setup.py b/turtlebot3_teleop/setup.py index b1ca4f3f..c4bb48d8 100644 --- a/turtlebot3_teleop/setup.py +++ b/turtlebot3_teleop/setup.py @@ -1,5 +1,7 @@ from setuptools import find_packages from setuptools import setup +import os +from glob import glob package_name = 'turtlebot3_teleop' @@ -10,6 +12,8 @@ data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), ], install_requires=[ 'setuptools', @@ -36,4 +40,4 @@ 'teleop_keyboard = turtlebot3_teleop.script.teleop_keyboard:main' ], }, -) +) \ No newline at end of file diff --git a/turtlebot3_utils/launch/oled_display.launch.py b/turtlebot3_utils/launch/oled_display.launch.py new file mode 100644 index 00000000..9f80bff7 --- /dev/null +++ b/turtlebot3_utils/launch/oled_display.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + oled_display_node = Node( + package='turtlebot3_utils', + executable='oled_display_node', + name='oled_display_node', + output='screen' + ) + + return LaunchDescription([ + oled_display_node + ]) \ No newline at end of file diff --git a/turtlebot3_utils/package.xml b/turtlebot3_utils/package.xml new file mode 100644 index 00000000..08ebc719 --- /dev/null +++ b/turtlebot3_utils/package.xml @@ -0,0 +1,23 @@ + + + + turtlebot3_utils + 0.0.0 + TODO: Package description + don + TODO: License declaration + + rclpy + std_msgs + sensor_msgs + nav_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/turtlebot3_utils/resource/turtlebot3_utils b/turtlebot3_utils/resource/turtlebot3_utils new file mode 100644 index 00000000..e69de29b diff --git a/turtlebot3_utils/setup.cfg b/turtlebot3_utils/setup.cfg new file mode 100644 index 00000000..556ce2f1 --- /dev/null +++ b/turtlebot3_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/turtlebot3_utils +[install] +install_scripts=$base/lib/turtlebot3_utils diff --git a/turtlebot3_utils/setup.py b/turtlebot3_utils/setup.py new file mode 100644 index 00000000..dc372799 --- /dev/null +++ b/turtlebot3_utils/setup.py @@ -0,0 +1,33 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = 'turtlebot3_utils' + +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']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='don', + maintainer_email='dwilliestyle@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'oled_display_node = turtlebot3_utils.oled_display_node:main', + ], + }, +) diff --git a/turtlebot3_utils/test/test_copyright.py b/turtlebot3_utils/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/turtlebot3_utils/test/test_copyright.py @@ -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(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/turtlebot3_utils/test/test_flake8.py b/turtlebot3_utils/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/turtlebot3_utils/test/test_flake8.py @@ -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(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/turtlebot3_utils/test/test_pep257.py b/turtlebot3_utils/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/turtlebot3_utils/test/test_pep257.py @@ -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(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/turtlebot3_utils/turtlebot3_utils/__init__.py b/turtlebot3_utils/turtlebot3_utils/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/turtlebot3_utils/turtlebot3_utils/oled_display_node.py b/turtlebot3_utils/turtlebot3_utils/oled_display_node.py new file mode 100644 index 00000000..13beb8ec --- /dev/null +++ b/turtlebot3_utils/turtlebot3_utils/oled_display_node.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import BatteryState +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Twist +import board +import busio +from PIL import Image, ImageDraw, ImageFont +import adafruit_ssd1306 +import math + +class OLEDDisplayNode(Node): + def __init__(self): + super().__init__('oled_display_node') + + # Initialize I2C and OLED + i2c = busio.I2C(board.SCL, board.SDA) + self.oled = adafruit_ssd1306.SSD1306_I2C(128, 64, i2c, addr=0x3c) + + # Clear display + self.oled.fill(0) + self.oled.show() + + # Create image for drawing + self.image = Image.new("1", (128, 64)) + self.draw = ImageDraw.Draw(self.image) + + # Data storage + self.battery_voltage = 0.0 + self.battery_percentage = 0.0 + self.linear_vel = 0.0 + self.angular_vel = 0.0 + self.cmd_linear = 0.0 + self.cmd_angular = 0.0 + + # Subscribers + self.create_subscription(BatteryState, '/battery_state', self.battery_callback, 10) + self.create_subscription(Odometry, '/odom', self.odom_callback, 10) + self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 10) + + # Update timer (5 Hz) + self.timer = self.create_timer(0.2, self.update_display) + + self.get_logger().info('OLED Display Node started') + + def battery_callback(self, msg): + self.battery_voltage = msg.voltage + self.battery_percentage = msg.percentage + + def odom_callback(self, msg): + self.linear_vel = msg.twist.twist.linear.x + self.angular_vel = msg.twist.twist.angular.z + + def cmd_vel_callback(self, msg): + self.cmd_linear = msg.linear.x + self.cmd_angular = msg.angular.z + + def update_display(self): + # Clear image + self.draw.rectangle((0, 0, 128, 64), outline=0, fill=0) + + # Draw text + self.draw.text((0, 0), "Don's TurtleBot3", fill=255) + self.draw.text((0, 12), f"Batt: {self.battery_voltage:.2f}V ({self.battery_percentage:.0f}%)", fill=255) + self.draw.text((0, 24), f"Vel: {self.linear_vel:.2f} m/s", fill=255) + self.draw.text((0, 36), f"Ang: {self.angular_vel:.2f} r/s", fill=255) + self.draw.text((0, 48), f"Cmd: {self.cmd_linear:.2f} m/s", fill=255) + + # Display image + self.oled.image(self.image) + self.oled.show() + +def main(args=None): + rclpy.init(args=args) + node = OLEDDisplayNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file