lesson 4: 代码实现—整体架构

一、整体说明

Diego 3dof的代码是移植的开源3D打印机项目Marlin的源代码,读者感兴趣可以访问Marlin项目github获取其源代码,Marlin项目由于要考虑适配不同的控制器,所以代码比较复杂,这里对其做了大幅的修改删减,已符合Diego 3dof的控制要求,修改后的文件结构如下图:

修改后一共剩余12个文件,且可以在Arudion UNO上运行,资源占用情况如下所示:

资源占用情况比较理想,如果资源占用情况比较高的情况下Arduino IDE会预警,可能在运行中有不可预知的不稳定现象。

同时由于diego3轴码垛和3D打印机的逆解不同,也做了相应的修改,后面会详细说明。

二、文件功能说明

1. Configuration.h 和Configuration_adv.h

这两个头文件中主要是针对运动学逆解,及马达控制的一些默认配置。

2. pin.h

这个头文件中,定义了Arduino的pin功能的对应关系

3. diego.h

这个头文件中定义了通用的函数及宏定义

4. planner.h 和 planner.cpp

这两个文件中主要是针对速度规划的函数、数据结构及运动学逆解运算。

5. stepper.h 和 stepper.cpp

这两个文件中主要是针对马达的控制算法

6.motion_control.h 和 motion_control.cpp

这两个文件中定义了运动规划的相关函数及数据结构

7. speed_lookupdate.h

这个文件中定义了在算法中用到的速度查询数组

8. diego_3dof.ino

arduino项目的主文件,功能包含,对串口命令的接收、g代码的解析、代码的初始化和主循环。

后续章节会针对每个部分展开详细讲解。

 

 

Lesson 3: Kinematic model

对于3轴码垛机械臂控制最基本的是对其建立运动学模型,而对于3轴码垛类型机械臂来说运动学模型,其本质就是给定空间3D坐标,求解3个轴的旋转角度。

如上图所示,左侧为实物坐标,右侧图为抽象到坐标系的几何表示,逆解过程就是知道末端坐标,而求解各个轴的旋转角度,进而转换为步进电机的步进数,下面我们利用立体几何,和解析几何知识来进行你运算分析。

一,假设条件

  • 坐标系采用右手坐标系,如上图所示
  • 机械臂的底座位于右手坐标系的XY平面
  • 底座旋转轴的位置,即为坐标系的原点位置,即上图做所视的O点
  • 我们把AB线所表示臂称之为大臂,BC线所表示的臂称之为小臂
  • OA线与XY平面的夹角为θ
  • AB线与Z轴的夹角为β
  • AB线与BC线之间的夹角为γ,即大臂与小臂的夹角
  • 点O,A,B,C始终处于同一平面,且此平面与XZ平面之间的夹角为α
  • 机械臂的初始位置为A,B,C三点的Y轴坐标为O,B,C点收回到距离Z轴最近的位置
  • 初始位置α=0,β=β0,γ=γ0
  • 已知条件:OA,AB,BC长度,θ角度是固定值

二、数学求解

已知C点的坐标(x,y,z),基于以上假设条件,求解α,β,γ。

为了方便求解,我们将相关点进行坐标系投影,如下图所示:

此方程其实比较简单,只需依据直线OCxy=OAxy+AxyBxy+BxyCxy,即列出方程,如下为方程式关系:

根据上述方程式,即可解出α,β,γ的值,基于此三个角度值除以步进电机的步进角度,轻松计算出从上一个位置到现在位置需要步进多少步。

Lesson 2: Hardware diagram

一、硬件连接

Diego 3dof的硬件连接比较简单,Arduino和A4988是通过Arduino CNC sheild v3连接的,Arduino CNC sheild可以直接插在Arduino上,然后再把3个A4988插在Arduino CNC sheild上即可,连接效果如下:

CNC sheild可以支持4路步进电机,由于我们3轴码垛只使用到了3个步进电机,所以只需要插3个A4988即可,步进电机去A4988的对应关系如下图:

 

二、Arduino Pin对应关系

A4988及马达编号Arduino IO对应功能
1D2马达1步进脉冲
1D5马达1方向
2D3马达2步进脉冲
2D6马达2方向
3D4马达2步进脉冲
3D7马达2方向

 

 

三、其他

1.Diego 3轴机械臂的机械部分需要根据图纸进行钣金加工,或者也可以3D打印,钣金加工的精度要高与3D打印的精度

2. 安装过程中需要用到的不同规格的螺丝刀,电钻等工。

Lesson 1: Hardware BOM

diego 3轴机械臂,采用三轴码垛结构,三轴都采用步进电机控制,装配好后如下图所示

如下表格列出了Diego 3轴机械臂所用硬件物料的详细清单,包括每个硬件图片、名称、数量、功能特性和在机械臂中的作用。

PhotoNameCountFeaturediego 3dof 码垛中的作用
Arduino UNO1•处理器 ATmega328
•工作电压 5V
•输入电压(推荐) 7-12V
•输入电压(范围) 6-20V
•数字IO脚 14 (其中6路作为PWM输出)
•模拟输入脚 6
IO脚直流电流 40 mA
•3.3V脚直流电流 50 mA
•Flash Memory 32 KB (ATmega328,其中0.5 KB 用于 bootloader)
•SRAM 2 KB (ATmega328)
•EEPROM 1 KB (ATmega328)
•工作时钟 16 MHz
•主控制板

Arduino cnc shield 1•步进电机驱动扩展板•驱动机械臂步进电机
A49883•步进电机驱动•驱动机械臂步进电机
42步进电机3•42步进电机
• 星形减速,减速比15:1
• 步进角度为1.8°
•机械臂3个轴的驱动马达
12V 18650 battery1•标称电压 :12V
•放电电流 :6.0A
•充电电流 :0.5A
•内阻 :≤60mΩ
•最高充电电压 :12.6V
•最低放电电压 :9.0V
•为6自由度机械臂舵机供电
•为机械爪舵机供电

硬件安装过程中使用的到工具,螺丝刀,老虎钳,电钻等工具未列出来,读者可以根据实际需求自行准备。

Lesson 26:Diego 1# —Object detection & location

google最近公布了基于tensorflow物体识别的Api,本文将利用Diego1#的深度摄像头调用物体识别API,在识别物体的同时计算物体与出机器人摄像头的距离。原理如下:

  • Object Detection 订阅Openni发布的Image消息,识别视频帧中的物体
  • Object Depth 订阅Openni发布的Depth Image消息,根据Object Detection识别出的物体列表,对应到Depth Image的位置,计算Object的深度信息
  • Publish Image将视频帧经过处理,增加识别信息Lable后,以Compressed Image消息发布出去,可以方便其他应用订阅

