基于ROS的机器人设计

(1)机械部分

嘿嘿 不是我设计的 这就不写了

软件方面的逻辑

串口通信

CAN通信

CAN通信

CAN通信

控制

控制

控制

ROS上位机

stm32主机

stm32从机1

stm32从机2

stm32从机N

设备1

设备2

设备N

(2)ROS系统与stm32串口通信

这个部分参考的是公众号《小白学移动机器人》
/---------------------------------以下ROS部分---------------------------------------------
ROS的部分

数据结构

union sendData
{
	short d;
	unsigned char data[2];
}leftVelSet,rightVelSet;

//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union receiveData
{
	short d;
	unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;

数据收发

    leftVelSet.d  = Left_v;//mm/s
    rightVelSet.d = Right_v;

    // 设置消息头
    for(i = 0; i < 2; i++)
        buf[i] = header[i];             //buf[0]  buf[1]
    
    // 设置机器人左右轮速度
    length = 5;
    buf[2] = length;                    //buf[2]
    for(i = 0; i < 2; i++)
    {
        buf[i + 3] = leftVelSet.data[i];  //buf[3] buf[4]
        buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
    }
    // 预留控制指令
    buf[3 + length - 1] = ctrlFlag;       //buf[7]

    // 设置校验值、消息尾
    buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
    buf[3 + length + 1] = ender[0];     //buf[9]
    buf[3 + length + 2] = ender[1];     //buf[10]

主体节点

	数据已经读取到了之后，如果要发布出去 直接定义一个新的消息数据类型
	或者直接套用合适的也行。
	
	直接pub.publish（收到的信息）

int main(int agrc,char **argv)
{
    ros::init(agrc,argv,"public_node");
    ros::NodeHandle nh;

    ros::Rate loop_rate(10);
    
    //串口初始化
    serialInit();

    while(ros::ok())
    {
        ros::spinOnce();
        //向STM32端发送数据，
        //前两个为double类型，最后一个为unsigned char类型
	    writeSpeed(testSend1,testSend2,testSend3);
        
        //从STM32接收数据
        //输入参数转化为小车的线速度、角速度、航向角（角度）、预留控制位
	    readSpeed(testRece1,testRece2,testRece3,testRece4);
        //打印数据
	   
	
        loop_rate.sleep();
    }
    return 0;
}

串口初始化函数

void serialInit()
{
    sp.set_option(serial_port::baud_rate(115200));
    sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
    sp.set_option(serial_port::parity(serial_port::parity::none));
    sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
    sp.set_option(serial_port::character_size(8));    
}

/-----------------------------------以上是ROS部分----------------------------------------

/---------------------------------以下是stm32部分----------------------------------------
串口接收中断

接收ROS系统下发的指令 设计为目标机器人x和y 的速度  （第三位保留）

void USART1_IRQHandler()
{
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
 	 {
		 USART_ClearITPendingBit(USART1,USART_IT_RXNE);
		 
		 //接收ROS的数据
		 usartReceiveOneData(&testRece1,&testRece2,&testRece3);
	 }
}

串口发送

通过stm32的 USARTx->SR 正常发送就好了

    leftVelNow.d  = leftVel;
	rightVelNow.d = rightVel;
	angleNow.d    = angle;
	
	// 设置消息头
	for(i = 0; i < 2; i++)
		buf[i] = header[i];                      // buf[0] buf[1] 
	
	// 设置机器人左右轮速度、角度
	length = 7;
	buf[2] = length;                             // buf[2]
	for(i = 0; i < 2; i++)
	{
		buf[i + 3] = leftVelNow.data[i];         // buf[3] buf[4]
		buf[i + 5] = rightVelNow.data[i];        // buf[5] buf[6]
		buf[i + 7] = angleNow.data[i];           // buf[7] buf[8]
	}
	// 预留控制指令
	buf[3 + length - 1] = ctrlFlag;              // buf[9]
	
	// 设置校验值、消息尾
	buf[3 + length] = getCrc8(buf, 3 + length);  // buf[10]
	buf[3 + length + 1] = ender[0];              // buf[11]
	buf[3 + length + 2] = ender[1];              // buf[12]
	
	//发送字符串数据
	USART_Send_String(buf,sizeof(buf));

/---------------------------------以上是stm32部分----------------------------------------

(3)stm32控制电机及CAN通信

由于设备众多，所以直接一个stm32控制一个电机 相互之间通过can总线通信。

（如果只是小车的话直接一块stm32就可以控制4个轮子或者是舵机了）

主机与从机的通信
主机负责接收所有从机的数据，不需要过滤，用id号区分不同从机的数据；

switch（id号）然后分别处理即可

主机还可以向不同从机发送信息。而从机则只接收来自主机的数据。
即：过滤掉除主机id以外的所有id

具体原理：https://blog.csdn.net/qq_36355662/article/details/80607453

//要过滤的ID高位 
CAN_FilterInitStructure.CAN_FilterIdHigh=0X00;  
//要过滤的ID低位                 
CAN_FilterInitStructure.CAN_FilterIdLow= (((u32)0x1314<<3)|CAN_ID_EXT|CAN_RTR_DATA)&0xFFFF; 

//过滤器屏蔽标识符的高16位值
CAN_FilterInitStructure.CAN_FilterMaskIdHigh=0xFFFF;   
//过滤器屏蔽标识符的低16位值         
CAN_FilterInitStructure.CAN_FilterMaskIdLow=0xFFFF; 

这里的CAN_FilterId和CAN_FilterMaskId是配合使用的，意思是CAN_FilterId指出需要屏蔽ID的什么内容，什么格式；CAN_FilterMaskId是指CAN_FilterId的每一位是否需要过滤，若CAN_FilterMaskId在某位上是1的话，ID对应位上的数值就必须和CAN_FilterId该位上的一样，保持一致，反之则是“不关心”。上述程序的设置的含义就是：只接收来自0x1314的数据，屏蔽其他ID的数据。

can总线的收发代码

主机就不用过滤
从机就如上文配置滤波

#if CAN_RX0_INT_ENABLE	//使能RX0中断
//中断服务函数			    
void USB_LP_CAN1_RX0_IRQHandler(void)
{
  	CanRxMsg RxMessage;
	int i=0;
    CAN_Receive(CAN1, 0, &RxMessage);
	for(i=0;i<8;i++)
	printf("rxbuf[%d]:%d\r\n",i,RxMessage.Data[i]);
}
#endif

//can发送一组数据(固定格式:ID为0X12,标准帧,数据帧)	
//len:数据长度(最大为8)				     
//msg:数据指针,最大为8个字节.
//返回值:0,成功;
//		 其他,失败;
u8 Can_Send_Msg(u8* msg,u8 len)
{	
  u8 mbox;
  u16 i=0;
  CanTxMsg TxMessage;
  TxMessage.StdId=0x12;					 // 标准标识符 
  TxMessage.ExtId=0x12;				   // 设置扩展标示符 
  TxMessage.IDE=CAN_Id_Standard; // 标准帧
  TxMessage.RTR=CAN_RTR_Data;		 // 数据帧
  TxMessage.DLC=len;						// 要发送的数据长度
  for(i=0;i<len;i++)
  TxMessage.Data[i]=msg[i];			          
  mbox= CAN_Transmit(CAN1, &TxMessage);   
  i=0;
  //等待发送结束
  while((CAN_TransmitStatus(CAN1, mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++;	
  if(i>=0XFFF)return 1;
  return 0;		

}
//can口接收数据查询
//buf:数据缓存区;	 
//返回值:0,无数据被收到;
//		 其他,接收的数据长度;
u8 Can_Receive_Msg(u8 *buf)
{		   		   
 	u32 i;
	CanRxMsg RxMessage;
    //没有接收到数据,直接退出 
    if( CAN_MessagePending(CAN1,CAN_FIFO0)==0)return 0;		
    CAN_Receive(CAN1, CAN_FIFO0, &RxMessage);//读取数据	
    for(i=0;i<8;i++)
    buf[i]=RxMessage.Data[i];  
	return RxMessage.DLC;	
}

单个stm32控制电机那就非常简单了 不值一提

(4)ROS系统的机器视觉以及神经网络

启动摄像头 再启动检测节点

sudo apt-get install ros-melodic-usb-cam

<launch>
  
  <include file="$(find usb_cam)/launch/usb_cam-test.launch" />
  
  <node pkg="ros_detection" name="ros_tensorflow_detect"  type="ros_tensorflow_detect.py" output="screen"> 
  node>

launch>

python的节点代码
订阅usb摄像头的图像话题 然后开始视觉识别
直接调用TensorFlow与ROS的API

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge
import cv2
import numpy as np
#import tensorflow as tf
import tensorflow.compat.v1 as tf
tf.disable_v2_behavior()
import os
import re


#cv2.namedWindow('class',cv2.WINDOW_NORMAL)

class RosTensorFlow():
    def __init__(self):
       
        self._session = tf.Session()
        self._cv_bridge = CvBridge()

        self._sub = rospy.Subscriber('usb_cam/image_raw', Image, self.callback, queue_size=1)
        self._pub = rospy.Publisher('/result_ripe', String, queue_size=1)
        self.score_threshold = rospy.get_param('~score_threshold', 0.1)
        self.use_top_k = rospy.get_param('~use_top_k', 5)

    def load(self, label_lookup_path, uid_lookup_path):

        if not tf.gfile.Exists(uid_lookup_path):
          tf.logging.fatal('File does not exist %s', uid_lookup_path)
        if not tf.gfile.Exists(label_lookup_path):
          tf.logging.fatal('File does not exist %s', label_lookup_path)

        # Loads mapping from string UID to human-readable string
        proto_as_ascii_lines = tf.gfile.GFile(uid_lookup_path).readlines()
        uid_to_human = {}
        p = re.compile(r'[n\d]*[ \S,]*')
        for line in proto_as_ascii_lines:
            parsed_items = p.findall(line)
            uid = parsed_items[0]
            human_string = parsed_items[2]
            uid_to_human[uid] = human_string

        # Loads mapping from string UID to integer node ID.
        node_id_to_uid = {}
        proto_as_ascii = tf.gfile.GFile(label_lookup_path).readlines()
        for line in proto_as_ascii:
            if line.startswith('  target_class:'):
                target_class = int(line.split(': ')[1])
            if line.startswith('  target_class_string:'):
                target_class_string = line.split(': ')[1]
                node_id_to_uid[target_class] = target_class_string[1:-2]

        # Loads the final mapping of integer node ID to human-readable string
        node_id_to_name = {}
        for key, val in node_id_to_uid.items():
            if val not in uid_to_human:
                tf.logging.fatal('Failed to locate: %s', val)
            name = uid_to_human[val]
            node_id_to_name[key] = name

        return node_id_to_name
    def callback(self, image_msg):
        cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, "bgr8")
        image_data = cv2.imencode('.jpg', cv_image)[1].tostring()
        #cv2.imshow("class", cv_image)
        pil_img = cv_image
        im_width = pil_img.shape[0]
        im_height = pil_img.shape[1]
        # Creates graph from saved GraphDef.
        softmax_tensor = self._session.graph.get_tensor_by_name('softmax:0')
        predictions = self._session.run(
            softmax_tensor, {'DecodeJpeg/contents:0': image_data})
        predictions = np.squeeze(predictions)
        count = 0
        for i in range (100):
            if predictions is None or predictions[i]>0.5:
                count= count+1
        for i in range (count):
            #  print(boxes[0][i][-1])
            y_min = boxes[0][i][0]*im_height
            x_min = boxes[0][i][1]*im_width
            y_max = boxes[0][i][2]*im_height
            x_max = boxes[0][i][3]*im_width
            cv2.rectangle(cv_image,x_min,y_max,(0,255,0),2)
        # Creates node ID --> English string lookup.
        node_lookup = self.load(PATH_TO_LABELS, PATH_TO_UID)
        top_k = predictions.argsort()[-self.use_top_k:][::-1]
        for node_id in top_k:
            
            if node_id not in node_lookup:
                human_string = ''
            else:
                human_string = node_lookup[node_id]

            score = predictions[node_id]
            if score > self.score_threshold:
                rospy.loginfo('%s (score = %.5f)' % (human_string, score))
                self._pub.publish(human_string)
        cv2.waitKey(3)


    def main(self):
        rospy.spin()

if __name__ == '__main__':
    
    ROOT_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), os.pardir))
    PATH_TO_CKPT = ROOT_PATH + '/include/classifier/classify_image_graph_def.pb'
    PATH_TO_LABELS = ROOT_PATH + '/include/classifier/imagenet_2012_challenge_label_map_proto.pbtxt'
    PATH_TO_UID = ROOT_PATH + '/include/classifier/imagenet_synset_to_human_label_map.txt'

    with tf.gfile.FastGFile(PATH_TO_CKPT, 'rb') as f:
    	graph_def = tf.GraphDef()
    	graph_def.ParseFromString(f.read())
    	_ = tf.import_graph_def(graph_def, name='')    
    
    rospy.init_node('ros_tensorflow_classify')
    tensor = RosTensorFlow()
    tensor.main()

