**和cpp不同，py是在终端添加可执行权限

ljy@ljy-Y7000P:~/camera_ws/src$ cd image_package_py/scripts/
ljy@ljy-Y7000P:~/camera_ws/src/image_package_py/scripts$ ls
image_node.py
ljy@ljy-Y7000P:~/camera_ws/src/image_package_py/scripts$ chmod +x image_node.py 
ljy@ljy-Y7000P:~/camera_ws/src/image_package_py/scripts$ ls
image_node.py
ljy@ljy-Y7000P:~/camera_ws/src/image_package_py/scripts$ 

rosrun的也直接是文件名，而不是cmake文件可执行权限那命名的node

仿真实现

一、ros相机图像获取实现

机器人头部相机-->ros的 sensor_msgs::Image格式--cv_bridge-->OpenCV的 Mat格式

想要显示图像，可以通过cv2.image()

这一块基本上是固定流程

#!/usr/bin/env python3
# coding=utf-8

#上面两个分别是为了说明编译器和显示中文添加的

import rospy
import cv2
from sensor_msgs.msg import Image # 引入image消息格式
from cv_bridge import CvBridge,CvBridgeError

def Cam_RGB_Callback(msg):
    bridge = CvBridge()
    try:
        cv_image = bridge.imgmsg_to_cv2(msg,"bgr8") #注意是bgr8
    except CvBridgeError as e:
        rospy.logerr("格式转换错误：%s",e)
        return
    
    cv2.imshow("RGB",cv_image) # 分别是显示框的标题，和显示的图像
    cv2.waitKey(1)   # 暂停1ms等待imshow完成工作


if __name__ == "__main__":
    rospy.init_node("show_cv_image")   #注意节点名不能重复
    # 订阅kinect2相机话题，Image消息类型，回调函数为Cam_RGB_Callback
    rgb_sub = rospy.Subscriber("/kinect2/qhd/image_color_rect",Image,Cam_RGB_Callback,queue_size=10)
    #spin让主函数阻塞处于等待
    rospy.spin()

二、颜色识别与定位

这里在前面的基础上，增加了对cv图像的处理。

*Python 写 ROS 图像节点时，回调函数尽量短。回调里只存数据，不要做太重的图像处理，更不要长时间卡在循环里。在回调函数里进行遍历可能导致程序卡住。

（1）颜色空间转换RGB --> HSV

（2）二值化分割提取目标物

（3）计算得出目标物质心坐标

#!/usr/bin/env python3
# coding=utf-8

import rospy
import cv2
from sensor_msgs.msg import Image 
from cv_bridge import CvBridge,CvBridgeError

h_min = 10
h_max = 40
s_min = 90
s_max = 255
v_min = 1
v_max = 255

bridge = CvBridge()
image = None

def Cam_RGB_Callback(msg):
    global h_min,h_max,s_min,s_max,v_min,v_max
    global image
    
    try:
        image = bridge.imgmsg_to_cv2(msg,"bgr8")
        
    except CvBridgeError as e:
        rospy.logerr("%s",e)
        return
    
    
    return
    
    
def nothing(x):
    pass