1.创建diego_tensorflow包

由于我们使用的tensorflow,所以我们首先需要安装tensorflow,可以参考tensorflow官方安装说明https://www.tensorflow.org/install/install_linux

Object_detection相关依赖安装见官方安装说明https://github.com/tensorflow/models/blob/master/object_detection/g3doc/installation.md

执行如下命令创建diego_tensorflow包

catkin_create_pkg diego_tensorflow std_msgs rospy roscpp cv_bridge

在diego_tensorflow目录下创建两个子目录

  • scripts:存放相关代码
  • launch:存放launch启动文件

创建完成后diego_tensorflow目录如下图所示:

下载object_detection包:https://github.com/tensorflow/models

下载后将object_detection包上传到diego_tensorflow/scripts目录下,如果自己做模型训练还需有上传slim包到diego_tensorflow/scripts目录下

物体识别的代码都写在ObjectDetectionDemo.py文件中,其中有关识别的代码大部分参考tensorflow官方示例,这里将其包装成为一个ROS节点,并增加物体深度数据的计算

2.ROS节点源代码

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image as ROSImage
from sensor_msgs.msg import CompressedImage as ROSImage_C
from cv_bridge import CvBridge
import cv2
import matplotlib
import numpy as np
import os
import six.moves.urllib as urllib
import sys
import tarfile
import tensorflow as tf
import zipfile
import uuid
from collections import defaultdict
from io import StringIO
from PIL import Image
from math import isnan

# This is needed since the notebook is stored in the object_detection folder.
from object_detection.utils import label_map_util
from object_detection.utils import visualization_utils as vis_util

class ObjectDetectionDemo():
    def __init__(self):
	rospy.init_node('object_detection_demo')
	
	# Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        self.depth_image =None
        
        self.depth_array = None
        
        model_path = rospy.get_param("~model_path", "")
        image_topic = rospy.get_param("~image_topic", "")
        depth_image_topic = rospy.get_param("~depth_image_topic", "")
        if_down=False
        self.vfc=0
        
        self._cv_bridge = CvBridge()
        
        # What model to download.
	#MODEL_NAME = 'ssd_mobilenet_v1_coco_11_06_2017'
	#MODEL_NAME='faster_rcnn_resnet101_coco_11_06_2017'
	MODEL_NAME ='ssd_inception_v2_coco_11_06_2017'
	#MODEL_NAME ='diego_object_detection_v1_07_2017'
	#MODEL_NAME ='faster_rcnn_inception_resnet_v2_atrous_coco_11_06_2017'
	MODEL_FILE = MODEL_NAME + '.tar.gz'
	DOWNLOAD_BASE = 'http://download.tensorflow.org/models/object_detection/'

	# Path to frozen detection graph. This is the actual model that is used for the object detection.
	PATH_TO_CKPT = MODEL_NAME + '/frozen_inference_graph.pb'

	# List of the strings that is used to add correct label for each box.
	PATH_TO_LABELS = os.path.join(model_path+'/data', 'mscoco_label_map.pbtxt')
	#PATH_TO_LABELS = os.path.join('data', 'mscoco_label_map.pbtxt')

	NUM_CLASSES = 90
	
	if if_down:
		opener = urllib.request.URLopener()
		opener.retrieve(DOWNLOAD_BASE + MODEL_FILE, MODEL_FILE)
		tar_file = tarfile.open(MODEL_FILE)
		for file in tar_file.getmembers():
			file_name = os.path.basename(file.name)
			if 'frozen_inference_graph.pb' in file_name:
        			tar_file.extract(file, os.getcwd())


	rospy.loginfo("begin initilize the tf...")
	self.detection_graph = tf.Graph()
	with self.detection_graph.as_default():
		od_graph_def = tf.GraphDef()
		with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
			serialized_graph = fid.read()
			od_graph_def.ParseFromString(serialized_graph)
			tf.import_graph_def(od_graph_def, name='')

	label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
	categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
	self.category_index = label_map_util.create_category_index(categories)
	
	# Subscribe to the registered depth image
        rospy.Subscriber(depth_image_topic, ROSImage, self.convert_depth_image)
        
        # Wait for the depth image to become available
        #rospy.wait_for_message('depth_image', ROSImage)
	
	self._sub = rospy.Subscriber(image_topic, ROSImage, self.callback, queue_size=1)	
	self._pub = rospy.Publisher('object_detection', ROSImage_C, queue_size=1)
	
	rospy.loginfo("initialization has finished...")
	
	
    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        # The depth image is a single-channel float32 image
        self.depth_image = self._cv_bridge.imgmsg_to_cv2(ros_image, "32FC1")

        # Convert the depth image to a Numpy array
        self.depth_array = np.array(self.depth_image, dtype=np.float32)
        #print(self.depth_array)
        
    def callback(self,image_msg):
	if self.vfc<12:
		self.vfc=self.vfc+1
	else:
		self.callbackfun(image_msg)
		self.vfc=0	
		    	
    def box_depth(self,boxes,im_width, im_height):
	# Now compute the depth component
        depth=[]
	for row in boxes[0]:
		n_z = sum_z = mean_z = 0
		# Get the min/max x and y values from the ROI
		if row[0]<row[1]:
			min_x = row[0]*im_width
			max_x = row[1]*im_width
		else:
			min_x = row[1]*im_width
			max_x = row[0]*im_width
			
		if row[2]<row[3]:
			min_y = row[2]*im_height
			max_y = row[3]*im_height
		else:
			min_y = row[3]*im_height
			max_y = row[2]*im_height
		# Get the average depth value over the ROI
		for x in range(int(min_x), int(max_x)):
            		for y in range(int(min_y), int(max_y)):
                		try:
					z = self.depth_array[y, x]
				except:
					continue
                
				# Depth values can be NaN which should be ignored
				if isnan(z):
					z=6
					continue
				else:
					sum_z = sum_z + z
					n_z += 1 
			mean_z = sum_z / (n_z+0.01)
		depth.append(mean_z)
	return depth
    def callbackfun(self, image_msg):
	with self.detection_graph.as_default():
		with tf.Session(graph=self.detection_graph) as sess:
			 cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
			 #cv_image = (self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8"))[300:450, 150:380]
			 pil_img = Image.fromarray(cv_image)			 
			 (im_width, im_height) = pil_img.size			 
			 # the array based representation of the image will be used later in order to prepare the
			 # result image with boxes and labels on it.
			 image_np =np.array(pil_img.getdata()).reshape((im_height, im_width, 3)).astype(np.uint8)
			 # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
			 image_np_expanded = np.expand_dims(image_np, axis=0)
			 image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
			 # Each box represents a part of the image where a particular object was detected.
			 boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
			 # Each score represent how level of confidence for each of the objects.
			 # Score is shown on the result image, together with the class label.
			 scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
			 classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
			 num_detections = self.detection_graph.get_tensor_by_name('num_detections:0')
			
			 # Actual detection.
			 (boxes, scores, classes, num_detections) = sess.run(
			 	[boxes, scores, classes, num_detections],
			 	feed_dict={image_tensor: image_np_expanded})
			 box_depths=self.box_depth(boxes,im_width,im_height)
			 print(box_depths)
			 # Visualization of the results of a detection.
			 vis_util.visualize_boxes_and_labels_on_image_array(
			 	image_np,
			 	np.squeeze(boxes),
			 	np.squeeze(classes).astype(np.int32),
			 	np.squeeze(scores),
			 	self.category_index,
                                box_depths,
			 	use_normalized_coordinates=True,
			 	line_thickness=8)
			 
			 ros_compressed_image=self._cv_bridge.cv2_to_compressed_imgmsg(image_np)
			 self._pub.publish(ros_compressed_image)
			
    
    def shutdown(self):
        rospy.loginfo("Stopping the tensorflow object detection...")
        rospy.sleep(1) 
        
