移动机器人速度规划代码

以下是一个基于ROS的移动机器人速度规划示例代码，使用了ROS中的move_base包和navigation包：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib

class RobotController:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('robot_controller', anonymous=True)
        
        # 订阅机器人当前位置
        rospy.Subscriber('/odom', Odometry, self.odom_callback)
        
        # 初始化移动机器人的客户端
        self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.move_base_client.wait_for_server()
        
        # 初始化机器人速度控制器
        self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.vel_msg = Twist()
        
        # 初始化机器人位置信息
        self.current_pose = PoseStamped()
        
    def odom_callback(self, odom_msg):
        self.current_pose.pose = odom_msg.pose.pose
        
    def move_to_goal(self, goal_x, goal_y):
        # 创建目标点
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = goal_x
        goal.target_pose.pose.position.y = goal_y
        goal.target_pose.pose.orientation.w = 1.0
        
        # 发送目标点给移动机器人的客户端
        self.move_base_client.send_goal(goal)
        
        # 等待机器人移动到目标点
        self.move_base_client.wait_for_result()
        
    def control_robot_velocity(self, linear_x, angular_z):
        # 发布机器人速度控制消息
        self.vel_msg.linear.x = linear_x
        self.vel_msg.angular.z = angular_z
        self.vel_pub.publish(self.vel_msg)
        
    def stop_robot(self):
        # 停止机器人移动
        self.move_base_client.cancel_all_goals()
        self.control_robot_velocity(0, 0)
        
if __name__ == '__main__':
    try:
        # 创建机器人控制器对象
        controller = RobotController()
        
        # 移动机器人到指定位置
        controller.move_to_goal(1.0, 1.0)
        
        # 控制机器人向左旋转
        controller.control_robot_velocity(0, 0.5)
        rospy.sleep(2)
        
        # 停止机器人移动
        controller.stop_robot()
        
    except rospy.ROSInterruptException:
        pass

以上代码仅供参考，具体的速度规划算法需要根据机器人的具体情况进行调整和优化。