如何解决在ROS turtlesim中如何在正弦曲线路径上移动乌龟
在ROS turtlesim中,我们如何在正弦路径中移动乌龟?我知道我们需要使用比例控制器来实现这一目标。但是我没有得到实际的方法。我已经附上了迄今为止我尝试过的相同代码
注意:在回调函数中,我已将0转换为2pi比例,将-pi转换为pi比例,这在ros中使用
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
PI = 3.1415926535897
import math
# Initial value of theta is 0
theta = 0
# Subscriber callback function
def pose_callback(pose):
global theta
req = 2 * math.pi
if pose.theta < 0:
alpha = req - (pose.theta + (2 * math.pi))
else:
alpha = req - pose.theta
alpha = 2 * math.pi - alpha
theta = alpha
# sin_graph function
def sin_graph():
# Starts a new node
global theta
rospy.init_node('sin_graph',anonymous=True)
# Initialization of publisher
veLocity_publisher = rospy.Publisher(
'/turtle1/cmd_vel',Twist,queue_size=10)
# Subscribing to topic Pose
rospy.Subscriber("/turtle1/pose",Pose,pose_callback)
vel_msg = Twist()
# Initializing basic data
speed = 0.2
radius = 1
vel_msg.linear.x = speed
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = speed/radius
# Rate at which message is published (10 times per second)
rate = rospy.Rate(10)
# Loop until current distance is re-initialized to zero(theta = 0)
while not rospy.is_shutdown():
vel_msg.linear.x = speed * math.cos(theta)
vel_msg.angular.z = math.sin(theta)
veLocity_publisher.publish(vel_msg)
rospy.loginfo("Moving in a sine curve")
print(theta)
rate.sleep()
# Forcing our robot to stop
print("Goal Reached")
vel_msg.linear.x = 0
vel_msg.angular.z = 0
veLocity_publisher.publish(vel_msg)
rospy.spin()
if __name__ == '__main__':
try:
# Testing our function
sin_graph()
except rospy.ROSInterruptException:
pass
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。