#!/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")