欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

ROS机器人编程实践——读书笔记4

程序员文章站 2022-06-01 08:42:49
...

一、ROS对图像处理,通过使用cv_bridge包来将ROS里的sensor_msgs/Image转换成OpenCV的格式。

follower_opencv.py

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
import cv2,cv_bridge

class Follower:
        def __init__(self):
                self.bridge=cv_bridge.CvBridge()
                cv2.namedWindow("window",1)
                self.image_sub=rospy.Subscriber('camera/rgb/image_raw',Image,self.image_callback)
        def image_callback(self,msg):
                image=self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
                cv2.imshow("window",image)
                cv2.waitKey(3)
                
rospy.init_node('follower')
follower=Follower()
rospy.spin()

$ roslaunch turtlebot_gazebo turtlebot_world.launch

$python follower_opencv.py

ROS机器人编程实践——读书笔记4

通过移动机器人获取不同的图像。

二、检测指示线

        我们根据颜色对图像进行逐行扫描,让机器人沿着颜色区域中心行进。检测黄线最简单的方法就是通过RGB颜色过滤找出与黄色接近的区域,不过简单的方法并不是很有效,轻微的光照变化,就会很大程度影响结果。将RGB颜色变化到HSV来改善这一问题。HSV将RGB图像分解色调(H)、饱和度(S)、明度(V)。在HSV图像将色调值与黄色接近的区域滤出得到一副二值图。

follower_color_filter.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
import cv2, cv_bridge, numpy

class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        cv2.namedWindow("window", 1)
        self.image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.image_callback)
        
    
    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg)
        
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # yuyv: yellow
        lower_yellow = numpy.array([50, 50, 170])   
        upper_yellow = numpy.array([255, 255, 190])
        
        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
        masked = cv2.bitwise_and(image, image, mask=mask)
        
        cv2.imshow("window", mask)
        cv2.imshow("hsv", hsv)
        cv2.waitKey(3)
        
rospy.init_node('follower')
follower = Follower()
rospy.spin()

ROS机器人编程实践——读书笔记4

三、跟踪黄线

        我们只考虑图像1/3高处的20行宽的部分,程序检测大概1m处的指示线的中心,然后标记一个圆点。

follower_line_finder.py

#!/usr/bin/env python
# BEGIN ALL
import rospy, cv2, cv_bridge, numpy
from sensor_msgs.msg import Image


class Follower:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    cv2.namedWindow("window", 1)
    self.image_sub = rospy.Subscriber('camera/rgb/image_raw', 
                                      Image, self.image_callback)

  def image_callback(self, msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    lower_yellow = numpy.array([ 10,  10,  10])
    upper_yellow = numpy.array([255, 255, 250])
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    
    # BEGIN CROP
    h, w, d = image.shape
    search_top = 3*h/4
    search_bot = search_top + 20
    mask[0:search_top, 0:w] = 0
    mask[search_bot:h, 0:w] = 0
    # END CROP
    # BEGIN FINDER
    M = cv2.moments(mask)
    if M['m00'] > 0:
      cx = int(M['m10']/M['m00'])
      cy = int(M['m01']/M['m00'])
    # END FINDER
    # BEGIN CIRCLE
      cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
    # END CIRCLE

    cv2.imshow("window", image)
    cv2.waitKey(3)

rospy.init_node('follower')
follower = Follower()
rospy.spin()
# END ALL

ROS机器人编程实践——读书笔记4

四、循线运动

        follower_p.py

#!/usr/bin/env python
# BEGIN ALL
import rospy, cv2, cv_bridge, numpy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

class Follower:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    cv2.namedWindow("window", 1)
    #self.image_sub = rospy.Subscriber('usb_cam/image_raw', 
    self.image_sub = rospy.Subscriber('camera/rgb/image_raw', 
                                      Image, self.image_callback)
    self.cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/teleop',
                                       Twist, queue_size=1)
    self.twist = Twist()
    
  def image_callback(self, msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    # gray color
    lower_yellow = numpy.array([ 10,  10,  10])
    upper_yellow = numpy.array([255, 255, 250])
    
    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
    
    h, w, d = image.shape
    search_top = 3*h/4
    search_bot = 3*h/4 + 20
    mask[0:search_top, 0:w] = 0
    mask[search_bot:h, 0:w] = 0
    M = cv2.moments(mask)
    if M['m00'] > 0:
      cx = int(M['m10']/M['m00'])
      cy = int(M['m01']/M['m00'])
      cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
      # BEGIN CONTROL
      err = cx - w/2
      self.twist.linear.x = 0.15
      self.twist.angular.z = -float(err) / 300   # 400: 0.1, 300: 0.15, 250, 0.2
      self.cmd_vel_pub.publish(self.twist)
      # END CONTROL
    cv2.imshow("window", image)
    cv2.imshow("mask", mask)
    cv2.waitKey(3)

rospy.init_node('follower')
follower = Follower()
rospy.spin()
# END ALL
ROS机器人编程实践——读书笔记4ROS机器人编程实践——读书笔记4