ROS与STM32通信(二)-pyserial

ROS与STM32通信一般分为两种，

• STM32上运行ros节点实现通信
• 使用普通的串口库进行通信，然后以话题方式发布

第一种方式具体实现过程可参考上篇文章ROS与STM32通信-rosserial，上述文章中的收发频率不一致情况，目前还没解决，所以本篇文章采用第二种方式来实现STM32与ROS通信，C++实现方式可参看这篇文章ROS与STM32通信,其利用ros serial库数据格式为C/C++共用体实现解析与发布。Python实现方式可使用pyserial库来实现通信，pyserial的用法可参考我之前写的文章python与stm32通信,数据格式我们采用Json格式来解析与发布。

以STM32读取MPU6050，然后ROS发布与订阅为例

下位机

参考之前写的文章STM32HAL库驱动MPU6050

main.c

while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
      while (mpu_dmp_get_data(&pitch, &roll, &yaw));    //必须要用while等待，才能读取成功

      printf("{\"roll\":%.4f,\"pitch\":%.4f,\"yaw\":%.4f}",roll, pitch, yaw); //Json字符串发送
      sprintf(oledBuf, "roll :%.2f", roll);
      OLED_ShowString(0, 28, (u8*)oledBuf, 12);
      sprintf(oledBuf, "pitch:%.2f", pitch);
      OLED_ShowString(0, 40, (u8*)oledBuf, 12);
      sprintf(oledBuf, "yaw  :%.2f", yaw);
      OLED_ShowString(0, 52, (u8*)oledBuf, 12);
      OLED_Refresh();
  }

使用printf重定向发送json字符串，注意C语言转义字符：

printf("{\"roll\":%.4f,\"pitch\":%.4f,\"yaw\":%.4f",roll, pitch, yaw); //Json字符串发送

可使用cutecom查看发送的消息

上位机

自定义msg消息

在功能包下新建文件夹为msg

新建文件Imu.msg（首字母大写），输入以下内容

float32 pitch
float32 roll
float32 yaw

package.xml添加依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeList.txt编辑msg相关配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)


## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Imu.msg
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES hello_vscode
 CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

然后编译整个工作空间catkin_make

Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/msg)

vscode配置

将前面生成的 python 文件路径配置进settings.json

{
    "python.autoComplete.extraPaths": [
        "/opt/ros/noetic/lib/python2.7/dist-packages"
    ],
    "python.analysis.extraPaths": [
        "/opt/ros/noetic/lib/python3/dist-packages",
        "/home/ghigher/ROS_SW/demo01_ws/devel/lib/python3/dist-packages"
    ]
}

发布

import serial
import rospy
import json
from hello_vscode.msg import Imu

# 检查字符串是否为json格式
def is_json(test_str):
    try:
        json_object = json.loads(test_str)  # 通过json.loads判断
    except Exception as e:
        return False
    return True

if __name__ == '__main__':
    try:
        port = '/dev/ttyUSB0'  # 串口号
        baud = 115200  # 波特率
        rospy.init_node("serial_node")
        ser = serial.Serial(port, baud, timeout=0.5)
        imu_pub = rospy.Publisher("imu", Imu, queue_size=10)
        flag = ser.isOpen()
        if flag:
            rospy.loginfo("Succeed to open port")
            while not rospy.is_shutdown():
                # data = ser.read(ser.in_waiting).decode('gbk')
                data = ser.readline().decode('gbk')
                imu_msg = Imu()
                if data != '' and is_json(data):
                    # print(data)
                    #json 解析
                    imu_data = json.loads(data)
                    imu_msg.pitch = imu_data["pitch"]
                    imu_msg.roll = imu_data["roll"]
                    imu_msg.yaw = imu_data["yaw"]
                    imu_pub.publish(imu_msg)
                    rospy.loginfo("pitch:%.2f, roll:%.2f, yaw:%.2f", imu_msg.pitch, imu_msg.roll, imu_msg.yaw)
    except Exception as exc:
        rospy.loginfo("Failed to open port")

python文件赋予权限并添加到CmakeList.txt

catkin_install_python(PROGRAMS
  scripts/ros_pyserial_pub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

连接stm32

赋予串口权限

sudo chmod 777 /dev/ttyUSB0 

运行发布文件

roscore
source ./devel/setup.bash 
rosrun hello_vscode ros_pyserial_pub.py 

订阅

查看话题

rostopic list

/imu
/rosout
/rosout_agg

订阅话题

rostopic echo /imu

python实现

#! /usr/bin/env python
#  -*-coding:utf8 -*-

import rospy
from hello_vscode.msg import Imu

def doImu(imu_msg):
    rospy.loginfo("--------------------------")
    rospy.loginfo("Pitch: %.4f", imu_msg.pitch)
    rospy.loginfo("Roll: %.4f", imu_msg.roll)
    rospy.loginfo("Yaw: %.4f", imu_msg.yaw)


if __name__=="__main__":

    rospy.init_node("imu_sub")
    sub = rospy.Subscriber("imu", Imu, doImu, queue_size=10)
    rospy.spin()

运行

roscore
source ./devel/setup.bash 
rosrun hello_vscode ros_pyserial_sub.py 

rqt_graph