ROS 2 Turtlesim Implementation of cmd_vel publisher

[DaynaG3]

Hello! I am currently facing some issues with implementing the ROSA agent to the turtlesim using ROS 2 Humble on Ubuntu 22.04, the agent is unable to publish twist messages to the /cmd_vel topic to move the turtle. It is only able to move the turtle using the teleport service call.

This is my current modified turtle.py file, any help or code examples I can refer to would be really helpful. Thank you!

from math import cos, sin, sqrt
from typing import List

import rclpy
from rclpy.publisher import Publisher
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty
from turtlesim.msg import Pose
from turtlesim.srv import Spawn, TeleportAbsolute, TeleportRelative, Kill, SetPen
from rclpy.node import Node

cmd_vel_pubs = {}

def add_cmd_vel_pub(name: str, publisher: Publisher):
    global cmd_vel_pubs
    cmd_vel_pubs[name] = publisher

def remove_cmd_vel_pub(name: str):
    global cmd_vel_pubs
    cmd_vel_pubs.pop(name, None)

# Add the default turtle1 publisher on startup
def init_node():
    rclpy.init()
    node = Node('turtle_agent')
    add_cmd_vel_pub("turtle1", node.create_publisher(Twist, '/turtle1/cmd_vel', 10))
    return node

def within_bounds(x: float, y: float) -> tuple:
    if 0 <= x <= 11 and 0 <= y <= 11:
        return True, "Coordinates are within bounds."
    else:
        return False, f"({x}, {y}) will be out of bounds. Range is [0, 11] for each."

def will_be_within_bounds(
    node: Node, name: str, velocity: float, lateral: float, angle: float, duration: float = 1.0
) -> tuple:
    pose = get_turtle_pose(node, [name])
    current_x = pose[name].x
    current_y = pose[name].y
    current_theta = pose[name].theta

    if abs(angle) < 1e-6:
        new_x = (
            current_x
            + (velocity * cos(current_theta) - lateral * sin(current_theta)) * duration
        )
        new_y = (
            current_y
            + (velocity * sin(current_theta) + lateral * cos(current_theta)) * duration
        )
    else:
        radius = sqrt(velocity**2 + lateral**2) / abs(angle)
        center_x = current_x - radius * sin(current_theta)
        center_y = current_y + radius * cos(current_theta)
        angle_traveled = angle * duration
        new_x = center_x + radius * sin(current_theta + angle_traveled)
        new_y = center_y - radius * cos(current_theta + angle_traveled)

        for t in range(int(duration) + 1):
            angle_t = current_theta + angle * t
            x_t = center_x + radius * sin(angle_t)
            y_t = center_y - radius * cos(angle_t)
            in_bounds, _ = within_bounds(x_t, y_t)
            if not in_bounds:
                return (
                    False,
                    f"The circular path will go out of bounds at ({x_t:.2f}, {y_t:.2f}).",
                )

    in_bounds, message = within_bounds(new_x, new_y)
    if not in_bounds:
        return (
            False,
            f"This command will move the turtle out of bounds to ({new_x:.2f}, {new_y:.2f}).",
        )

    return True, f"The turtle will remain within bounds at ({new_x:.2f}, {new_y:.2f})."

def spawn_turtle(node: Node, name: str, x: float, y: float, theta: float) -> str:
    in_bounds, message = within_bounds(x, y)
    if not in_bounds:
        return message

    name = name.replace("/", "")

    try:
        spawn = node.create_client(Spawn, '/spawn')
        spawn.wait_for_service(timeout_sec=5)
        spawn_req = Spawn.Request()
        spawn_req.x = x
        spawn_req.y = y
        spawn_req.theta = theta
        spawn_req.name = name
        spawn.call_async(spawn_req)

        global cmd_vel_pubs
        cmd_vel_pubs[name] = node.create_publisher(Twist, f'/{name}/cmd_vel', 10)

        return f"{name} spawned at x: {x}, y: {y}, theta: {theta}."
    except Exception as e:
        return f"Failed to spawn {name}: {e}"

def kill_turtle(node: Node, names: List[str]):
    names = [name.replace("/", "") for name in names]
    response = ""
    global cmd_vel_pubs

    for name in names:
        try:
            kill = node.create_client(Kill, f'/{name}/kill')
            kill.wait_for_service(timeout_sec=5)
            kill.call_async(Kill.Request())

            cmd_vel_pubs.pop(name, None)

            response += f"Successfully killed {name}.\n"
        except rclpy.exceptions.ROSInterruptException as e:
            response += f"Failed to kill {name}: {e}\n"

    return response

def clear_turtlesim(node: Node):
    try:
        clear = node.create_client(Empty, '/clear')
        clear.wait_for_service(timeout_sec=5)
        clear.call_async(Empty.Request())
        return "Successfully cleared the turtlesim background."
    except Exception as e:
        return f"Failed to clear the turtlesim background: {e}"