if __name__ == "__main__":
    rospy.init_node("hsv_node",anonymous=True) #增加随即后缀避免重名
    rgb_sub = rospy.Subscriber("kinect2/qhd/image_color_rect",Image,Cam_RGB_Callback,queue_size=1)
    
    cv2.namedWindow("Threshold")  # 为了能够动态调节增加的窗口，这个不能删掉，因为后面会搜索这个
    # 在窗口里创建的滑杆 cv2.createTrackbar(滑杆名字, 窗口名字, 初始值, 最大值, 回调函数)
    cv2.createTrackbar("h_min","Threshold",h_min, 179, nothing)   #opencv设置h为0-179
    cv2.createTrackbar("h_max","Threshold",h_max, 179, nothing)
    cv2.createTrackbar("s_min","Threshold",s_min, 255, nothing)
    cv2.createTrackbar("s_max","Threshold",s_max, 255, nothing)
    cv2.createTrackbar("v_min","Threshold",v_min, 255, nothing)
    cv2.createTrackbar("v_max","Threshold",v_max, 255, nothing)
    
    # 这里可以删掉，因为会创建新的
    # cv2.namedWindow("RGB")
    # cv2.namedWindow("HSV")
    # cv2.namedWindow("Result")
    
    rate = rospy.Rate(30)  # 表示1s循环30次
    while not rospy.is_shutdown():
        h_min = cv2.getTrackbarPos("h_min","Threshold")   # cv2.getTrackbarPos("滑杆名字", "窗口名字")去这个窗口找数值
        h_max = cv2.getTrackbarPos("h_max","Threshold")
        s_min = cv2.getTrackbarPos("s_min","Threshold")
        s_max = cv2.getTrackbarPos("s_max","Threshold")
        v_min = cv2.getTrackbarPos("v_min","Threshold")
        v_max = cv2.getTrackbarPos("v_max","Threshold")
        
        cv_image = image.copy()   #避免回调和主函数同时处理
        
        # 将rgb转成hsv格式，也是固定格式
        hsv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        # HSV空间进行均衡化，全是固定流程
        h, s, v = cv2.split(hsv_image)
        v = cv2.equalizeHist(v)
        hsv_image = cv2.merge([h, s, v])
        
        #使用颜色阈值进行二值化
        th_image = cv2.inRange(hsv_image,(h_min,s_min,v_min),(h_max,s_max,v_max))
        
        # 开操作 (去除一些噪点)
        element = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
        th_image = cv2.morphologyEx(th_image, cv2.MORPH_OPEN, element)
        # 闭操作 (连接一些连通域)
        th_image = cv2.morphologyEx(th_image, cv2.MORPH_CLOSE, element)
        
        
        # #遍历图像寻找并标记目标点（遍历可能导致太慢，参考下面处理）
        # target_x,target_y,p_count = 0,0,0
        # img_h,img_w = th_image.shape[:2]   #shape是（高，宽，通道数）
        # #注意图像高在前
        # for y in range(img_h):
        #     for x in range(img_w):
        #         if th_image[y,x] == 255:
        #             p_count +=1
        #             target_x +=x
        #             target_y +=y
                    
                    
        # if p_count>0:
        #     target_x = target_x//p_count    
        #     target_y = target_y//p_count   
        #     #cv2.line(图像, 起点, 终点, 颜色, 线宽) 颜色是bgr
        #     cv2.line(cv_image,(target_x-10,target_y),(target_x+10,target_y),[255,0,0],2)
        #     cv2.line(cv_image,(target_x,target_y-10),(target_x,target_y+10),[255,0,0],2)   
        #     print("数量",p_count)    
        # else:
        #     print("没有目标颜色")
        
        
        
        # th_image 是这个格式的，0 表示黑色，不是目标颜色，255 表示白色，是目标颜色
        #     0     0     0     0     0
        #     0     255   255   255   0
        #     0     255   255   255   0
        #     0     0     0     0     0
        # M 是一个字典，m00：白色区域的总量；m10：所有白色像素的 x 坐标加权总和；m01：所有白色像素的 y 坐标加权总和
        
        
        M = cv2.moments(th_image)

        if M["m00"] > 0:
            target_x = int(M["m10"] / M["m00"])
            target_y = int(M["m01"] / M["m00"])
            p_count = int(M["m00"] / 255)

            cv2.line(cv_image, (target_x - 10, target_y), (target_x + 10, target_y), (255, 0, 0), 2)
            cv2.line(cv_image, (target_x, target_y - 10), (target_x, target_y + 10), (255, 0, 0), 2)

            print("数量", p_count)
        else:
            print("没有目标颜色")
            
            
        cv2.imshow("RGB",cv_image)
        cv2.imshow("HSV",hsv_image)
        cv2.imshow("Result",th_image)
        cv2.waitKey(5)
        
        
        rate.sleep()
        
    cv2.destroyAllWindows()  #程序结束时，关闭所有窗口

深度相机实现