还有一些功能，比如提前车道线，人脸识别，颜色识别都是一样的逻辑，就是代码中的API接口有点差异，不赘述了。

(5)ROS系统的导航与定位

建图用gmapping或Cartographer等等算法都行

gampping算法的使用 其他的也差不多
但是gampping虽然没有直接调用里程计，但是应该是比较依赖里程计的。

<launch>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      
      <param name="base_frame" value="base_link"/>

      <param name="odom_frame" value="odom"/> 
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    node>

    
    
    
launch>

move_base功能包直接规划出路线

具体到话题就是cmd_vel
这个功能包我比较关注的功能之一就是：
计算出我的机器人以什么样的速度运行达到我设定的目标点

<launch>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
      <remap from="scan" to="scan"/>
      
      <param name="base_frame" value="base_link"/>

      <param name="odom_frame" value="odom"/> 
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <param name="sigma" value="0.05"/>
      <param name="kernelSize" value="1"/>
      <param name="lstep" value="0.05"/>
      <param name="astep" value="0.05"/>
      <param name="iterations" value="5"/>
      <param name="lsigma" value="0.075"/>
      <param name="ogain" value="3.0"/>
      <param name="lskip" value="0"/>
      <param name="srr" value="0.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.0"/>
      <param name="delta" value="0.05"/>
      <param name="llsamplerange" value="0.01"/>
      <param name="llsamplestep" value="0.01"/>
      <param name="lasamplerange" value="0.005"/>
      <param name="lasamplestep" value="0.005"/>
    node>
 
    
    
launch>

如果需要在室外和一些复杂条件下导航以及运行，那么就需要一些比较复杂的功能包，我也做了一些补充。

功能包应用积累

这些功能在不考虑优化算法的情况下，只需调用功能包，寻找API接口即可