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 COMPONENTSroscpprospystd_msgsmessage_generation
)## Generate messages in the 'msg' folder
add_message_files(FILESImu.msg
)## Generate added messages and services with any dependencies listed here
generate_messages(DEPENDENCIESstd_msgs
)catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES hello_vscodeCATKIN_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 Falsereturn Trueif __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(PROGRAMSscripts/ros_pyserial_pub.pyDESTINATION ${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 Imudef 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()

运行

 roscoresource ./devel/setup.bash rosrun hello_vscode ros_pyserial_sub.py 

rqt_graph