[英]Printing only the starting position and final position in ROS
我是 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獲取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方向默認消息是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]
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.