Cx330_HomeWork/ROS_TurtleControl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

优化程序，添加小乌龟实时坐标位置

Browse Source
This commit is contained in:
张梦南 2025-10-12 13:34:17 +08:00
parent cdcf44c156
commit 5eb2014113
2 changed files with 18 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/Aee1220310013/scripts/client.py
View File

@ -2,14 +2,23 @@
import rospy import rospy
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from turtlesim.srv import TeleportAbsolute from turtlesim.srv import TeleportAbsolute
from turtlesim.msg import Pose
from Aee1220310013.srv import ProcessID from Aee1220310013.srv import ProcessID
current_pose = None # 全局变量保存乌龟位姿
# 位姿回调函数
def pose_callback(msg):
global current_pose
current_pose = msg
print(f"当前位置: x={msg.x:.2f}, y={msg.y:.2f}, 方向={msg.theta:.2f}")
def main(): def main():
rospy.init_node('turtle_controller_client') rospy.init_node('turtle_controller_client')
student_id = input("输入学号：").strip() student_id = input("输入学号：").strip()
# 调用服务获取参数 # 处理学号
rospy.wait_for_service('/process_id') rospy.wait_for_service('/process_id')
try: try:
process_id = rospy.ServiceProxy('/process_id', ProcessID) process_id = rospy.ServiceProxy('/process_id', ProcessID)
@ -19,14 +28,17 @@ def main():
return return
x, y, r, direction = resp.x, resp.y, resp.radius, resp.direction x, y, r, direction = resp.x, resp.y, resp.radius, resp.direction
print(f"服务返回: x={x}, y={y}, 半径={r}, 方向={direction}") print(f"x={x}, y={y}, 半径={r}, 方向={direction}")
# 移动乌龟到 (x, y) # 移动乌龟到 (x, y)
rospy.wait_for_service('/turtle1/teleport_absolute') rospy.wait_for_service('/turtle1/teleport_absolute')
teleport = rospy.ServiceProxy('/turtle1/teleport_absolute', TeleportAbsolute) teleport = rospy.ServiceProxy('/turtle1/teleport_absolute', TeleportAbsolute)
teleport(x, y, 0) teleport(x, y, 0)
print(f"乌龟已跳转到 ({x}, {y})") print(f"乌龟已跳转到 ({x}, {y})")
# 乌龟位置
rospy.Subscriber('/turtle1/pose', Pose, pose_callback)
# 画圆 # 画圆
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
rate = rospy.Rate(10) rate = rospy.Rate(10)
@ -38,7 +50,7 @@ def main():
vel_msg.linear.x = linear_speed vel_msg.linear.x = linear_speed
vel_msg.angular.z = direction * angular_speed vel_msg.angular.z = direction * angular_speed
print("开始画圆，按 Ctrl+Z 停止。") print("开始画圆，按 Ctrl+C 停止。")
while not rospy.is_shutdown(): while not rospy.is_shutdown():
pub.publish(vel_msg) pub.publish(vel_msg)

4
src/Aee1220310013/scripts/server.py
View File

@ -24,7 +24,7 @@ def handle_process_id(req):
total_three = sum(int(d) for d in last_three) total_three = sum(int(d) for d in last_three)
radius = total_three % 3 radius = total_three % 3
if radius == 0: if radius == 0:
radius = 3 radius = 3 / 2
# 方向（奇数顺时针，偶数逆时针） # 方向（奇数顺时针，偶数逆时针）
direction = -1 if last_one % 2 else 1 direction = -1 if last_one % 2 else 1
@ -35,7 +35,7 @@ def handle_process_id(req):
def id_processor_server(): def id_processor_server():
rospy.init_node('id_processor_server') rospy.init_node('id_processor_server')
service = rospy.Service('/process_id', ProcessID, handle_process_id) service = rospy.Service('/process_id', ProcessID, handle_process_id)
rospy.loginfo("服务端已启动：/process_id") rospy.loginfo("服务端已启动。")
rospy.spin() rospy.spin()
if __name__ == "__main__": if __name__ == "__main__":