turtlesim_nav/scripts/tuturalsim_motion_pose.py

#!/usr/bin/env python3
import math
import time

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose


# Global State
x = 0
y = 0
yaw = 0

def pose_callback(pose_msg):
    global x, y, yaw
    x = pose_msg.x
    y = pose_msg.y
    yaw = pose_msg.theta

# TODO CONVERT ALL OF THIS INTO A CLASS
# TODO ADD TYPE ANNOTATIONS TO ALL THIS SHIT

def grid_clean(velocity_publisher):

    desired_pose = Pose()
    desired_pose.x = 1
    desired_pose.y = 1
    desired_pose.theta = 0

    go_to_goal(velocity_publisher, 1, 1)

    set_desired_orientation(velocity_publisher, 30, math.radians(desired_pose.theta))

    for i in range(5):
        move(velocity_publisher, 2.0, 1.0, True)
        rotate(velocity_publisher, 20, 90, False)
        move(velocity_publisher, 2.0, 9.0, True)
        rotate(velocity_publisher, 20, 90, True)
        move(velocity_publisher, 2.0, 1.0, True)
        rotate(velocity_publisher, 20, 90, True)
        move(velocity_publisher, 2.0, 9.0, True)
        rotate(velocity_publisher, 20, 90, False)
    pass

def spiral(velocity_publisher, rk, wk):
    """
    r = a + b(Theta)
    """
    velocity_msg = Twist()
    loop_rate = rospy.Rate(1)

    while (x< 10.5) and (y < 10.5):
        # At every iteration we will increase the linear velocity
        # We keep constant the angular velocity though
        rk = rk+1
        velocity_msg.linear.x = rk
        velocity_msg.angular.z = wk

        velocity_publisher.publish(velocity_msg)
        loop_rate.sleep()

    velocity_msg.linear.x = 0
    velocity_msg.angular.z = 0
    velocity_publisher.publish(velocity_msg)

def set_desired_orientation(velocity_publisher, speed_in_degrees, desired_angle_degrees):
    """Set desired orientation as an absolute angle."""
    relative_angle_radians = math.radians(desired_angle_degrees) - yaw
    clockwise = 0
    if relative_angle_radians < 0:
        clockwise = 1
    else:
        closewise = 0
    print("relative_angle_radians: ", math.degrees(relative_angle_radians))
    print("desired_angle_degree: ", desired_angle_degrees)
    rotate(velocity_publisher, speed_in_degrees, math.degrees(abs(relative_angle_radians)), clockwise)
    


# TODO YOu could update this to accept a Pose message as the 
# Target, with a distance tolerance (Epsilon) used when determining
# if you have arrived at the target
def go_to_goal(velocity_publisher, x_goal, y_goal):
    global x, y, yaw

    velocity_message = Twist()

    while True:
        # TODO Make this a param
        # aka Kp proportional gain for linear velocity
        K_linear = 0.5
        distance = abs(math.sqrt(((x_goal-x) ** 2) + ((y_goal-y) ** 2)))

        # This is the P in PID Control. So we have P-Controller
        # The linear speed is proportional to the distance
        # K_linear is our contanst.
        # So if the distance is high, the speed will be high,
        # but if the distance is 0 the speed will be zero
        linear_speed = distance * K_linear

        # TODO Make this a param
        # or Ki which is the proporional gain for the angular velocity
        K_angular = 4.0
        # Mathematically, atan2 gives the angle between two vectors
        desired_angle_goal = math.atan2(y_goal-y, x_goal-x)

        # Likewise the angular speed is proportional to the distance
        # beween the desired angle and the current angle
        # The larger the angle the larger the angular speed
        angular_speed = (desired_angle_goal-yaw)*K_angular
        
        velocity_message.linear.x = linear_speed
        velocity_message.angular.z = angular_speed

        velocity_publisher.publish(velocity_message)
        print(f"x={x}, y={y}, distance to goal: {distance} ")

        # With float values it is extremely hard to comare a
        # distance == 0 so we want to use some kind of reasoble
        # error aka epsilon
        if distance < 0.01:
            break