def get_turtle_pose(node: Node, names: List[str]) -> dict:
    names = [name.replace("/", "") for name in names]
    poses = {}

    for name in names:
        try:
            msg = node.create_subscription(Pose, f'/{name}/pose', lambda msg: msg, 10)
            poses[name] = msg
        except rclpy.exceptions.ROSInterruptException:
            return {"Error": f"Failed to get pose for {name}: /{name}/pose not available."}

    return poses

def teleport_absolute(
    node: Node, name: str, x: float, y: float, theta: float, hide_pen: bool = True
):
    in_bounds, message = within_bounds(x, y)
    if not in_bounds:
        return message

    try:
        teleport = node.create_client(TeleportAbsolute, f'/{name}/teleport_absolute')
        teleport.wait_for_service(timeout_sec=5)

        teleport_req = TeleportAbsolute.Request()
        teleport_req.x = x
        teleport_req.y = y
        teleport_req.theta = theta
        teleport.call_async(teleport_req)

        return f"{name} new pose: ({x}, {y}) at {theta} radians."
    except Exception as e:
        return f"Failed to teleport the turtle: {e}"

def teleport_relative(self, name: str, linear: float, angular: float):
        """
        Teleport a turtle relative to its current position.

        :param name: name of the turtle
        :param linear: linear distance
        :param angular: angular distance
        """
        client = self.create_client(TeleportRelative, f'/{name}/teleport_relative')
        
        # Wait for the service to be available
        if not client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error(f"Service /{name}/teleport_relative not available.")
            return f"Failed to teleport the {name}: service not available."
        
        # Create the request
        request = TeleportRelative.Request()
        request.linear = linear
        request.angular = angular

def publish_twist_to_cmd_vel(
    node: Node, name: str, velocity: float, lateral: float, angle: float, steps: int = 1
):
    name = name.replace("/", "")

    if name not in cmd_vel_pubs:
            return f"Publisher for {name} not found. Call create_cmd_vel_publisher() first."
    
    in_bounds, message = will_be_within_bounds(node, name, velocity, lateral, angle, duration=steps)
    if not in_bounds:
        return message

    #turtlesim is a 2D environment
    vel = Twist()
    vel.linear.x = velocity
    vel.linear.y = lateral
    vel.linear.z = 0.0
    vel.angular.x = 0.0
    vel.angular.y = 0.0
    vel.angular.z = angle

    try:
        pub = cmd_vel_pubs[name]
        for _ in range(steps):
            pub.publish(vel)
            node.get_clock().sleep(1)  # 1 second interval
    except Exception as e:
        return f"Failed to publish {vel} to /{name}/cmd_vel: {e}"
    finally:
        current_pose = get_turtle_pose(node, [name])
        return (
            f"New Pose ({name}): x={current_pose[name].x}, y={current_pose[name].y}, "
            f"theta={current_pose[name].theta} rads."
        )

def stop_turtle(node: Node, name: str):
    return publish_twist_to_cmd_vel(
        node,
        name,
        0.0,
        0.0,
        0.0,
    )

def reset_turtlesim(node: Node):
    try:
        reset = node.create_client(Empty, '/reset')
        reset.wait_for_service(timeout_sec=5)
        reset.call_async(Empty.Request())
        cmd_vel_pubs.clear()
        cmd_vel_pubs["turtle1"] = node.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        return "Successfully reset the turtlesim environment."
    except Exception as e:
        return f"Failed to reset the turtlesim environment: {e}"

def set_pen(node: Node, name: str, r: int, g: int, b: int, width: int, off: int):
    name = name.replace("/", "")
    try:
        set_pen = node.create_client(SetPen, f'/{name}/set_pen')
        set_pen.wait_for_service(timeout_sec=5)
        set_pen_req = SetPen.Request()
        set_pen_req.r = r
        set_pen_req.g = g
        set_pen_req.b = b
        set_pen_req.width = width
        set_pen_req.off = off
        set_pen.call_async(set_pen_req)
        return f"Successfully set the pen color for {name}."
    except Exception as e:
        return f"Failed to set the pen color for {name}: {e}"`

[RobRoyce]

Very interesting. Do you get any error messages when trying to publish? Is anything being published to /cmd_vel or /turtle1/cmd_vel?

Where is your init_node function being called?

[DaynaG3]

I would get this response from the ROSA agent when trying to prompt it further to publish the twist message directly to /turtle1/cmd_vel

When echoing the /turtle1/cmd_vel topic there are no messages.

The init_node function is in the turtle_agent.py

class TurtleAgent(Node, ROSA):

    def __init__(self):
        super().__init__('turtle_agent')

        self.node = init_node()  # Initialize the Turtle node

Thank you for your reply and I hope to hear from you! :)