Modified Twist imports to work with Jazzy. Also added Joy Teleop

.gitignore

@@ -60,3 +60,7 @@ build-*/
 *.gch
 /.project
 .DS_Store
+.vscode/
+log/
+build/
+install/

README.md

@@ -1,5 +1,7 @@
 # TurtleBot3
-<img src="https://raw.githubusercontent.com/ROBOTIS-GIT/emanual/master/assets/images/platform/turtlebot3/logo_turtlebot3.png" width="300">
+<img src="media/20260206_132256.jpg" width="650">
+
+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

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'
+            ])
+        ),
     ])

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'
+        ),
+    ])

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,46 +185,35 @@ 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):
     rclpy.init(args=args)
 
     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()

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

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
+    ])

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'
         ],
     },
-)
+)