写了倒车的程序却无法运行
#!/usr/bin/env python# coding=utf-8
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
class ReverseParkingController:
def __init__(self):
rospy.init_node('reverse_parking_controller')
rospy.on_shutdown(self.shutdown)
self.reverse_distance = 0.0
self.reverse_mileage = 0.0
self.odom_received = False
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
def odom_callback(self, odom):
if not self.odom_received:
self.odom_received = True
# 假设车辆的行驶方向与x轴平行，只关注x轴上的位移变化
delta_distance = odom.pose.pose.position.x - self.reverse_distance
self.reverse_distance = odom.pose.pose.position.x
# 更新倒车里程信息
if delta_distance < 0:
self.reverse_mileage += abs(delta_distance)
def reverse_parking(self, distance):
rate = rospy.Rate(10)# 控制频率为10Hz
cmd_vel = Twist()
# 设置线速度和角速度，让车辆后退
cmd_vel.linear.x = -0.2# 假设线速度为-0.2m/s
cmd_vel.angular.z = 0.0
# 循环发布速度指令直到达到倒车距离
while not rospy.is_shutdown():
if self.odom_received and self.reverse_distance > distance:
self.cmd_vel_pub.publish(cmd_vel)
else:
break
rate.sleep()
# 发布停止指令
cmd_vel.linear.x = 0.0
self.cmd_vel_pub.publish(cmd_vel)
rospy.loginfo('Reverse parking complete. Reverse distance: %.2f meters', self.reverse_distance)
rospy.loginfo('Reverse mileage: %.2f meters', self.reverse_mileage)
def shutdown(self):
rospy.loginfo('Shutting down reverse parking controller...')
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(1)
if __name__ == '__main__':
try:
controller = ReverseParkingController()
controller.reverse_parking(5.0)# 设置倒车距离为5.0米
except rospy.ROSInterruptException:
pass
终端没有报错，也在launch文件里打开了底盘，但是启动后没有任何反应
页:
[1]