远程控制slam小车及pid调试&&PC与树莓派ssh链接时出现间歇性联通段开网络故障acailable I Destination Host Unreachable_然后5s后切换了一个地图

一时失言乱红尘 2022-12-08 04:50 159阅读 0赞

## 远程控制slam小车 ##

把小车树莓派及pc端ubuntu通过无线路由器连接到统一局域网中

2.登入路由器查看是设备是否连接成功

http://192.168.1.1/  
路由器密码

树莓派: zxcar  密码:123

ssh  zxcar@树莓派的IP   然后输入密码 就可以了    确保自己的电脑和树莓派在同一个网络

远程拷贝

scp -r src zxcar@192.168.1.12:~/itheima4_ws

远程登入

ssh  zxcar@192.168.1.12

密码 123

上位机单独启动驱动命令

rosrun car_driver car_driver.py

ubuntu链接串口

ls -al /dev/ttyU*

ros 控制键盘调试

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

键盘控制各个键介绍：  
\---------------------------  
   U   I     O     
   J    K     L  
   M   <    >  
K—停止  
I、J、<、L—前、左、后、右  
q/z : 最大速度增加/减少10%  
w/x : 仅线性速度增加10%  
e/c : 仅角速度增加10%

# **查找设备** #

lsusb -vvv

**设置设备串口udev,定义rules文件  
激活rules**

１．拷贝到以下目录

sudo cp 58-robot.rules /etc/udev/rules.d/

或

sudo cp 58-robot.rules /lib/udev/rules.d/

２．加载文件

sudo service udev reload

３．启动

sudo service udev restart

启动报错提示用下面的命令：

sudo systemctl daemon-reload

４．插上设备 查看是否安装完成

ls -al /dev/skser*
    ls -al /dev/sulid*

命令

roslaunch zxcar bringup.launch

树莓派启动命令建图

roslaunch zxcar lidar_slam.launch

这个要在pc本机启动

roslaunch zxcar lidar_slam_rviz.launch

本机启动键盘控制

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

问题:

sk@sk-PC:~/workspas/slam$ rosrun teleop\_twist\_keyboard teleop\_twist\_keyboard.py   
Traceback (most recent call last):  
  File "/home/sk/workspas/slam/src/teleop\_twist\_keyboard/teleop\_twist\_keyboard.py", line 7, in <module>  
    import roslib; roslib.load\_manifest('teleop\_twist\_keyboard')  
  File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 64, in load\_manifest  
    sys.path = \_generate\_python\_path(package\_name, \_rospack) + sys.path  
  File "/opt/ros/melodic/lib/python2.7/dist-packages/roslib/launcher.py", line 97, in \_generate\_python\_path  
    m = rospack.get\_manifest(pkg)  
  File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 171, in get\_manifest  
    return self.\_load\_manifest(name)  
  File "/usr/lib/python2.7/dist-packages/rospkg/rospack.py", line 215, in \_load\_manifest  
    retval = self.\_manifests\[name\] = parse\_manifest\_file(self.get\_path(name), self.\_manifest\_name, rospack=self)  
  File "/usr/lib/python2.7/dist-packages/rospkg/manifest.py", line 414, in parse\_manifest\_file  
    \_static\_rosdep\_view = init\_rospack\_interface()  
  File "/usr/lib/python2.7/dist-packages/rosdep2/rospack.py", line 59, in init\_rospack\_interface  
    lookup = \_get\_default\_RosdepLookup(Options())  
  File "/usr/lib/python2.7/dist-packages/rosdep2/main.py", line 134, in \_get\_default\_RosdepLookup  
    verbose=options.verbose)  
  File "/usr/lib/python2.7/dist-packages/rosdep2/sources\_list.py", line 594, in create\_default  
    sources = load\_cached\_sources\_list(sources\_cache\_dir=sources\_cache\_dir, verbose=verbose)  
  File "/usr/lib/python2.7/dist-packages/rosdep2/sources\_list.py", line 552, in load\_cached\_sources\_list  
    with open(cache\_index, 'r') as f:  
IOError: \[Errno 13\] Permission denied: '/home/sk/.ros/rosdep/sources.cache/index'

**IOError: \[Errno 13\] 权限问题**

**解决:**

cd /home/sk/.ros/rosdep/sources.cache/
    sudo chmod 777 index

问题2:链接不上或找不到节点 可能ip问题  
输入

ifconfig

与

echo $ROS_HOSTNAME

查看ip是否一致  
 强制更改 本机ip; 把动态命令写死

export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`

改

export ROS_HOSTNAME=192.168.1.10

动态命令在多网卡或网络中断会导致把本机ip发送给mast节点的ip不准确

第一种:动态多机部署ros 方法:

主机:树莓派端配置

export ROS_IP=`hostname -I | awk '{print $1}'`
    export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`
    export ROS_MASTER_URI=http://`hostname -I | awk '{print $1}'`:11311

从机:

export ROS_IP=`hostname -I | awk '{print $1}'`
    export ROS_HOSTNAME=`hostname -I | awk '{print $1}'`
    export ROS_MASTER_URI=http://树莓派的IP地址:11311

第二种:多机部署ros 方法:

> 主机和从机要在同一个局域网内

1. 主机添加在hosts中添加所有节点的ip和主机名(主机配置)

```bash
       sudo gedit /etc/hosts
       ```

> ip  主机名

2. 从机配置master节点服务(主机也要修改)