def rotate(velocity_publisher, angular_speed_degree, relative_angle_degree, clockwise):
    """
    velocity_publisher: Publisher
    angular_speed_degree: Degrees per second
    relative_
    """
    velocity_message = Twist()

    # Convert to radians
    angular_speed = math.radians(abs(angular_speed_degree))

    if clockwise:
        velocity_message.angular.z = -abs(angular_speed)
    else:
        velocity_message.angular.z = abs(angular_speed)

    loop_rate = rospy.Rate(10)
    t0 = rospy.Time.now().to_sec()

    while True:
        rospy.loginfo("Turtlesim rotates")
        velocity_publisher.publish(velocity_message)

        # Here we are calculating the rotated angle over time
        t1 = rospy.Time.now().to_sec()
        # This is an estimate 
        current_angle_degree = (t1-t0) * angular_speed_degree
        loop_rate.sleep() # In CPP you would also need to call ros spinOnce to dispatch one message

        if current_angle_degree > relative_angle_degree:
            rospy.loginfo("Reached")
            break
    
    velocity_message.angular.z = 0
    velocity_publisher.publish(velocity_message)

def move(velocity_publisher, speed, distance, is_forward):
    velocity_msg = Twist()

    # I don't like the global state access personally...
    global x, y

    # Save initial location
    x0 = x
    y0 = y

    if is_forward:
        velocity_msg.linear.x = abs(speed)
    else:
        velocity_msg.linear.x = -abs(speed)
    
    distance_moved = 0.0

    # NOTE if you have a lower update rate and higher velocity the
    # Robot will move further than intended since it is not getting
    # timely updates on if it has reached it's goal or not.
    # SO the effect of the loop rate has significant impact on the postion
    # achieved by the robot. 
    #
    #[INFO] [1646232853.396795]: Turtlesim moves forward
    # 4.0959999561309814
    # We can see that the target of 4.0 was exceeded by 0.095~ meters
    # a higher refresh rate would have detected that the target distance
    # was reached sooner.
    loop_rate = rospy.Rate(10) # Publish velocity message 10 times a second

    while True:
        rospy.loginfo("Turtlesim moves forward")
        velocity_publisher.publish(velocity_msg)
        loop_rate.sleep()
        # Calc Distance
        # Linear distance formula. (2 dimensions)
        # This works because the global x and y state are updated continually by the Pose callback
        # function that reports the robot's state.
        #
        # Alternative we could have taken the speed multipled by the difference in time (speed * (t1 - t0))
        # which would have needed us to grab a t0 and t1 using time.time() since that returns a time in seconds
        # This would not have required you to subscribe to the pose of the robot so there wouldn't be any global
        # state to manage
        #
        # THE ROS WAY TO CORRECTLY CALCULATE DISTANCE WOULD HAVE BEEN TO USE tf AKA TRANSFORMS!
        # That is covered in the SLAM & Nav in ROS for begginers 2
        distance_moved = abs(math.sqrt(((x - x0) **2) + ((y-y0) ** 2)))
        print(distance_moved)
        if distance_moved > distance:
            rospy.loginfo("Reached")
            break

    #Stop the robot when the target distance has been reached
    velocity_msg.linear.x = 0
    velocity_publisher.publish(velocity_msg)


# TODO Create an action for requesting a move.

if __name__ == "__main__":
    try:
        rospy.init_node("tuturalsim_motion_pose", anonymous=True)
        
        #declare velocity publisher
        cmd_vel_topic = '/turtle1/cmd_vel'
        velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)

        postion_topic = "/turtle1/pose"
        pose_subscriber = rospy.Subscriber(postion_topic, Pose, pose_callback)
        time.sleep(2)
        # Move one meter per second traveling forward for a target distance of 4 meters
        # move(velocity_publisher, 1.0, 4.0, True)
        # # Rotate 5 degrees a second for 45 degrees
        # rotate(velocity_publisher, 5.0, 45, False)
        # go_to_goal(velocity_publisher, 2 ,2)
        # set_desired_orientation(velocity_publisher, 30, 45)
        # spiral(velocity_publisher, 0, 2)
        grid_clean(velocity_publisher)
    except rospy.ROSInterruptException:
        rospy.loginfo("Node Terminated")