if __name__ == '__main__':
    try:
        ObjectDetectionDemo()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("RosTensorFlow_ObjectDetectionDemo has started.")

下面我们来解释主要的代码逻辑

    def __init__(self):
	rospy.init_node('object_detection_demo')
	
	# Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        self.depth_image =None
        
        self.depth_array = None
        
        model_path = rospy.get_param("~model_path", "")
        image_topic = rospy.get_param("~image_topic", "")
        depth_image_topic = rospy.get_param("~depth_image_topic", "")
        if_down=False
        self.vfc=0
        
        self._cv_bridge = CvBridge()

以上代码是ROS的标准初始化代码,变量的初始化,及launch文件中参数的读取

  • model_path定义object_detection所使用的模型路径
  • image_topic订阅的image主题
  • depth_image_topic订阅的深度image主题
        # What model to download.
	#MODEL_NAME = 'ssd_mobilenet_v1_coco_11_06_2017'
	#MODEL_NAME='faster_rcnn_resnet101_coco_11_06_2017'
	MODEL_NAME ='ssd_inception_v2_coco_11_06_2017'
	#MODEL_NAME ='diego_object_detection_v1_07_2017'
	#MODEL_NAME ='faster_rcnn_inception_resnet_v2_atrous_coco_11_06_2017'
	MODEL_FILE = MODEL_NAME + '.tar.gz'
	DOWNLOAD_BASE = 'http://download.tensorflow.org/models/object_detection/'

	# Path to frozen detection graph. This is the actual model that is used for the object detection.
	PATH_TO_CKPT = MODEL_NAME + '/frozen_inference_graph.pb'

	# List of the strings that is used to add correct label for each box.
	PATH_TO_LABELS = os.path.join(model_path+'/data', 'mscoco_label_map.pbtxt')
	#PATH_TO_LABELS = os.path.join('data', 'mscoco_label_map.pbtxt')

	NUM_CLASSES = 90
	
	if if_down:
		opener = urllib.request.URLopener()
		opener.retrieve(DOWNLOAD_BASE + MODEL_FILE, MODEL_FILE)
		tar_file = tarfile.open(MODEL_FILE)
		for file in tar_file.getmembers():
			file_name = os.path.basename(file.name)
			if 'frozen_inference_graph.pb' in file_name:
        			tar_file.extract(file, os.getcwd())

以上代码设置object_detection所使用的模型,及下载解压相应的文件,这里设置了一个if_down的开关,第一次运行的时候可以打开此开关下载,以后可以关掉,因为下载的时间比较长,下载一次后面就无需再下载了。

