机器人导航

代码示例：ROS导航系统

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

class RobotNavigation:
    def __init__(self):
        rospy.init_node('robot_navigation')
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        
        self.current_pose = None
        self.scan_data = None
        
    def odom_callback(self, msg):
        self.current_pose = msg.pose.pose
        
    def scan_callback(self, msg):
        self.scan_data = msg.ranges
        
    def navigate(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            if self.scan_data and self.current_pose:
                # 简单的避障逻辑
                if min(self.scan_data) < 0.5:
                    self.stop_robot()
                else:
                    self.move_forward()
            rate.sleep()
            
    def move_forward(self):
        cmd = Twist()
        cmd.linear.x = 0.5
        self.cmd_vel_pub.publish(cmd)
        
    def stop_robot(self):
        cmd = Twist()
        cmd.linear.x = 0.0
        self.cmd_vel_pub.publish(cmd)

if __name__ == '__main__':
    try:
        nav = RobotNavigation()
        nav.navigate()
    except rospy.ROSInterruptException:
        pass