ros2的twist消息丢失
我遇到了个奇怪的问题,反映在输出里面:
Enter command: stop()
[INFO] [1714134056.214406367] [move_robot]: Robot stopped
Enter command: move_forward(0.2,2)
Enter command:
背景:我在编写完用指令控制小车以规定速度移动固定的距离时,发现stop命令可以执行,而move_forward(0.2,2)指令却无法发送twist消息,move_forward 方法的日志,没有正确调用。
测试:代码在执行 move_forward 方法后立即退出了,而不等待机器人移动完毕,这可能是因为在发布 Twist 消息后,没有足够的时间给机器人移动到目标位置,我move_forward 方法后添加一个短暂的睡眠时间,发现还是没用;然后我在终端中输入命令来查看 /cmd_vel 话题消息,结果什么都没有,说明问题很可能出在发布速度命令的部分,但是我再次检查相关代码还是什么错误都没发现。
希望可以得到帮助,万分感谢!
整个程序代码:
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import time # Import time for time management
class MoveRobot(Node):
def __init__(self):
super().__init__('move_robot')
self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
self.speed = 0
self.distance = 0
self.timer_ = self.create_timer(0.1, self.timer_callback)
self.start_time = 0
self.command = None
def timer_callback(self):
if self.command == 'move_forward':
self.move_forward()
def move_forward(self):
twist = Twist()
twist.linear.x = self.speed
self.publisher_.publish(twist)
self.get_logger().info('Published forward command with speed: {}'.format(self.speed))
while time.time() - self.start_time < self.distance / self.speed:
time.sleep(0.1)
self.stop()
def stop(self):
twist = Twist()
self.publisher_.publish(twist)
self.get_logger().info('Robot stopped')
self.command = None
def parse_command(self, command):
cmd_parts = command.split('(')
cmd_name = cmd_parts[0]
if cmd_name == 'stop':
self.stop()
return
args = cmd_parts[1].strip(')').split(',')
self.command = cmd_name
self.speed = float(args[0])
self.distance = float(args[1])
self.start_time = time.time() # Update start time using time.time()
def main(args=None):
rclpy.init(args=args)
move_robot = MoveRobot()
try:
while rclpy.ok():
command = input('Enter command: ')
if command == 'exit':
break
move_robot.parse_command(command)
finally:
move_robot.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()