self.detection_graph = tf.Graph()
	with self.detection_graph.as_default():
		od_graph_def = tf.GraphDef()
		with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
			serialized_graph = fid.read()
			od_graph_def.ParseFromString(serialized_graph)
			tf.import_graph_def(od_graph_def, name='')

	label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
	categories = label_map_util.convert_label_map_to_categories(label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
	self.category_index = label_map_util.create_category_index(categories)

以上代码是tensorflow的初始化代码。

        # Subscribe to the registered depth image
        rospy.Subscriber(depth_image_topic, ROSImage, self.convert_depth_image)
        
        # Wait for the depth image to become available
        #rospy.wait_for_message('depth_image', ROSImage)
	
	self._sub = rospy.Subscriber(image_topic, ROSImage, self.callback, queue_size=1)	
	self._pub = rospy.Publisher('object_detection', ROSImage_C, queue_size=1)

以上代码,我们定义此节点订阅depth_image和image两个topic,同时发布一个名为object_detection的Compressed Imagetopic
depth_image的回调函数是convert_depth_image
image的回调函数是callback

    def convert_depth_image(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        # The depth image is a single-channel float32 image
        self.depth_image = self._cv_bridge.imgmsg_to_cv2(ros_image, "32FC1")

        # Convert the depth image to a Numpy array
        self.depth_array = np.array(self.depth_image, dtype=np.float32)
        #print(self.depth_array)

以上是depth_image处理的回调函数,首先将depth_image主题转换成opencv类型的,然后在将图片转换为numpy数组,赋值给depth_array成员变量

    def callback(self,image_msg):
	if self.vfc<12:
		self.vfc=self.vfc+1
	else:
		self.callbackfun(image_msg)
		self.vfc=0

以上是image处理的回调函数,这里控制视频帧的处理频率,主要是为了减少运算量,可以灵活调整,最终视频帧的处理是在callbackfun中处理的

    def box_depth(self,boxes,im_width, im_height):
	# Now compute the depth component
        depth=[]
	for row in boxes[0]:
		n_z = sum_z = mean_z = 0
		# Get the min/max x and y values from the ROI
		if row[0]<row[1]:
			min_x = row[0]*im_width
			max_x = row[1]*im_width
		else:
			min_x = row[1]*im_width
			max_x = row[0]*im_width
			
		if row[2]<row[3]:
			min_y = row[2]*im_height
			max_y = row[3]*im_height
		else:
			min_y = row[3]*im_height
			max_y = row[2]*im_height
		# Get the average depth value over the ROI
		for x in range(int(min_x), int(max_x)):
            		for y in range(int(min_y), int(max_y)):
                		try:
					z = self.depth_array[y, x]
				except:
					continue
                
				# Depth values can be NaN which should be ignored
				if isnan(z):
					z=6
					continue
				else:
					sum_z = sum_z + z
					n_z += 1 
			mean_z = sum_z / (n_z+0.01)
		depth.append(mean_z)
	return depth

以上代码是深度数据计算,输入参数boxes就是object_detection识别出来的物体的矩形标识rect,我们根据物体的矩形范围,匹配深度图片相应的区域,计算区域内的平均深度值作为此物体的深度数据。返回一个与boxes想对应的1维数组
由于深度图,和一般图片是异步处理,可能出现帧不对应的问题,在这里处理的比较简单,没有考虑此问题,可以通过缓存深度图片的方式来解决,通过时间戳来匹配最近的深度图片。

def callbackfun(self, image_msg):
	with self.detection_graph.as_default():
		with tf.Session(graph=self.detection_graph) as sess:
			 cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
			 #cv_image = (self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8"))[300:450, 150:380]
			 pil_img = Image.fromarray(cv_image)			 
			 (im_width, im_height) = pil_img.size			 
			 # the array based representation of the image will be used later in order to prepare the
			 # result image with boxes and labels on it.
			 image_np =np.array(pil_img.getdata()).reshape((im_height, im_width, 3)).astype(np.uint8)
			 # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
			 image_np_expanded = np.expand_dims(image_np, axis=0)
			 image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
			 # Each box represents a part of the image where a particular object was detected.
			 boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
			 # Each score represent how level of confidence for each of the objects.
			 # Score is shown on the result image, together with the class label.
			 scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
			 classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
			 num_detections = self.detection_graph.get_tensor_by_name('num_detections:0')
			
			 # Actual detection.
			 (boxes, scores, classes, num_detections) = sess.run(
			 	[boxes, scores, classes, num_detections],
			 	feed_dict={image_tensor: image_np_expanded})
			 box_depths=self.box_depth(boxes,im_width,im_height)
			 # Visualization of the results of a detection.
			 vis_util.visualize_boxes_and_labels_on_image_array(
			 	image_np,
			 	np.squeeze(boxes),
			 	np.squeeze(classes).astype(np.int32),
			 	np.squeeze(scores),
			 	self.category_index,
                                box_depths
			 	use_normalized_coordinates=True,
			 	line_thickness=8)
			 
			 ros_compressed_image=self._cv_bridge.cv2_to_compressed_imgmsg(image_np)
			 self._pub.publish(ros_compressed_image)

以上代码是图片的回调函数,主要是将image消息转换为opencv格式,然后再转换成numpy数组,调用object_detection来识别图片中的物体,再调用 vis_util.visualize_boxes_and_labels_on_image_array将识别出来的物体在图片上标识出来,最后将处理后的图片发布为compressed_image类型的消息

现在我们只需要简单修改一下Object_detection/utils目录下的visualization_utils.py文件,就可以显示深度信息

def visualize_boxes_and_labels_on_image_array(image,
                                              boxes,
                                              classes,
                                              scores,
                                              category_index,
	                                      box_depths=None,
                                              instance_masks=None,
                                              keypoints=None,
                                              use_normalized_coordinates=False,
                                              max_boxes_to_draw=20,
                                              min_score_thresh=.5,
                                              agnostic_mode=False,
                                              line_thickness=4):

在visualize_boxes_and_labels_on_image_array定义中增加box_depths=None,缺省值为None

  for i in range(min(max_boxes_to_draw, boxes.shape[0])):
    if scores is None or scores[i] > min_score_thresh:
      box = tuple(boxes[i].tolist())
      if instance_masks is not None:
        box_to_instance_masks_map[box] = instance_masks[i]
      if keypoints is not None:
        box_to_keypoints_map[box].extend(keypoints[i])
      if scores is None:
        box_to_color_map[box] = 'black'
      else:
        if not agnostic_mode:
          if classes[i] in category_index.keys():
            class_name = category_index[classes[i]]['name']
          else:
            class_name = 'N/A'
          display_str = '{}: {}%'.format(
              class_name,
              int(100*scores[i]))
        else:
          display_str = 'score: {}%'.format(int(100 * scores[i]))
          
        #modify by diego robot
        if box_depths!=None:
        	display_str=display_str+"\ndepth: "+str(box_depths[i])
        	global depth_info
        	depth_info=True
        else:
        	global depth_info
        	depth_info=False
        #######################
        box_to_display_str_map[box].append(display_str)
        if agnostic_mode:
          box_to_color_map[box] = 'DarkOrange'
        else:
          box_to_color_map[box] = STANDARD_COLORS[
              classes[i] % len(STANDARD_COLORS)]

在第一个for循环里面,的box_to_display_str_map[box].append(display_str)一句前面增加如上diego robot修改部分代码

depth_info=False

在文件的开头部分定义全局变量,表示是否有深度信息传递进来

 # Reverse list and print from bottom to top.
  for display_str in display_str_list[::-1]: 
    text_width, text_height = font.getsize(display_str)    
    #modify by william
    global depth_info
    if depth_info:
  	text_height=text_height*2
    ###################
    	
    margin = np.ceil(0.05 * text_height)
    draw.rectangle(
        [(left, text_bottom - text_height - 2 * margin), (left + text_width,
                                                          text_bottom)],
        fill=color)
    draw.text(
        (left + margin, text_bottom - text_height - margin),
        display_str,
        fill='black',
        font=font)
    text_bottom -= text_height - 2 * margin

在draw_bounding_box_on_image函数的margin = np.ceil(0.05 * text_height)一句前面增加如上diego robot修改部分代码

3.launch文件

<launch>
   <node pkg="diego_tensorflow" name="ObjectDetectionDemo" type="ObjectDetectionDemo.py" output="screen">

       <param name="image_topic" value="/camera/rgb/image_raw" />

       <param name="depth_image_topic" value="/camera/depth_registered/image" />

       <param name="model_path" value="$(find diego_tensorflow)/scripts/object_detection" />

   </node>

</launch>

launch文件中定义了相应的参数,image_topic,depth_image_topic,model_path,读者可以根据自己的实际情况设定

4.启动节点

启动openni

roslaunch diego_vision openni_node.launch 

启动object_detection

roslaunch diego_tensorflow object_detection_demo.launch

5.通过手机APP订阅object_detection

我们只需要设置一个image_topic为object_detection,既可以在手机上看到物体识别的效果

object_detection物体识别是建立在训练模型上的,本文所采用的是官方所提供的训练好的模型,识别率还是比较高的。但是如果机器人所工作的环境与训练的图片差异比较大,识别率会大大降低。所以我们在实际使用中可以训练自己物体模型,在结合机器人的其他传感器和装置来达到实际应用的要求。比如可以应用在人员流量的监控,车辆的监控,或者特定物体的跟踪,再配合机械臂,深度相机,可以实现物体位置的判断,抓取等功能。

Lesson 25:Diego 1# 4WD —No.3:上位机通讯

ROS Arduino Bridge本质上是上位机通过串口发送控制命令来实现对Arduino的控制,所以我们要实现4驱的控制,我们也必须的修改通讯部分

1.Arduino firmware修改

1.1 command.h

在此文件中增加4驱所需的命令及宏定义

#ifndef COMMANDS_H
#define COMMANDS_H

#define ANALOG_READ    'a'
#define GET_BAUDRATE   'b'
#define PIN_MODE       'c'
#define DIGITAL_READ   'd'
#define READ_ENCODERS  'e'
#define MOTOR_SPEEDS   'm'
#define PING           'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE    's'
#define SERVO_READ     't'
#define UPDATE_PID     'u'
#define DIGITAL_WRITE  'w'
#define ANALOG_WRITE   'x'
#define LEFT            0
#define RIGHT           1
#define LEFT_H          2 //新增
#define RIGHT_H         3 //新增
#define READ_PIDOUT    'f'
#define READ_PIDIN     'i'
#define READ_MPU6050   'g'

#endif

1.2 RosArduinoBridge-diego.ino

此文件是主程序,由于此文件代码比较多,故这里只介绍新增部分代码,完整的代码请见github

在runCommand()函数中修改在4驱模式下读取pidin 的代码

    case READ_PIDIN:
      Serial.print(readPidIn(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidIn(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidIn(RIGHT));
      Serial.print(" ");
      Serial.print(readPidIn(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidIn(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下读取pidout 的代码

    case READ_PIDOUT:
      Serial.print(readPidOut(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidOut(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidOut(RIGHT));
      Serial.print(" ");
      Serial.print(readPidOut(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidOut(RIGHT_H));
#endif       
      break;

在runCommand()函数中修改在4驱模式下读取编码器数据的代码

    case READ_ENCODERS:
      Serial.print(readEncoder(LEFT));
      Serial.print(" ");
#ifdef L298P 
      Serial.println(readEncoder(RIGHT));
#endif
#ifdef L298P_4WD
      Serial.print(readEncoder(RIGHT));
      Serial.print(" ");
      Serial.print(readEncoder(LEFT_H));
      Serial.print(" ");
      Serial.println(readEncoder(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下设置马达速度的代码

    case MOTOR_SPEEDS:
      /* Reset the auto stop timer */
      lastMotorCommand = millis();
      if (arg1 == 0 && arg2 == 0) {
#ifdef L298P        
        setMotorSpeeds(0, 0);
#endif

#ifdef L298P_4WD
        setMotorSpeeds(0, 0, 0, 0);
#endif        
        moving = 0;
      }
      else moving = 1;
      leftPID.TargetTicksPerFrame = arg1;
      rightPID.TargetTicksPerFrame = arg2;
#ifdef L298P_4WD      
      leftPID_h.TargetTicksPerFrame = arg1;
      rightPID_h.TargetTicksPerFrame = arg2;
#endif       
      Serial.println("OK");
      break;

在runCommand()函数中修改在4驱模式下更新PID参数的代码

    case UPDATE_PID:
      while ((str = strtok_r(p, ":", &p)) != '\0') {
        pid_args[i] = atoi(str);
        i++;
      }

      left_Kp = pid_args[0];
      left_Kd = pid_args[1];
      left_Ki = pid_args[2];
      left_Ko = pid_args[3];

      right_Kp = pid_args[4];
      right_Kd = pid_args[5];
      right_Ki = pid_args[6];
      right_Ko = pid_args[7];

#ifdef L298P_4WD

      left_h_Kp = pid_args[0];
      left_h_Kd = pid_args[1];
      left_h_Ki = pid_args[2];
      left_h_Ko = pid_args[3];

      right_h_Kp = pid_args[4];
      right_h_Kd = pid_args[5];
      right_h_Ki = pid_args[6];
      right_h_Ko = pid_args[7];
#endif
      
      Serial.println("OK");
      break;

在setup()函数中设置在4驱模式下新增的pin对应的寄存器的操作

void setup() {
  Serial.begin(BAUDRATE);
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
  //set as inputs
  DDRD &= ~(1 << LEFT_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  DDRD &= ~(1 << LEFT_H_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_H_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif  

  //enable pull up resistors
  PORTD |= (1 << LEFT_ENC_PIN_A);
  PORTD |= (1 << LEFT_ENC_PIN_B);
  PORTC |= (1 << RIGHT_ENC_PIN_A);
  PORTC |= (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  PORTD &= ~(1 << LEFT_H_ENC_PIN_A);
  PORTD &= ~(1 << LEFT_H_ENC_PIN_B);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_A);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif    
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B) | (1 << LEFT_H_ENC_PIN_A) | (1 << LEFT_H_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B) | (1 << RIGHT_H_ENC_PIN_A) | (1 << RIGHT_H_ENC_PIN_B);
#endif
  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
  PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
  initMotorController();
  resetPID();
#endif

  /* Attach servos if used */
#ifdef USE_SERVOS
  int i;
  for (i = 0; i < N_SERVOS; i++) {
    servosPos[i]=90;
  }
  servodriver.begin();
  servodriver.setPWMFreq(50);
#endif
}

在loop()函数中修改在4驱模式下自动停止的逻辑

  // Check to see if we have exceeded the auto-stop interval
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
    ;
#ifdef L298P    
    setMotorSpeeds(0, 0);
#endif
#ifdef L298P_4WD
    setMotorSpeeds(0, 0, 0, 0);
#endif
    moving = 0;
  }

#endif

2.上位机代码修改

上位机的代码修改主要是针对arduino_driver.py和base_controller.py的修改。

2.1 arduino_driver.py修改

修改def get_pidin(self):使其支持4个电机的pidin读取

    def get_pidin(self):
        values = self.execute_array('i')
        print("pidin_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidin was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_pidout(self):使其支持4个电机的pidout读取

    def get_pidout(self):
        values = self.execute_array('f')
        print("pidout_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidout was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_encoder_counts(self):使其支持4个电机的编码器数据读取

    def get_encoder_counts(self):
        values = self.execute_array('e')
        if len(values) not in [2,4]:
            print "Encoder count was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:

修改完后,将arduino的固件更新,就可以启动4驱底盘控制了,要注意的时候要同时打开上位机和arduino上的4驱开关,如果是使用2驱的代码,也要记得关掉4驱的开关。

3.启动4驱底盘控制

现在执行如下命令可以启动4驱底盘控制

roslaunch diego_nav diego_arduino_run.launch 

现在我们可以通过配套的ROS APP连接Diego#进行控制

首先创建一个新的ROS机器人连接,设置相应的ROS Master ip,和cmd_vel主题。

连接上机器人后,可以选择操作杆,和重力感应来对机器人操作

Lesson 24:Diego 1# 4WD —No.2:motor control

针对马达控制我们主要需要修改两部分:

  • 驱动部分,使其可以支持同时控制4个马达
  • PID调速部分,试验表明,虽然是一样的型号的马达,但是给相同的PWM值,马达的转速也不一样,所以我们需要分别对4个马达进行PID调速

1.马达驱动

1.1. DualL298PMotorShield4WD.h文件的修改
增加了4个马达对应的PWM和方向控制的Pin引脚定义,和4个马达速度的设置函数

#ifndef DualL298PMotorShield4WD_h
#define DualL298PMotorShield4WD_h

#include 

class DualL298PMotorShield4WD
{
  public:  
    // CONSTRUCTORS
    DualL298PMotorShield4WD(); // Default pin selection.
    
    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setM3Speed(int speed); // Set speed for M3.
    void setM4Speed(int speed); // Set speed for M4.
    void setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed); // Set speed for both M1 and M2.
    
  private:
  
    // left motor
    static const unsigned char _M1DIR = 12;
    static const unsigned char _M2DIR = 7;
    static const unsigned char _M1PWM = 10;
    static const unsigned char _M2PWM = 6;
    
    // right motor
    static const unsigned char _M4DIR = 8;
    static const unsigned char _M3DIR = 13;
    static const unsigned char _M4PWM = 9;
    static const unsigned char _M3PWM = 11;
    
};

#endif

1.2. DualL298PMotorShield4WD.cpp文件的修改

主要是针对4个马达速度设置函数的实现,逻辑都是一样的,这里只截取一个马达的代码,代码的逻辑请看代码注释

// Set speed for motor 4, speed is a number betwenn -400 and 400
void DualL298PMotorShield4WD::setM4Speed(int speed)
{
  unsigned char reverse = 0;
  
  if (speed < 0) //速度是否小于0 { speed = -speed; // 如果小于0,则是反转 reverse = 1; // 反转标志变量设置为1 } if (speed > 255)  // 限定最大速度为255
    speed = 255;
  if (reverse)  //反转状态下
  {
    digitalWrite(_M4DIR,LOW);//设定马达转向的pin位低电平
    analogWrite(_M4PWM, speed);
  }
  else //正向转动状态下
  {
    digitalWrite(_M4DIR,HIGH);设定马达转向的pin位高电平
    analogWrite(_M4PWM, speed);
  }
}

在setSpeeds函数中分别调用4个马达的速度设置函数。

// Set speed for motor 1, 2, 3, 4
void DualL298PMotorShield4WD::setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
  setM3Speed(m3Speed);
  setM4Speed(m4Speed);  
}

DualL298PMotorShield4WD.h和DualL298PMotorShield4WD.cpp修改完成后,在arduino 的library目录下新建一个名为dual-L298P-motor-shield-master-4wd的目录,将两个文件放到此文件夹下

现在我们打开android ide的library就可以看到我们刚才添加的库

1.1.3 motor_driver.h的修改

增加4驱马达控制的函数setMotorSpeeds,参数为4个马达的速度

void initMotorController();
void setMotorSpeed(int i, int spd);
#ifdef L298P
void setMotorSpeeds(int leftSpeed, int rightSpeed);
#endif
#ifdef L298P_4WD
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2);
#endif

1.1.4 motor_driver.ino的修改

#ifdef L298P_4WD
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2){
  setMotorSpeed(1, leftSpeed_1);
  setMotorSpeed(3, rightSpeed_1);
  setMotorSpeed(2, leftSpeed_2);
  setMotorSpeed(4, rightSpeed_2);
}
#endif
#else
#error A motor driver must be selected!
#endif

2.PID控制

PID控制都在diff_controller.h文件中定义修改

新增两个在4驱模式下的PID控制变量

#ifdef L298P_4WD
SetPointInfo leftPID_h, rightPID_h;
#endif

新增两个在4驱模式下的PID控制参数

#ifdef L298P_4WD

int left_h_Kp=Kp;
int left_h_Kd=Kd;
int left_h_Ki=Ki;
int left_h_Ko=Ko;

int right_h_Kp=Kp;
int right_h_Kd=Kd;
int right_h_Ki=Ki;
int right_h_Ko=Ko;

#endif

新增两个在4驱模式下的PID控制函数

#ifdef L298P_4WD
/* PID routine to compute the next motor commands */
void dorightPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input =  p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (right_h_Kp * Perror - right_h_Kd * (input - p->PrevInput) + p->ITerm) / right_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("right output:");
//  Serial.println(p->output);
}
#endif
...
#ifdef L298P_4WD

/* PID routine to compute the next motor commands */
void doleftPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (left_h_Kp * Perror - left_h_Kd * (input - p->PrevInput) + p->ITerm) / left_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("left output:");
//  Serial.println(p->output);
}
#endif

修改readPIDIn函数,增加在4驱模式下,pidin的读取

long readPidIn(int i) {
  long pidin=0;
  if (i == LEFT){
    pidin = leftPID.PrevInput;
  }else if (i == RIGHT){
    pidin = rightPID.PrevInput;
  }
#ifdef L298P_4WD
  else if (i== RIGHT_H){
    pidin = rightPID_h.PrevInput;
  }else{
    pidin = leftPID_h.PrevInput;
  }
#endif  
  return pidin;
}

修改readPIDOut函数,增加在4驱模式下,pidOut的读取

long readPidOut(int i) {
  long pidout=0;
  if (i == LEFT){
    pidout = leftPID.output;
  }else if (i == RIGHT){
    pidout = rightPID.output;
  }
#ifdef L298P_4WD
  else if (i == RIGHT_H){
    pidout = rightPID_h.output;
  }else{
    pidout = leftPID_h.output;
  }
#endif   
  return pidout;
}

修改updatePID函数,增加在4驱模式下马达PID控制的调用

void updatePID() {
  /* Read the encoders */
  leftPID.Encoder =readEncoder(LEFT);
  rightPID.Encoder =readEncoder(RIGHT);
#ifdef L298P_4WD 
  leftPID_h.Encoder =readEncoder(LEFT_H);
  rightPID_h.Encoder =readEncoder(RIGHT_H);
#endif
  
  /* If we're not moving there is nothing more to do */
  if (!moving){
    /*
    * Reset PIDs once, to prevent startup spikes,
    * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
    * PrevInput is considered a good proxy to detect
    * whether reset has already happened
    */
#ifdef L298P    
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
#endif
#ifdef L298P_4WD
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0 || leftPID_h.PrevInput != 0 || rightPID_h.PrevInput != 0) resetPID();
#endif    
    return;
  }

  /* Compute PID update for each motor */
  dorightPID(&rightPID);
  doleftPID(&leftPID);
#ifdef L298P_4WD
  dorightPID_h(&rightPID_h);
  doleftPID_h(&leftPID_h);
#endif  

  /* Set the motor speeds accordingly */
#ifdef L298P   
  setMotorSpeeds(leftPID.output, rightPID.output);
#endif

#ifdef L298P_4WD
  setMotorSpeeds(leftPID.output,leftPID_h.output, rightPID.output,rightPID_h.output);
#endif

}

至此4个马达已经可以独立驱动,独立的PID调速了,在下一篇教程中,会介绍如和上位机互动起来,让4驱底盘跑起来。

Lesson 23:Diego 1# 4WD —NO.1:ENCODER

4驱底盘由于四个轮子可以独立控制,所以具有优秀的通过性,这篇文章介绍Diego 1#的四驱底盘,所有源代码都已经上传到github。

这里会分几篇文章来介绍4驱动版diego 1#的开发,这篇我们主要说明4驱动底盘4个马达编码器数据的读取。

1.1硬件说明

  • 底盘材质:铝合金材质
  • 轮胎:12cm 橡胶轮胎
  • 电机:370直流电机,带霍尔码盘测速,输出AB项编码信号

1.2 控制器

  • 主控制器 arduino UNO,使用uno分别对4个电机进行方向,PWM控制,并采用终端方式采集4个电机输出的编码信号
  • 电机控制器 两块L298p, 网上采购,控制引脚不同
  • 上位机:树莓派,或者mini pc

2.编码器数据读取

所以代码都基于diego 1# github上的代码进行修改,这里只说明4驱版本部分的代码

首先我们在ROSAduinoBridge_diego.ino文件中定义一个预编译符号,这个预编译符号可以启动或者关闭4驱的代码,这样方便我们开关4驱的功能

#define L298P_4WD

在diego 1 4wd中实现了使用arduino uno读取4个马达的AB项编码输出,一般的网上说明中arduino uno只有两个外部中断,好像只能读取一个马达的AB项编码输出,但事实上arudino中所有IO引脚都可以作为中断使用,通过操作中断寄存器的方式。

首先我们在encoder_driver.h文件中定义编码器连接的Arduino uno引脚:

#ifdef ARDUINO_ENC_COUNTER
  //below can be changed, but should be PORTD pins; 
  //otherwise additional changes in the code are required
  #define LEFT_ENC_PIN_A PD2  //pin 2
  #define LEFT_ENC_PIN_B PD3  //pin 3
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_ENC_PIN_A PC2  //pin A2
  #define RIGHT_ENC_PIN_B PC3   //pin A3

#ifdef L298P_4WD
  #define LEFT_H_ENC_PIN_A PD4  //pin 4
  #define LEFT_H_ENC_PIN_B PD5  //pin 5
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_H_ENC_PIN_A PC0  //pin A0
  #define RIGHT_H_ENC_PIN_B PC1   //pin A1
#endif  
#endif

从此文件中可以看到在原来2驱的基础上增加了4WD模式下的编码器连接引脚定义,其中左后方的电机AB项连接数字IO的D4和D5,而右后方电机AB项连接模拟IO的A0和A1,这样4个马达的编码器数据读取我们就用到8个IO口,加上PWM控制,转动方向的控制,在Diego 1# 4WD版本中,底盘控制一共用了16个IO,最后只剩D0,D1作为串口和上位机通讯,和A4,A5作为I2C的接口与I2C接口模块通讯,可以说Arduino UNO做到了充分利用。

在encoder_driver.ino文件中增加对4WD新增引脚的中断处理。

arduino uno中一共有3个引脚中断函数分别是

  • ISR (PCINT0_vect)对应 D8 to D13
  • ISR (PCINT1_vect) 对应 A0 to A5
  • ISR (PCINT2_vect) 对应 D0 to D7

diego1# 4wd中只需要用到两个中断处理函数 ISR(PCINT2_vect)和ISR(PCINT1_vect),代码如下:

  ISR (PCINT2_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     static uint8_t enc_last_h=0;
#endif          
     enc_last <<=2; //shift previous state two places
     enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PIND & (3 << 4))>>4;
#endif 
  
     left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     left_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif    
  }
  
  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
  ISR (PCINT1_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     //uint8_t pinct=PINC;
     static uint8_t enc_last_h=0;
#endif   	
     enc_last <<=2; //shift previous state two places
     enc_last |= (PINC & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PINC & 3);
#endif  
     right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     right_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif
  }

这段代码中主要是针对中断寄存器PINC和PIND的操作,每个IO pin都对应PINC或者PIND的一位,IO有中断产生时,对应的位就会被置位,我们只需要读取相应为即可,如(PIND & (3 << 4))>>4读取的就是PIND的第4,5位,也就是D4,D5的数据。

Lesson 22:ar_track_alvar

ROS里面有一个非常好用的AR标签包,可以产生AR标签,识别AR标签。我们可以基于此功能实现很多好玩的AR应用,这篇文章中我们将介绍如何使用这个包,及基于此包我们实现AR标签的跟随。

1.安装ar_track_alvar

ar_track_alvar可以通过apt_get来安装,非常方便:

sudo apt-get install ros-kinetic-ar-track-alvar

安装完成后暨在/opt/ros/kinetic/share/目录下有ar_track_alvar的目录,而且在launch子目录下已经有了launch文件。

如果你ROS一开始就是安装的完整版本的话,这个包就已经安装好了,不需要单独安装。

2.生成ar tag标签

ar_track_alvar提供的ar标签的生成功能,我们首先进入存放标签文件的目录

cd ~/diego1/src/diego_ar_tags/data

接下来执行执行生成标签的命令,就会在此文件夹下生成对应的标签,一条命令只生成一个标签

rosrun ar_track_alvar createMarker 0
rosrun ar_track_alvar createMarker 1
rosrun ar_track_alvar createMarker 2
rosrun ar_track_alvar createMarker 3
rosrun ar_track_alvar createMarker 4

这时候我们查看目录就会看到生成的ar文件,是png格式的图片

双击就可以打开文件

可以把标签答应出来,在跟随的时候使用。

3.ar标签跟随

3.1 源代码

下面是ar_follower.py的源文件,是从rbx2的项目中提取出来的,可以直接使用

#!/usr/bin/env python

"""
    ar_follower.py - Version 1.0 2013-08-25
    
    Follow an AR tag published on the /ar_pose_marker topic.  The /ar_pose_marker topic
    is published by the ar_track_alvar package
    
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2013 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
"""

import rospy
from ar_track_alvar_msgs.msg import AlvarMarkers
from geometry_msgs.msg import Twist
from math import copysign

class ARFollower():
    def __init__(self):
        rospy.init_node("ar_follower")
                        
        # Set the shutdown function (stop the robot)
        rospy.on_shutdown(self.shutdown)
        
        # How often should we update the robot's motion?
        self.rate = rospy.get_param("~rate", 10)
        r = rospy.Rate(self.rate) 
        
        # The maximum rotation speed in radians per second
        self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)
        
        # The minimum rotation speed in radians per second
        self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.5)
        
        # The maximum distance a target can be from the robot for us to track
        self.max_x = rospy.get_param("~max_x", 20.0)
        
        # The goal distance (in meters) to keep between the robot and the marker
        self.goal_x = rospy.get_param("~goal_x", 0.6)
        
        # How far away from the goal distance (in meters) before the robot reacts
        self.x_threshold = rospy.get_param("~x_threshold", 0.05)
        
        # How far away from being centered (y displacement) on the AR marker
        # before the robot reacts (units are meters)
        self.y_threshold = rospy.get_param("~y_threshold", 0.05)
        
        # How much do we weight the goal distance (x) when making a movement
        self.x_scale = rospy.get_param("~x_scale", 0.5)

        # How much do we weight y-displacement when making a movement        
        self.y_scale = rospy.get_param("~y_scale", 1.0)
        
        # The max linear speed in meters per second
        self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)
        
        # The minimum linear speed in meters per second
        self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)

        # Publisher to control the robot's movement
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # Intialize the movement command
        self.move_cmd = Twist()
        
        # Set flag to indicate when the AR marker is visible
        self.target_visible = False
        
        # Wait for the ar_pose_marker topic to become available
        rospy.loginfo("Waiting for ar_pose_marker topic...")
        rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
        
        # Subscribe to the ar_pose_marker topic to get the image width and height
        rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.set_cmd_vel)
        
        rospy.loginfo("Marker messages detected. Starting follower...")
        
        # Begin the cmd_vel publishing loop
        while not rospy.is_shutdown():
            # Send the Twist command to the robot
            self.cmd_vel_pub.publish(self.move_cmd)
            
            # Sleep for 1/self.rate seconds
            r.sleep()

    def set_cmd_vel(self, msg):
        # Pick off the first marker (in case there is more than one)
        try:
            marker = msg.markers[0]
            if not self.target_visible:
                rospy.loginfo("FOLLOWER is Tracking Target!")
            self.target_visible = True
        except:
            # If target is loar, stop the robot by slowing it incrementally
            self.move_cmd.linear.x /= 1.5
            self.move_cmd.angular.z /= 1.5
            
            if self.target_visible:
                rospy.loginfo("FOLLOWER LOST Target!")
            self.target_visible = False
            
            return
                
        # Get the displacement of the marker relative to the base
        target_offset_y = marker.pose.pose.position.y
        rospy.loginfo("target_offset_y"+str(target_offset_y))
        
        # Get the distance of the marker from the base
        target_offset_x = marker.pose.pose.position.x
        rospy.loginfo("target_offset_x"+str(target_offset_x))
        
        # Rotate the robot only if the displacement of the target exceeds the threshold
        if abs(target_offset_y) > self.y_threshold:
            # Set the rotation speed proportional to the displacement of the target
            speed = target_offset_y * self.y_scale
            self.move_cmd.angular.z = copysign(max(self.min_angular_speed,
                                        min(self.max_angular_speed, abs(speed))), speed)
        else:
            self.move_cmd.angular.z = 0.0
 
        # Now get the linear speed
        if abs(target_offset_x - self.goal_x) > self.x_threshold:
            speed = (target_offset_x - self.goal_x) * self.x_scale
            if speed < 0:
                speed *= 1.5
            self.move_cmd.linear.x = copysign(min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed)
        else:
            self.move_cmd.linear.x = 0.0

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)     

