Блог

Я учусь перемещать черепаху в Turtlesim. Я пытаюсь задать новую цель и перезапустить функцию перехода к позиции цели, в то время как turtle переходит к другой позиции цели.

Мой код здесь:

 #!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
import time

x = 0
y = 0
yaw = 0

def takepose(pose_message):
    global x, y, yaw
    x = pose_message.x
    y = pose_message.y
    yaw = pose_message.theta


def go_to_goalpose(x_goal, y_goal):
    global x, y, yaw
    
    velocity_message = Twist()

    while True:
        K_linear = 0.5
        distance = abs(math.sqrt((((x_goal - x) ** 2)   ((y_goal - y) ** 2))))

        linear_speed = distance * K_linear

        K_angular = 4.0
        desired_angle_goal = math.atan2(y_goal - y, x_goal-x)
        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)

        if (distance <= 0.01):
            break

    
if __name__ == "__main__":
    try:
        rospy.init_node("turtlesim_motion_pose", anonymous=True)

        cmd_vel_topic = "/turtle1/cmd_vel"
        velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)

        position_topic = "/turtle1/pose"
        pose_subscriber = rospy.Subscriber(position_topic, Pose, takepose)
        time.sleep(2)

        go_to_goalpose(0.0, 0.0)

    except rospy.ROSInterruptException:
        rospy.loginfo("node terminated")
 

Как я могу дать новую цель и перезапустить функцию go_to_goalpose, пока функция работает.

Souliton

В этой ссылке рассказывается, как это сделать.

http://wiki.ros.org/turtlesim/Tutorials/Go to Goal

Поскольку вы делаете while True это изнутри go_to_goalpose , я не думаю, что вы действительно могли бы это сделать. Вы могли бы реализовать эту функцию как обратный вызов, подобный takepose_and_move тому, который фактически вызывался бы всякий раз, когда вы получаете сообщение по теме ‘/ turtle1 / pose’ и публикуете скорость. В этом случае вы будете двигаться только до тех пор, пока продолжаете получать сообщение.