在ROS中只打印起始位置和结束位置

Question

我是 ROS 的新手，我得到了 "play with" 的一些代码。我想让我的乌龟直线前进一米，然后转 45 度角。我得到了正确的结果（或者至少看起来是这样......）但我也想要打印我的乌龟的起始和最终位置。我添加了一些代码以不间断的方式打印日志，这意味着每次迭代我都会得到我的乌龟的 x，y 位置，但我只想要它在开始和结束时，另外我想添加一个角度 theta 来表示我的角度乌龟在.

这是我的代码：

import sys, rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
PI = 3.1415926535897

theta = 0

def pose_callback(pose_msg):
    rospy.loginfo("x: %.2f, y: %.2f" % (pose_msg.x, pose_msg.y))

def move():
    msg.linear.x = FORWARD_SPEED_IN_MPS
    t0 = rospy.Time.now().to_sec()
    current_distance = 0
    # Move turtle as wanted distance
    while current_distance < DISTANCE_IN_METERS:
        pub.publish(msg)
        # Get current time.
        t1 = rospy.Time.now().to_sec()
        # Calc how much distance our turtle moved.
        current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)
    msg.linear.x = 0

def turn():
    current_angle = 0
    angular_speed = ROUND_SPEED * 2 * PI / 360
    relative_angle = 45 * 2 * PI / 360
    t0 = rospy.Time.now().to_sec()
    msg.angular.z = abs(angular_speed)
    while current_angle < relative_angle:
        pub.publish(msg)
        t1 = rospy.Time.now().to_sec()
        current_angle = angular_speed * (t1 - t0)

if __name__ == "__main__":
    robot_name = sys.argv[1]
    FORWARD_SPEED_IN_MPS = 0.5
    DISTANCE_IN_METERS = 1
    ROUND_SPEED = 5

    # Initialize the node
    rospy.init_node("move_turtle")
    # A publisher for the movement data
    pub = rospy.Publisher(robot_name+"/cmd_vel", Twist, queue_size=10)
    # A listener for pose
    sub = rospy.Subscriber(robot_name+"/pose", Pose, pose_callback, None, 10)

    # The default constructor will set all commands to 0
    msg = Twist()
    pose = Pose()
    # Loop at 10Hz, publishing movement commands until we shut down
    rate = rospy.Rate(10)
    # Drive forward at a given speed.  The robot points up the x-axis.
    move()
    # Turn counter-clockwise at a given speed.
    turn()

感谢您的帮助。

Answer 1

您可以从 Turtlesim Pose Message 获得 position、orientation 和 velocity，我添加了一个检查机器人速度的条件：

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

PI = 3.1415926535897
theta = 0

def pose_callback(msg):
    if msg.linear_velocity == 0 and msg.angular_velocity == 0:
        rospy.loginfo("x: %.2f, y: %.2f" % (msg.x, msg.y))
        rospy.loginfo('Orientation in euler - theta:{}'.format(msg.theta))

def move():
    msg.linear.x = FORWARD_SPEED_IN_MPS
    t0 = rospy.Time.now().to_sec()
    current_distance = 0

    while current_distance < DISTANCE_IN_METERS:
        pub.publish(msg)
        t1 = rospy.Time.now().to_sec()
        current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)
    msg.linear.x = 0

def turn():
    current_angle = 0
    angular_speed = ROUND_SPEED * 2 * PI / 360
    relative_angle = 45 * 2 * PI / 360
    t0 = rospy.Time.now().to_sec()
    msg.angular.z = abs(angular_speed)

    while current_angle < relative_angle:
        pub.publish(msg)
        t1 = rospy.Time.now().to_sec()
        current_angle = angular_speed * (t1 - t0)

if __name__ == "__main__":
    FORWARD_SPEED_IN_MPS = 0.5
    DISTANCE_IN_METERS = 1
    ROUND_SPEED = 5
    rospy.init_node("move_turtle")
    pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10)
    sub = rospy.Subscriber("turtle1/pose", Pose, pose_callback)
    msg = Twist()
    rate = rospy.Rate(100)
    move()
    turn()
    time.sleep(2)

[注意]:

在turtlesim中Orientation默认消息是euler类型，但在大多数ROS节点类型是quaternion，所以如果你想得到quaternion orientation类型，你必须转换它：

from tf.transformations import quaternion_from_euler

euler = (0, 0, pose.z)

quaternion = quaternion_from_euler(euler)
x = quaternion[0]
y = quaternion[1]
z = quaternion[2]
w = quaternion[3]

在ROS中只打印起始位置和结束位置

Printing only the starting position and final position in ROS

python

ros