我是 ROS 新手,他们给了我一些代码来“玩”。 我希望我的乌龟直线前进一米,然后转向 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()
感谢您的帮助。
最佳答案
您可以从 Turtlesim Pose Message 获取位置
、方向
和速度
我添加了一个检查机器人速度的条件:
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
> 方向类型,必须转换:
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]
关于python - 仅打印ROS中的起始位置和最终位置,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/53304831/