if __name__ == '__main__':
    try:
        ARFollower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("AR follower node terminated.")

3.2代码解释

这段代码的原理其实非常简单,就是调用ar_track_alvar发布的/ar_pose_marker主题,然后将该主题的信息转换为cmd_vel主题的控制信息来控制机器人运动。我们先来看下/ar_pose_maker主题

上图执行rostopic echo /ar_pose_marker命令的截图,从中可以看出此topic中包含着一个Pose类型的消息,我们只需要处理position部分的x,y,z的值就可以了,其中x就是机器人到ar 标签的距离,y值就是相对于与摄像头中心位置的偏移,理解了ar_pose_marker我们就很容易理解代码的原理,这里就不多做叙述了。

3.3 launch文件

ar_large_markers_xition.launch文件启动ar_track_alvar node

<launch>
     <arg name="marker_size" default="12.5" />
     <arg name="max_new_marker_error" default="0.08" /> 
     <arg name="max_track_error" default="0.4" />
     <arg name="cam_image_topic" default="/camera/depth_registered/points" />
     <arg name="cam_info_topic" default="/camera/rgb/camera_info" /> 
     <arg name="output_frame" default="/base_link" />
     <arg name="debug" default="false" />
     <arg if="$(arg debug)" name="launch_prefix" value="xterm -e gdb --args" />
     <arg unless="$(arg debug)" name="launch_prefix" value="" />
     <node pkg="tf" type="static_transform_publisher" name="base_link_2_camera_link" args="0.0 0.0 0.2 0 0 0 /base_link /camera_link 40"/> 
     <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" launch-prefix="$(arg launch_prefix)" /> <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" launch-prefix="$(arg launch_prefix)" />
</launch>

diego_ar_follower.launch 文件启动 diego_ar_follower node,跟随的参数可以在这个文件中设置。

<launch>

     <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
         <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
     </node>

     <node pkg="diego_ar_tags" name="ar_follower" type="ar_follower.py" clear_params="true" output="screen">
         <rosparam>
             rate: 10
             max_x: 20.0
             goal_x: 0.7
             x_threshold: 0.1
             y_threshold: 0.05
             y_scale: 2.0
             x_scale: 1.0
             max_angular_speed: 1.0
             min_angular_speed: 0.2
             max_linear_speed: 0.2
             min_linear_speed: 0.05
         </rosparam>

    </node>
</launch>

4.启动节点

启动摄像头,在diego中使用xtion深度相机,所以在这里我们首先启动openni

roslaunch diego_vision openni_node.launch

启动ar_track_alvar节点

roslaunch diego_ar_tags ar_large_markers_xition.launch

启动diego_ar_follower节点

roslaunch diego_ar_tags diego_ar_follower.launch

现在只需要拿着我们事先打印好的ar tag在Xtion相机前移动,机器人就会跟着你走。

 

 

Posts navigation

1 2 3 4