【仿真】Ros by example1 控制底座、导航、路径规划、SLAM代码注释

一、控制底座

效果：让机器人花一段时间向前移动1m，再旋转180度，最后返回原点。

1、基于定时的timed_out_and_back.py

# -*- coding: utf-8 -*-    有中文注释
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from math import pi

class OutAndBack():
    def __init__(self):
        #1、初始化：给出节点名字，设置回调函数
        rospy.init_node('out_and_back', anonymous=False)
        rospy.on_shutdown(self.shutdown)
        
        #2、发布话题，机器人运动速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        rate = 50
        r = rospy.Rate(rate)
        #3、设定前进速度、距离；转动速度、距离
        linear_speed = 0.2
        goal_distance = 1.0
        linear_duration = goal_distance / linear_speed
        angular_speed = 1.0
        goal_angle = pi
        angular_duration = goal_angle / angular_speed
     
        #往返两次行程 
        for i in range(2):
            #4、前进1m
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            ticks = int(linear_duration * rate)
            
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
            # 旋转之前停下来
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # 5、转动180度
            move_cmd.angular.z = angular_speed
            ticks = int(goal_angle * rate)
            
            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
            # 下一步之前停下来
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    
            
        # 发布空的Twist消息让机器人停下来
        self.cmd_vel.publish(Twist())
        
    def shutdown(self):
        # 发布空的Twist消息让机器人停下来
        rospy.loginfo("Stopping the robo