Created
March 19, 2026 14:06
-
-
Save zwzheng45/560eacf71a86d59d93b3e6d2ac2992b5 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python3 | |
| import time | |
| import rospy | |
| from geometry_msgs.msg import Twist | |
| import math | |
| MAX_FWD_SPEED=3 | |
| SPIN_SPEED=0.5 | |
| HZ=10 | |
| def init(): | |
| global HZ | |
| global msg,rate,pub,sub | |
| rospy.init_node("cw1_turtlesim") | |
| msg=Twist() | |
| rate=rospy.Rate(HZ) | |
| pub=rospy.Publisher('/turtle1/cmd_vel',Twist,queue_size=10) | |
| # make angle diff between pi & -pi | |
| def normalize_angle(a): | |
| return math.atan2(math.sin(a),math.cos(a)) | |
| def rotate(angle): | |
| global msg,pub,rate,SPIN_SPEED | |
| target_rad=normalize_angle(math.radians(angle)) | |
| tick=int((target_rad/SPIN_SPEED)*HZ) | |
| msg.angular.z=SPIN_SPEED | |
| for i in range(tick): | |
| pub.publish(msg) | |
| rate.sleep() | |
| msg=Twist() | |
| pub.publish(msg) | |
| def move(dis): | |
| global HZ,MAX_FWD_SPEED,rate,msg | |
| tick=int((dis/MAX_FWD_SPEED)*HZ) | |
| msg.linear.x=MAX_FWD_SPEED | |
| for i in range(tick): | |
| pub.publish(msg) | |
| rate.sleep() | |
| msg=Twist() | |
| pub.publish(msg) | |
| init() | |
| move(4.5) | |
| rotate(125) | |
| move(2) | |
| rotate(20) | |
| move(4.5) | |
| rotate(40) | |
| move(3.7) | |
| rotate(97) | |
| move(9.5) | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment