2
$\begingroup$

So I have a task of making turtle sim draw a heart using parametric equations I don't know if all my equations are correct but here's my code

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
class TurtlecontrollerNode(Node):
    def __init__(self):
        super().__init__("turtle_controller")
        self.cmd_vel_puplisher_ = self.create_publisher(Twist,"/turtle1/cmd_vel",10)
        self.pose_subscriber_=self.create_subscription(Pose,"/turtle1/pose",self.pose_callback,10)
        self.get_logger().info("Turtle controller has been started.")
    def pose_callback(self,pose: Pose):
        #self.get_logger().info("("+str(pose.x)+","+str(pose.y)+")")
        rate = self.create_rate(10)
        x = lambda t: 16 * math.sin(t)**3 / 4 # x(t) = 16sin^3(t) / 4
        y = lambda t: 13 * math.cos(t) - 5 * math.cos(2*t) - 2 * math.cos(3*t) - math.cos(4*t) / 4 # y(t) = (13cos(t) - 5cos(2t) - 2cos(3t) - cos(4t)) / 4
        t_min = 0 # t from 0 to 2pi radians
        t_max = 2 * math.pi
        
        for t in range(int(t_min * 10), int(t_max * 10 + 1), 1):
          # Convert t from tenths of radians to radians
          t = t / 10.0

          # Calculate the linear and angular velocities for the turtle based on x and y values
          linear_velocity = math.sqrt((x(t + 0.1) - x(t))**2 + (y(t + 0.1) - y(t))**2) * 10 # linear velocity = distance / time, where distance = sqrt((x(t+0.1) - x(t))^2 + (y(t+0.1) - y(t))^2) and time = 0.1
          angular_velocity = math.atan2(y(t + 0.1) - y(t), x(t + 0.1) - x(t)) - math.atan2(y(t), x(t)) # angular velocity = angle / time, where angle = atan2(y(t+0.1) - y(t), x(t+0.1) - x(t)) - atan2(y(t), x(t)) and time = 0.1

          # Create a Twist message with the calculated velocities
          twist = Twist()
          twist.linear.x = linear_velocity
          twist.angular.z = angular_velocity

          # Publish the Twist message to the topic '/turtle1/cmd_vel'

          # Sleep for 0.1 seconds
          rate.sleep()
          self.cmd_vel_puplisher_.publish(twist)
          self.get_logger().info("("+str(pose.x)+","+str(pose.y)+")")
            

def main (args=None):
    rclpy.init(args=args)
    node = TurtlecontrollerNode()
    rclpy.spin(node)


    rclpy.shutdown()


my main problem is that the node gets stuck in waiting when it reaches rate.sleep() so it updates twist once, I know this because when I run the node it gives me no updates on the position from this line of code self.get_logger().info("("+str(pose.x)+","+str(pose.y)+")") and when I put that line above the rate.sleep() it updates once giving me the midpoint of the grid of 5.4 and 5.4 how to fix this I have been stuck on it for a while now.
I have tried using a timer instead but it doesn't work still and removing the rate.sleep line just makes it run through the for loop very fast and just doesn't update the velocities

$\endgroup$

1 Answer 1

2
$\begingroup$

Your approach of using timers and callbacks this way is slightly odd, for several reasons:

  • you're re-creating a timer each time you get a pose (which, under the hood, makes use of the callback system, so you're recreating a callback every time)
  • you are publishing many many times in response to one subscription (generally a subscriber only really publishes one set of messages in response to one input)
  • you're not making use of timer callbacks, and you're instead using sleep within a callback that isn't using something like rclpy.sleep.

If you want to do it this way, here's how I would do it instead:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PoseStamped
import math

class tcn(Node):
    def __init__(self):
        super().__init__("tcn")
        self.create_timer(1.0, self.timer_callback)
        self.pose_subscriber = self.create_subscription(
            PoseStamped, "pose", self.pose_callback, 10
        )
        self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10)

        self.new_data = False
        self.working_on_data = False
        self.t = 0.0

    def pose_callback(self, msg):
        self.goal_pose = msg.pose
        self.new_data = True

    def timer_callback(self):
        self.get_logger().info("Timer called")
        if self.new_data:
            self.get_logger().info("New data")
            self.new_data = False
            self.working_on_data = True
            self.t = 0.0
        if self.working_on_data:
            self.get_logger().info("Working on data")
            self.t += (2 * math.pi) / 10.0  # increment by 1/10th of a radian
            linear_vel = 0.0  # do some math here
            angular_vel = 0.0  # do some math here

            twist = Twist()
            twist.linear.x = linear_vel
            twist.angular.z = angular_vel

            self.cmd_vel_publisher.publish(twist)

            if self.t >= 2 * math.pi:
                self.working_on_data = False

def main(args=None):
    rclpy.init(args=args)
    node = tcn()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

What you'll notice I'm doing differently here is:

  • the timer is its own devoted callback
  • the pose subscriber is its own devoted callback, too, allowing it to focus on updating data
  • the timer callback is allowed to run every single time, but we conditionally check what we should be doing each loop.

But, I would also consider if what you really should be using here is an Action: you're probably still learning about these concepts so you may not have gotten to that point yet, but at some point in the future you should consider revisiting this exercise and implementing it as an action. Actions are a better choice for these longer-term execution tasks.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.