```bash
       sudo gedit ~/.bashrc
       
       export ROS_HOSTNAME=本机ip
       
       export ROS_MASTER_URI=http://主机ip:11311

## **设置树莓派连接wifi** ##

**HUAWEI-sukai1   01524922**

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1][]

把树莓派tf卡插入电脑

1.新建一个文件名字ssh的空文件

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2][]

4 ） 使 用 记 事 本 工 具 ， 按 照 下 面 的 参 考 格 式 填 入 记 事 本 内 容 ， 然 后 命 名 为

“wpa\_supplicant.conf ”（温馨提示：可先输入下方内容，保存并退出该文件。然后将文件名

进行重命名，需删除包含 txt 在内的所有文件名，替换为上方名称。如有弹窗提示，选择“是”

34即可），再保存至“boot”盘符内。

country=CN

ctrl\_interface=DIR=/var/run/wpa\_supplicant GROUP=netdev

update\_config=1

network=\{

ssid="Hiwonder"

psk="123456789"

key\_mgmt=WPA-PSK

priority=1

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4][]

弹出tf卡插入树莓派,让树莓派与电脑都连接到热点

## ##

###  PC与树莓派ssh链接时出现间歇性联通段开网络故障 ###

sendmsg: No buffer space acailable

sendmsg: Network is Unreachable

From 192.168.\*\*.\*\*\*  icmp\_se=183 Destination Host Unreachable

connection closed by 192.168.\*\*.\*\*\*   port 22

以下是ping的图片

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 5][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 6][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 7][]

故障排除  
        如果当你尝试连接到小派时遇到connection reset by peer错误，可能是由SSH key引起的。你可以通过下面的命令来重置它。

首先，删除旧的key文件：

sudo rm /etc/ssh/ssh_host_*

然后生成一个新的：

sudo dpkg-reconfigure openssh-server

然后再试一次。

重启

reboot

在windows 中使用putty登入试试

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 8][]

NameError: global name 'ser' is not defined

serial 这个报错：

查出来必须要卸载serial 不要安装：
    pip uninstall serial
    
    要安装：
    pip install pyserial

### slam小车pid调试 ###

安装 rqt\_plot

rosdep install  rqt\_plot

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 9][]

启动命令:

rqt\_plot

![20200917191823350.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 10][]

在上位机car\_driver驱动中需要把速度信息发布出来:

![20200917191851357.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 11][]

rostopic list 查看节点

![20200917191913198.png][]

控制台输出句柄消息

rostopic echo /zxcar/get\_vel

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 12][]

rqt\_plot 中监听速度

命令

rqt\_plot

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 13][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 14][]

调整y方向的范围:

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 15][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 16][]

调试:

Rosrun  teleop\_twist\_keyboard teleop\_twist\_keyboard.py   //启动键盘控制  
$rviz    //参考上节开启navigatin.rviz观察机器人仿真情况  
采用键盘控制模拟机器人，模拟机器人开始移动。注意鼠标指针必须位于teleop\_twist\_keyboard终端页面，否则控制键盘模拟机器人无法移动。  
键盘控制各个键介绍：  
\---------------------------  
   U   I     O     
   J    K     L  
   M   <    >  
K—停止  
I、J、<、L—前、左、后、右  
q/z : 最大速度增加/减少10%  
w/x : 仅线性速度增加10%  
e/c : 仅角速度增加10%

![20200917192023807.png][]

速度设置到0.215

![20200917192034182.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 17][]

启动rosrun rqt\_topic rqt\_topic

![2020091719210644.png][]

小车反馈回来,小车收到的数据

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 18][]

看角速度的数据

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 19][]

py\_slam\_exploring\_slam.py自动导航建模

#!/usr/bin/env python 
    # -*- coding: utf-8 -*-
     
    import roslib;
    import rospy  
    import actionlib  
    from actionlib_msgs.msg import *  
    from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
    from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
    from random import sample  
    from math import pow, sqrt  
     
    class NavTest():  
        def __init__(self):  
            rospy.init_node('exploring_slam', anonymous=True)  
            rospy.on_shutdown(self.shutdown)  
     
            # 在每个目标位置暂停的时间 (单位：s)
            self.rest_time = rospy.get_param("~rest_time", 2)  
     
            # 是否仿真？  
            self.fake_test = rospy.get_param("~fake_test", True)  
     
            # 到达目标的状态  
            goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
                           'SUCCEEDED', 'ABORTED', 'REJECTED',  
                           'PREEMPTING', 'RECALLING', 'RECALLED',  
                           'LOST']  
     
            # 设置目标点的位置  
            # 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点  
            # 在终端中就会看到该点的坐标信息  
            locations = dict()  
     
            locations['1'] = Pose(Point(4.589, -0.376, 0.000),  Quaternion(0.000, 0.000, -0.447, 0.894))  
            locations['2'] = Pose(Point(4.231, -6.050, 0.000),  Quaternion(0.000, 0.000, -0.847, 0.532))  
            locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))  
            locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
            locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))  
            locations['6'] = Pose(Point(2.924, 0.018, 0.000),   Quaternion(0.000, 0.000, 0.000, 1.000))  
     
            # 发布控制机器人的消息  
            self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)  
     
            # 订阅move_base服务器的消息  
            self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
     
            rospy.loginfo("Waiting for move_base action server...")  
     
            # 60s等待时间限制  
            self.move_base.wait_for_server(rospy.Duration(60))  
            rospy.loginfo("Connected to move base server")  
      
            # 保存机器人的在rviz中的初始位置  
            initial_pose = PoseWithCovarianceStamped()  
     
            # 保存成功率、运行时间、和距离的变量  
            n_locations = len(locations)  
            n_goals = 0  
            n_successes = 0  
            i = n_locations  
            distance_traveled = 0  
            start_time = rospy.Time.now()  
            running_time = 0  
            location = ""  
            last_location = ""    
     
            # 确保有初始位置  
            while initial_pose.header.stamp == "":  
                rospy.sleep(1)  
     
            rospy.loginfo("Starting navigation test")  
     
            # 开始主循环，随机导航  
            while not rospy.is_shutdown():  
                # 如果已经走完了所有点，再重新开始排序  
                if i == n_locations:  
                    i = 0  
                    sequence = sample(locations, n_locations)  
     
                    # 如果最后一个点和第一个点相同，则跳过  
                    if sequence[0] == last_location:  
                        i = 1  
     
                # 在当前的排序中获取下一个目标点  
                location = sequence[i]  
     
                # 跟踪行驶距离  
                # 使用更新的初始位置  
                if initial_pose.header.stamp == "":  
                    distance = sqrt(pow(locations[location].position.x -   
                                        locations[last_location].position.x, 2) +  
                                    pow(locations[location].position.y -   
                                        locations[last_location].position.y, 2))  
                else:  
                    rospy.loginfo("Updating current pose.")  
                    distance = sqrt(pow(locations[location].position.x -   
                                        initial_pose.pose.pose.position.x, 2) +  
                                    pow(locations[location].position.y -   
                                        initial_pose.pose.pose.position.y, 2))  
                    initial_pose.header.stamp = ""  
     
                # 存储上一次的位置，计算距离  
                last_location = location  
     
                # 计数器加1  
                i += 1  
                n_goals += 1  
     
                # 设定下一个目标点  
                self.goal = MoveBaseGoal()  
                self.goal.target_pose.pose = locations[location]  
                self.goal.target_pose.header.frame_id = 'map'  
                self.goal.target_pose.header.stamp = rospy.Time.now()  
     
                # 让用户知道下一个位置  
                rospy.loginfo("Going to: " + str(location))  
     
                # 向下一个位置进发  
                self.move_base.send_goal(self.goal)  
     
                # 五分钟时间限制  
                finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   
     
                # 查看是否成功到达  
                if not finished_within_time:  
    			    #取消机器人运动,不需要运动到指定点了
                    self.move_base.cancel_goal()  
                    rospy.loginfo("Timed out achieving goal")  
                else:  
                    state = self.move_base.get_state()  
                    if state == GoalStatus.SUCCEEDED:  
                        rospy.loginfo("Goal succeeded!")  
                        n_successes += 1  
                        distance_traveled += distance  
                        rospy.loginfo("State:" + str(state))  
                    else:  
                      rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
     
                # 运行所用时间  
                running_time = rospy.Time.now() - start_time  
                running_time = running_time.secs / 60.0  
     
                # 输出本次导航的所有信息  
                rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
                              str(n_goals) + " = " +   
                              str(100 * n_successes/n_goals) + "%")  
     
                rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
                              " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
     
                rospy.sleep(self.rest_time)  
     
        def update_initial_pose(self, initial_pose):  
            self.initial_pose = initial_pose  
     
        def shutdown(self):  
            rospy.loginfo("Stopping the robot...")  
            self.move_base.cancel_goal()  
            rospy.sleep(2)  
            self.cmd_vel_pub.publish(Twist())  
            rospy.sleep(1)  
     
    def trunc(f, n):  
        slen = len('%.*f' % (n, f))  
     
        return float(str(f)[:slen])  
     
    if __name__ == '__main__':  
        try:  
            NavTest()  
            rospy.spin()  
     
        except rospy.ROSInterruptException:  
            rospy.loginfo("Exploring SLAM finished.")

py\_opencv\_motion\_detector.py物体跟随

#!/usr/bin/env python
    # -*- coding: utf-8 -*-
    import rospy
    import cv2
    import numpy as np
    from sensor_msgs.msg import Image, RegionOfInterest
    from cv_bridge import CvBridge, CvBridgeError
     
    class motionDetector:
        def __init__(self):
            rospy.on_shutdown(self.cleanup);
     
            # 创建cv_bridge
            self.bridge = CvBridge()
            self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
     
            # 设置参数：最小区域、阈值
            self.minArea   = rospy.get_param("~minArea",   500)
            self.threshold = rospy.get_param("~threshold", 25)
     
            self.firstFrame = None
            self.text = "Unoccupied"
     
            # 初始化订阅rgb格式图像数据的订阅者，此处图像topic的话题名可以在launch文件中重映射
            self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
     
        def image_callback(self, data):
            # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
            try:
                cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")     
                frame = np.array(cv_image, dtype=np.uint8)
            except CvBridgeError, e:
                print e
     
            # 创建灰度图像
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            gray = cv2.GaussianBlur(gray, (21, 21), 0)
     
            # 使用两帧图像做比较，检测移动物体的区域
            if self.firstFrame is None:
                self.firstFrame = gray
                return  
    		#获取差分图 就是将两幅图像作差	两个图片相减，这里用的是灰度图，类型是uint8
            frameDelta = cv2.absdiff(self.firstFrame, gray)
            thresh = cv2.threshold(frameDelta, self.threshold, 255, cv2.THRESH_BINARY)[1]
     
            thresh = cv2.dilate(thresh, None, iterations=2)
            binary, cnts, hierarchy= cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
     
            for c in cnts:
                # 如果检测到的区域小于设置值，则忽略
                if cv2.contourArea(c) < self.minArea:
                   continue 
     
                # 在输出画面上框出识别到的物体
                (x, y, w, h) = cv2.boundingRect(c)
                cv2.rectangle(frame, (x, y), (x + w, y + h), (50, 255, 50), 2)
                self.text = "Occupied"
     
            # 在输出画面上打当前状态和时间戳信息
            cv2.putText(frame, "Status: {}".format(self.text), (10, 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
     
            # 将识别后的图像转换成ROS消息并发布
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
     
        def cleanup(self):
            print "Shutting down vision node."
            cv2.destroyAllWindows()
     
    if __name__ == '__main__':
        try:
            # 初始化ros节点
            rospy.init_node("motion_detector")
            rospy.loginfo("motion_detector node is started...")
            rospy.loginfo("Please subscribe the ROS image.")
            motionDetector()
            rospy.spin()
        except KeyboardInterrupt:
            print "Shutting down motion detector node."
            cv2.destroyAllWindows()

py\_slam\_startros\_在代码中启动ros节点roslaunch和rosrun

import subprocess
    import rospy
    import rosnode
     
    class launch_demo:
        def __init__(self, cmd=None):
            self.cmd = cmd
     
        def launch(self):
            self.child = subprocess.Popen(self.cmd)
            return True
     
        def shutdown(self):
            self.child.terminate()
            self.child.wait()
            return True
     
    if __name__ == "__main__":
        rospy.init_node('launch_demo',anonymous=True)
     
        launch_nav = launch_demo(["roslaunch", "pibot_simulator", "nav.launch"])
     
        launch_nav.launch()
     
        r = rospy.Rate(0.2)
        r.sleep()
     
        rospy.loginfo("switch map...")
        r = rospy.Rate(1)
        r.sleep()
     
        rosnode.kill_nodes(['map_server'])
     
        map_name = "/home/pibot/ros_ws/src/pibot_simulator/maps/blank_map_with_obstacle.yaml"
     
        map_node = subprocess.Popen(["rosrun", "map_server", "map_server", map_name, "__name:=map_server"])
     
        while not rospy.is_shutdown():
            r.sleep()

**上面使用`python`代码启动了一个`PIBOT`模拟器的导航，然后`5s`后切换了一个地图**

*  使用`subprocess.Popen`可以启动一个进程(`roslaunch`或者`rosrun`)
 *  使用`rosnode.kill_nodes`可以杀死一个`rosnode`

py\_slam\_定点导航

from launch_demo import launch_demo
    import rospy
     
    import actionlib
    from actionlib_msgs.msg import *
    from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
    from nav_msgs.msg import Path
    from geometry_msgs.msg import PoseWithCovarianceStamped
    from tf_conversions import transformations
    from math import pi
     
    class navigation_demo:
        def __init__(self):
            self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
     
            self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
            self.move_base.wait_for_server(rospy.Duration(60))
     
        def set_pose(self, p):
            if self.move_base is None:
                return False
     
            x, y, th = p
     
            pose = PoseWithCovarianceStamped()
            pose.header.stamp = rospy.Time.now()
            pose.header.frame_id = 'map'
            pose.pose.pose.position.x = x
            pose.pose.pose.position.y = y
            q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
            pose.pose.pose.orientation.x = q[0]
            pose.pose.pose.orientation.y = q[1]
            pose.pose.pose.orientation.z = q[2]
            pose.pose.pose.orientation.w = q[3]
     
            self.set_pose_pub.publish(pose)
            return True
     
        def _done_cb(self, status, result):
            rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
     
        def _active_cb(self):
            rospy.loginfo("[Navi] navigation has be actived")
     
        def _feedback_cb(self, feedback):
            rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
     
        def goto(self, p):
            goal = MoveBaseGoal()
     
            goal.target_pose.header.frame_id = 'map'
            goal.target_pose.header.stamp = rospy.Time.now()
            goal.target_pose.pose.position.x = p[0]
            goal.target_pose.pose.position.y = p[1]
            q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
            goal.target_pose.pose.orientation.x = q[0]
            goal.target_pose.pose.orientation.y = q[1]
            goal.target_pose.pose.orientation.z = q[2]
            goal.target_pose.pose.orientation.w = q[3]
     
            self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
            return True
     
        def cancel(self):
            self.move_base.cancel_all_goals()
            return True
     
    if __name__ == "__main__":
        rospy.init_node('navigation_demo',anonymous=True)
     
        launch_nav = launch_demo(["roslaunch", "pibot_simulator", "nav.launch"])
        launch_nav.launch()
     
        r = rospy.Rate(0.2)
        r.sleep()
     
        rospy.loginfo("set pose...")
        r = rospy.Rate(1)
        r.sleep()
        navi = navigation_demo()
        navi.set_pose([-0.7,-0.4,0])
     
        rospy.loginfo("goto goal...")
        r = rospy.Rate(1)
        r.sleep()
        navi.goto([0.25,4, 90])
     
        while not rospy.is_shutdown():
            r.sleep()

**上面完成设置机器人位置和导航到某一位置的功能**

`navi.set_pose([-0.7,-0.4,0])` 设置机器人位于地图中位置（-0.7，-0.4） 姿态`yaw=0°`

`navi.goto([0.25,4, 90])` 该接口调用一个服务完成机器人运动至位置（0.25，4）姿态`yaw=90°`

有了该接口就不难完成多点导航巡逻等应用

# Twiddle 调校最优解PID #

为了避免人为的猜测、试错来获取比较好的PID值，我们可以设计一种自动化的调参逻辑循环，根据使指定目标函数的值达到最小时，得到更加优秀的参数列表。

一种更智能、自动的方法是使用梯度下降算法。 前提是您从三个增益的初始猜测向量开始。 通常对P使用小的非零值，对I和D使用0。 然后，分别对每个收益进行小幅更改，然后测试目标函数是否降低。 如果降低了，将沿相同方向不断更改参数，否则尝试沿相反方向调整参数。 如果增益值的增加或减少均不会降低成本函数，则减小增益增量的大小并重复。 整个循环应继续进行，直到增量大小降至某个阈值以下。

![13-l-tuning-strategies-01][]

我们可以把以下过程称之为 Twiddle（旋弄、捻弄），可以帮助我们确定合适的P、I、D三个值，其原理是构建一个初始化为 \[0,0,0\]\[0,0,0\] 的参数列表，然后循环多次，根据每次循环返回的误差均值，调大或调小参数（调整的步伐参数由另一个3个值的列表控制）。直到步伐参数之和小于一定阈值时，停止循环，把此时参数列表作为最终的PID参数。

![image-20200910131251664][]

**代码实现：**

1.  拷贝依赖文件 [robot.py][] 到项目目录
2.  新建文件 `pid_twiddle.py`并编写如下内容：

from robot import Robot, show
    import numpy as np
    
    def make_robot():
        """
        创建并初始化机器人小车, 设置初始位置为(0, -1), 初始旋转角度为0
        """
        robot = Robot()
        robot.set(0, -1, 0)
        robot.set_steering_drift(10 / 180 * np.pi)
        return robot
    
    
    # NOTE: We use params instead of k_p, k_d, k_i
    def run(robot, params, n=100, speed=1.0):
        x_trajectory = []
        y_trajectory = []
        err = 0
        prev_cte = 0 - robot.y
        int_cte = 0
        for i in range(2 * n):
            cte = 0 - robot.y
            diff_cte = cte - prev_cte
            int_cte += cte
            prev_cte = cte
    
            steer = params[0] * cte + params[1] * diff_cte + params[2] * int_cte
    
            robot.move(steer, speed)
            x_trajectory.append(robot.x)
            y_trajectory.append(robot.y)
            if i >= n:
                err += cte ** 2
        return x_trajectory, y_trajectory, err / n
    
    
    # Make this tolerance bigger if you are timing out!
    def twiddle(tol=0.2):
        p = [0, 0, 0]
        dp = [1, 1, 1]
        robot = make_robot()
        x_trajectory, y_trajectory, best_err = run(robot, p)
    
        it = 0
        while sum(dp) > tol:
            # 循环，直到系数之和小于等于阈值（默认阈值为0.2，起始值为3.0）
            print("Iteration {}, best error = {}".format(it, best_err))
            for i in range(len(p)):
                p[i] += dp[i]
                robot.reset()
                x_trajectory, y_trajectory, err = run(robot, p)
    
                if err < best_err:
                    best_err = err
                    dp[i] *= 1.1     # 此值有助于减少总误差，下次可以多加点
                else:
                    p[i] -= 2 * dp[i]# 此值不利于于减少总误差，直接把刚加的去掉，并且向反方向减一倍
                    robot.reset()
                    x_trajectory, y_trajectory, err = run(robot, p)
    
                    if err < best_err:
                        best_err = err
                        dp[i] *= 1.1 # 如果反方向有利于减少总误差，扩大此值
                    else:
                        p[i] += dp[i] # 恢复为原值
                        dp[i] *= 0.9  # 把缩小变化系数
            it += 1
        return p, best_err
    
    
    params, err = twiddle()
    print("Final twiddle error = {} params = {}".format(err, params))
    robot = make_robot()
    x_trajectory, y_trajectory, err = run(robot, params)
    
    show(x_trajectory, y_trajectory, label="Twiddle PID")

**运行结果：**

![image-20200908171258048][]

可以与之前使用的PID进行对比，明显可以看出当前PID在x=50附近就开始在目标轨迹附近稳定下来。而之前的要到x=100的位置才可以稳定下来。

**输出日志：**

根据最后的输出日志可知：

最小的误差均值为 `7.940560962605189e-07`

此时对应的P、D、I分别为： `10.716018504541431, 18.68325573584582, 0.020275559590445292`

Iteration 0, best error = 7972.071547906822
    Iteration 1, best error = 0.048853806107299856
    Iteration 2, best error = 0.03026214567061226
    Iteration 3, best error = 0.0077046028132098255
    Iteration 4, best error = 0.003222969736312333
    Iteration 5, best error = 0.0016693580238629137
    Iteration 6, best error = 0.0009763548793623677
    Iteration 7, best error = 0.0006143945322198215
    Iteration 8, best error = 0.0006143945322198215
    Iteration 9, best error = 0.0006143945322198215
    Iteration 10, best error = 0.0006143945322198215
    Iteration 11, best error = 0.0006143945322198215
    Iteration 12, best error = 0.0006143945322198215
    Iteration 13, best error = 0.0006143945322198215
    Iteration 14, best error = 0.0006143945322198215
    Iteration 15, best error = 0.0006143945322198215
    Iteration 16, best error = 0.0006143945322198215
    Iteration 17, best error = 0.0006143945322198215
    Iteration 18, best error = 0.000612580641120018
    Iteration 19, best error = 0.0005393890590252054
    Iteration 20, best error = 0.0004707073557690764
    Iteration 21, best error = 0.00040854195467355256
    Iteration 22, best error = 0.00036173191894990056
    Iteration 23, best error = 0.0003496794660242756
    Iteration 24, best error = 0.00030372592629584626
    Iteration 25, best error = 0.00026649497706284647
    Iteration 26, best error = 0.00026502850791677363
    Iteration 27, best error = 0.00023267758106029443
    Iteration 28, best error = 0.00023267758106029443
    Iteration 29, best error = 0.00023267758106029443
    Iteration 30, best error = 0.00023267758106029443
    Iteration 31, best error = 0.00023267758106029443
    Iteration 32, best error = 0.00023222333181571606
    Iteration 33, best error = 0.00023222333181571606
    Iteration 34, best error = 0.00023222333181571606
    Iteration 35, best error = 0.00023222333181571606
    Iteration 36, best error = 0.00023222333181571606
    Iteration 37, best error = 0.00022978905387007294
    Iteration 38, best error = 0.00019428921416175855
    Iteration 39, best error = 0.00014919774411248834
    Iteration 40, best error = 2.2810673236127803e-05
    Iteration 41, best error = 6.65198948619874e-06
    Iteration 42, best error = 8.957452817602662e-07
    Iteration 43, best error = 8.957452817602662e-07
    Iteration 44, best error = 8.957452817602662e-07
    Iteration 45, best error = 8.957452817602662e-07
    Iteration 46, best error = 8.957452817602662e-07
    Iteration 47, best error = 8.957452817602662e-07
    Iteration 48, best error = 8.957452817602662e-07
    Iteration 49, best error = 8.957452817602662e-07
    Iteration 50, best error = 8.957452817602662e-07
    Iteration 51, best error = 7.940560962605189e-07
    Iteration 52, best error = 7.940560962605189e-07
    Iteration 53, best error = 7.940560962605189e-07
    Iteration 54, best error = 7.940560962605189e-07
    Iteration 55, best error = 7.940560962605189e-07
    Iteration 56, best error = 7.940560962605189e-07
    Iteration 57, best error = 7.940560962605189e-07
    Final twiddle error = 7.940560962605189e-07 params = [10.716018504541431, 18.68325573

robot.py

import random
    import numpy as np
    import matplotlib.pyplot as plt
    
    class Robot(object):
    
        def __init__(self, length=20.0):
            """
            鍒涘缓鏈哄櫒浜哄苟鍒濆鍖栦綅缃拰鏂瑰悜涓�0, 0, 0.
            """
            self.x = 0.0
            self.y = 0.0
            self.orientation = 0.0    # 涓嶺杞存鏂瑰悜鐨勫す瑙掞紙鍗曚綅涓哄姬搴︼級
            self.length = length      # 鍓嶅悗杞瓙鐨勮酱璺�
            self.steering_noise = 0.0 # 鏂瑰悜鍣０
            self.distance_noise = 0.0 # 璺濈鍣０
            self.steering_drift = 0.0 # 鏂瑰悜婕傜Щ
    
            self.default_state = {"x": self.x,"y": self.y,"o": self.orientation}
    
        def reset(self):
            self.x = self.default_state["x"]
            self.y = self.default_state["y"]
            self.orientation = self.default_state["o"]
    
        def set(self, x, y, orientation):
            """
            璁剧疆鏈哄櫒浜虹殑鍧愭爣鍙婃柟鍚�
            """
            self.x = x
            self.y = y
            self.orientation = orientation % (2.0 * np.pi)
            self.default_state = {"x": self.x,"y": self.y,"o": self.orientation}
    
        def set_noise(self, steering_noise, distance_noise):
            """
            璁剧疆鍣０鍙傛暟
            :param steering_noise: 杞悜鍣０
            :param distance_noise: 璺濈鍣０
            """
            # makes it possible to change the noise parameters
            # this is often useful in particle filters
            self.steering_noise = steering_noise
            self.distance_noise = distance_noise
    
        def set_steering_drift(self, drift):
            """
            璁剧疆绯荤粺鐨勮浆鍚戞紓绉诲弬鏁�
            """
            self.steering_drift = drift
    
        def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
            """
            灏忚溅鐨勭Щ鍔ㄥ嚱鏁�
    
            :param steering: 鍓嶈疆鐨勮浆鍚戣锛屾渶澶у€间负max_steering_angle
            :param distance: 鎬昏椹惰窛绂伙紝涓€鑸负闈炶礋
            :param tolerance: 杞悜鐨勬渶灏忓樊鍊硷紙闃堝€硷級锛屽皬浜庢闃堝€兼椂锛岃灏忚溅璧扮洿绾匡紝鍗曚綅涓哄姬搴�
            :param max_steering_angle: 鏈€澶ц浆鍚戣锛岄粯璁や负 180 / 4.0 = 45掳
            """
            # if steering > max_steering_angle:
            #     steering = max_steering_angle
            # if steering < -max_steering_angle:
            #     steering = -max_steering_angle
            steering = np.clip(steering, -max_steering_angle, max_steering_angle)
            if distance < 0.0:
                distance = 0.0
    
            # apply noise
            steering2 = random.gauss(steering, self.steering_noise)
            distance2 = random.gauss(distance, self.distance_noise)
    
            # apply steering drift
            steering2 += self.steering_drift
    
            # 瑙掗€熷害 = 绾块€熷害 / 杞集鍗婂緞
            # Execute motion
            turn = np.tan(steering2) * distance2 / self.length
    
            if abs(turn) < tolerance:
                # approximate by straight line motion 杩戜技鐩寸嚎妯″瀷
                self.x += distance2 * np.cos(self.orientation)
                self.y += distance2 * np.sin(self.orientation)
                self.orientation = (self.orientation + turn) % (2.0 * np.pi)
            else:
                # approximate bicycle model for motion 杩戜技鑷杞︽ā鍨�
                radius = distance2 / turn
                cx = self.x - (np.sin(self.orientation) * radius)
                cy = self.y + (np.cos(self.orientation) * radius)
                self.orientation = (self.orientation + turn) % (2.0 * np.pi)
                self.x = cx + (np.sin(self.orientation) * radius)
                self.y = cy - (np.cos(self.orientation) * radius)
    
        def __repr__(self):
            return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
    
    
    def show(x_trajectory, y_trajectory, p_array=[], i_array=[], d_array=[], label = 'PID'):
        n = len(x_trajectory)
        # fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
        # fig, ax1 = plt.subplots(figsize=(8, 4))
        fig = plt.figure()
        ax1 = fig.add_subplot(211)
        ax1.plot(x_trajectory, np.zeros(n), 'pink', label='reference')
        ax1.plot(x_trajectory, y_trajectory, 'black', label= label + ' controller')
        ax1.set_xlabel('x')  # Add an x-label to the axes.
        ax1.set_ylabel('y')  # Add a y-label to the axes.
        ax1.set_title('Car-Position')
        h, l = ax1.get_legend_handles_labels()
        ax1.legend(h, l)  # h 涓虹嚎鏉″璞″垪琛紝 l涓烘枃瀛楁弿杩板垪琛�
    
        ax2 = fig.add_subplot(212)
        if len(p_array) > 0:
            ax2.plot(x_trajectory, p_array, color='r', label='p')
        if len(i_array) > 0:
            ax2.plot(x_trajectory, i_array, color='g', label='i')
        if len(d_array) > 0:
            ax2.plot(x_trajectory, d_array, color='b', label='d')
    
        ax2.set_title('PID-Value')
        h, l = ax2.get_legend_handles_labels()
        ax2.legend(h, l)  # h 涓虹嚎鏉″璞″垪琛紝 l涓烘枃瀛楁弿杩板垪琛�
    
        plt.ylim((-1.5, 1.5))
        plt.tight_layout()
        plt.show()

## 小车跟随颜色块 ##

小车跟随黄色块进行运动,当然如果大家在地面上贴一根黄色的线,小车也可以跟随黄色线进行运动.

#! /usr/bin/env python
    # encoding: utf-8
    import rospy
    from geometry_msgs.msg import Twist
    import cv2 as cv
    
    def shutdown():
        twist = Twist()
        twist.linear.x = 0
        twist.angular.z = 0
        cmd_vel_Publisher.publish(twist)
        print "stop car..."
    
    if __name__ == '__main__':
        rospy.init_node("yellow_follow")
    
        # 当程序退出
        rospy.on_shutdown(shutdown);
    
        # ros控制的频率
        rate = rospy.Rate(100)
    
        # 定义publisher : cmd_vel
        cmd_vel_Publisher = rospy.Publisher("/cmd_vel",Twist,queue_size=1)
    
    
    
        capture = cv.VideoCapture(0)
        print capture.isOpened()
    
        ok,frame = capture.read()
    
        lowerb = (23,43,46)
        upperb = (34,255,255)
    
        height,width = frame.shape[0:2]
        screen_center = width / 2
        offset = 50
        while not rospy.is_shutdown():
    
            # 将图像转成HSV颜色空间
            hsv_frame = cv.cvtColor(frame, cv.COLOR_BGR2HSV)
            # 基于颜色的物品提取
            mask = cv.inRange(hsv_frame,lowerb,upperb)
            # 找出面积最大的区域
            _,contours,_ = cv.findContours(mask,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)
    
            maxArea = 0
            maxIndex = 0
            for i,c in enumerate(contours):
                area = cv.contourArea(c)
                if area > maxArea:
                    maxArea = area
                    maxIndex = i
            # 绘制
            cv.drawContours(frame,contours,maxIndex,(0,0,255),2)
            # 获取外切矩形
            x,y,w,h = cv.boundingRect(contours[maxIndex])
            cv.rectangle(frame,(x,y),(x+w,y+h),(255,255,0),2)
    
            # 获取外切矩形的中心像素点
            center_x = int(x + w/2)
            center_y = int(y + h/2)
            cv.circle(frame,(center_x,center_y),5,(0,0,255),-1)
    
            # 判断当前小车应该是左转还是右转还是直行
            twist = Twist()
            if center_x < screen_center - offset:
                twist.linear.x = 0.05
                twist.angular.z = 0.2
                print "turn left"
            elif center_x >= screen_center - offset and center_x <= screen_center + offset:
                twist.linear.x = 0.1
                twist.angular.z = 0.0
                print "go"
            elif center_x >  screen_center + offset:
                twist.linear.x = 0.05
                twist.angular.z = -0.2
                print "turn right"
            else:
                twist.linear.x = 0
                twist.angular.z = 0
                print "stop"
    
            # 将速度信息发送出去
            cmd_vel_Publisher.publish(twist)
    
            cv.imshow("mask",mask)
            cv.imshow("frame",frame)
            cv.waitKey(1)
            rate.sleep()
    
            ok, frame = capture.read()

pid

intergral = 0;
    derivative = 0;
    prev_error = 0;
    pid = 0
    def pid_control(curr,target):
        global intergral,derivative,prev_error,pid
        error = target - curr
        intergral += error;
        derivative = prev_error;
    
        # pid 计算公式
        kp = 0.2
        ki = 0.0
        kd = 0.02
        pid = kp*error + ki * intergral + kd* derivative
        prev_error = error;
        print pid;
        return pid

视觉巡线

##基于颜色提取
    #车道线表现出来是黄色的,所以我们可以利用我们前面基于HSV颜色范围的方式进行提取,最终提取出来的结果#
    def fetch_yellow(img):
        hsv_img = cv.cvtColor(img,cv.COLOR_BGR2HSV)
        low = (25,46,46);
        upper = (35,255,255)
        binary = cv.inRange(hsv_img,low,upper)
    
        if DEBUG:
            cv.imshow("binary color",binary);
        return binary
    
    ##截取ROI
    def fetch_roi(img):
        # 提取感兴趣的而区域
        height,width = img.shape[0:2]
        roi = img[int(height/2):height,0:width]
    
        # 对提取的图像进行闭操作
        kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))
        roi = cv.morphologyEx(roi,cv.MORPH_CLOSE,kernel,iterations=3)
    
        _, contours, _ = cv.findContours(roi,cv.RETR_EXTERNAL,cv.CHAIN_APPROX_SIMPLE)
    
        dst = np.zeros_like(roi)
        for i,c in enumerate(contours):
            area = cv.contourArea(c)
            if area>200:
                cv.drawContours(dst,contours,i,255,-1)
    
    
        if DEBUG:
            cv.imshow("roi",dst)
        return dst;
    
    
    ##提取边缘
    def fetch_edge(img):
        canny = cv.Canny(img,200,400)
        if DEBUG:
            cv.imshow("canny",canny)
        return canny
    
    
    ##找出直线
    #先提取了黄色,然后又提取感兴趣的区域,在感兴趣的区域中,我们提取了图像的边缘信息,在这一步我们就需要#来找出视线中可能的车道线了
    def make_points(line_kb,height):
        # 求右边的平均斜率和截距
        lines_avg_kb = np.average(line_kb,axis=0);
    
        # y = kx + b 绘制右边的直线
        y1 = height*0.5;
        x1 = (y1 - lines_avg_kb[1]) / lines_avg_kb[0];
    
        y2 = 0
        x2 = (y2 - lines_avg_kb[1]) / lines_avg_kb[0];
    
        return [x1,y1+0.5*height,x2,y2+0.5*height]
    
    def fetch_lines(img,height,width):
    
        lines = cv.HoughLinesP(img,1,np.pi/180,10,minLineLength=40,maxLineGap=5);
    
        left_kb = [];
        right_kb = [];
    
        if lines is None:
            return [];
    
        boundary = 1/3
        left_region_boundary = width * (1 - boundary)  # left lane line segment should be on left 2/3 of the screen
        right_region_boundary = width * boundary
        # 计算每一条线段的斜率和截距
        for line in lines:
            x1,y1,x2,y2 = line[0];
            # 计算当前线段的斜率和截距 y = kx + b
            if x1 == x2:
                continue
            params = np.polyfit((x1,x2),(y1,y2),1);
            # 斜率
            k = params[0];
            # 截距
            b = params[1];
    
            if k < 0:
                if x1 < left_region_boundary and x2 < left_region_boundary:
                    left_kb.append((k,b));
            else:
                if x1 > right_region_boundary and x2 > right_region_boundary:
                    right_kb.append((k,b));
    
        lines = []
        if len(left_kb)>0:
            left_line = make_points(left_kb,480)
            lines.append(left_line)
    
    
        if len(right_kb)>0:
            right_line = make_points(right_kb,height)
            lines.append(right_line)
    
        return lines;
    
    
    #计算引导线
    def calc_head_line(lines,width,height):
        if len(lines) == 0:
            return None;
        x = width/2
        y = height
    
        if len(lines) == 1:
            x1,y1,x2,y2 = lines[0]
            # params = np.polyfit((x1,x2),(y1,y2),1);
            # # 斜率
            # k = params[0];
            # # 截距
            # b = params[1];
    
            x_head = x2 + (x - x1)
            y_head = 0.5*height;
            # x_head = (y_head - b)/k
            return [x,y,x_head,y_head]
        else:
            _,_,line1_x2,_ = lines[0]
            _,_,line2_x2,_ = lines[1]
            x_head = (line1_x2 + line2_x2)/2
            y_head = 0.5*height
    
            return [x,y,x_head,y_head]
    
    
    #为了方便观察,显示所有线的函数
    def show_lines(lines,frame,head_line=None):
        if DEBUG:
            try:
                line_img = np.zeros_like(frame)
                for x1,y1,x2,y2 in lines:
                    cv.line(line_img,(int(x1),int(y1)),(int(x2),int(y2)),(0,255,0),10)
    
                if head_line:
                    x1,y1,x2,y2 = head_line
                    cv.line(line_img,(int(x1),int(y1)),(int(x2),int(y2)),(0,0,255),10)
    
                dst = cv.addWeighted(line_img,0.8,frame,1,0)
                cv.imshow("dst",dst)
            except Exception as e:
                print e
    
    
    ##计算偏差角度
    #为了便于控制小车转向的角度大小,我们计算引导线偏离屏幕中央的度数. 若引导线偏离中线较大,则角速度要#增大,反之角速度要减小.这里其实也是为我们接下来视觉PID引导做铺垫
    def calc_angle(x_offset,y_offset):
        angle_to_mid_radian = math.atan(x_offset / y_offset)  
        return angle_to_mid_radian
    
    
    
    ##pid
    #并且在这个过程中,偏离的角度越大,小车的角速度也越大,反之越小.这个正好可以用PID来进行控制,基于这样#的设想,我们用上位机视觉PID来控制小车,我们的目标就是要把偏离角度控制在0附近
    intergral = 0;
    derivative = 0;
    prev_error = 0;
    pid = 0
    def pid_control(curr,target):
        global intergral,derivative,prev_error,pid
        error = target - curr
        intergral += error;
        derivative = prev_error;
    
        # pid 计算公式
        kp = 0.2
        ki = 0.0
        kd = 0.02
        pid = kp*error + ki * intergral + kd* derivative
        prev_error = error;
        print pid;
        return pid
    
    if __name__ == '__main__':
    
        # Give the node a name
        rospy.init_node('heima_linefollow', anonymous=False)
    
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(shutdown)
    
        # How fast will we check the odometry values?
        rate = rospy.Rate(100)
    
        # Publisher to control the robot's speed
        cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    
        capture = cv.VideoCapture(0);
    
        ok,frame = capture.read()
    
        height,width = frame.shape[0:2]
        # while ok:
        while not rospy.is_shutdown():
            ok,frame = capture.read()
            # 提取黄色
            binary = fetch_yellow(frame)
            # 提取看兴趣的内容
            roi = fetch_roi(binary)
            # 提取边缘
            canny = fetch_edge(roi)
            # 提取线
            lines = fetch_lines(canny,height,width)
            head_line = calc_head_line(lines,width,height)
    
    
            if head_line:
                mid = width/2 ;
                _,_,curr,_ = head_line
    
                x_offset = curr - mid;
                y_offset = height*0.5
    
                angle = calc_angle(x_offset,y_offset)
    
                if angle > 60 or angle < -60:
                    continue
                show_lines(lines,frame,head_line)
                value = pid_control(angle,0)
                print "x_offset:{},{},{},angle={}".format(x_offset,mid,curr,angle)
                # 假设恒定线速度为0.25
                R = 0.1445/math.tan(value)
                V = 0.10
                W = V/R;
                twist = Twist()
                twist.linear.x = V
                twist.angular.z = W
    
                cmd_vel.publish(twist)
    
    
            cv.imshow("frame",frame)
            cv.waitKey(1)
            rate.sleep()

=========================================================================================================================

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20221123/6690e1ab2e3d46b1a585826721a5a15b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1]: /images/20221123/ee2b9d9ab8d94b599b6b97bc87da864d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2]: /images/20221123/4c0a6b8219a6433db0b04c132b579b06.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3]: /images/20221123/58a82d00843646208abe9a6ab16c4cfd.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4]: /images/20221123/8552e8c480f6490ab9ced664fe7e93b3.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 5]: /images/20221123/2b0f2bac726f4c769b2dcee362ed3189.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 6]: /images/20221123/1d4d50670c3147168126b81630480717.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 7]: /images/20221123/a24f8d16de504c1ba19efaa112e25e09.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 8]: /images/20221123/6f3f3bd4c76d4e0586a5e5fe1d042fe5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 9]: /images/20221123/b25facad06cf417685b5689860a7d399.png
[20200917191823350.png]: /images/20221123/4a9e9acd85f047c487c111e6deae35a5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 10]: /images/20221123/df93ed0f04444571974e56175dac4ba1.png
[20200917191851357.png]: /images/20221123/0b1e2783ff0c4740afbeec1c4105847d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 11]: /images/20221123/85a3d80191364d72a8386e7cb2baec61.png
[20200917191913198.png]: /images/20221123/1f836b29f85542d3bd0a20db2483533f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 12]: /images/20221123/e0dad5331ce24b5a99f344ebce8d0c1c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 13]: /images/20221123/3eaf87c43e0a489683484e5ffc6aefa9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 14]: /images/20221123/69473328afd342a3945334c6ec1272f5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 15]: /images/20221123/85ef96a3f5f04085a715118bcba9bc3b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 16]: /images/20221123/e1c3cfbf24454e29b3c924ab93350d4b.png
[20200917192023807.png]: /images/20221123/e867e1242e5343228ac3aa33122de978.png
[20200917192034182.png]: /images/20221123/d475a224bb744b299ef04a4487d7c292.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 17]: /images/20221123/ddca58c5a4d64ce99b056ec0d6ad1234.png
[2020091719210644.png]: /images/20221123/95122ac0edd949eeb6f2beab87101ce1.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 18]: /images/20221123/0fbe18b7301a40b1a75e0e772d3093db.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 19]: /images/20221123/cd981943ab194323a6edf29c113ff16f.png
[13-l-tuning-strategies-01]: /images/20221123/2998f8ee254640609c7002136fde54f8.png
[image-20200910131251664]: /images/20221123/fb924f9ed870474786b8e87f008b6fde.png
[robot.py]: http://robot.czxy.com/car/control/code/robot.py
[image-20200908171258048]: /images/20221123/ee13287ad7a5469ebebe78a371ea9006.png