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

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语言转义字符:

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

可使用cutecom查看发送的消息

上位机

自定义msg消息

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

新建文件Imu.msg(首字母大写),输入以下内容

C 复制代码
float32 pitch
float32 roll
float32 yaw

package.xml添加依赖

xml 复制代码
  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

CMakeList.txt编辑msg相关配置

cmake 复制代码
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

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"
    ]
}

发布

python 复制代码
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

cmake 复制代码
catkin_install_python(PROGRAMS
  scripts/ros_pyserial_pub.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

连接stm32

赋予串口权限

复制代码
sudo chmod 777 /dev/ttyUSB0 

运行发布文件

shell 复制代码
roscore
source ./devel/setup.bash 
rosrun hello_vscode ros_pyserial_pub.py 

订阅

查看话题

复制代码
rostopic list

/imu
/rosout
/rosout_agg

订阅话题

复制代码
rostopic echo /imu

python实现

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()

运行

sh 复制代码
 roscore
 source ./devel/setup.bash 
 rosrun hello_vscode ros_pyserial_sub.py 
sh 复制代码
rqt_graph
相关推荐
一川月白7095 小时前
51单片机---硬件学习(电子琴、主从应答模式、modbus模型、DS18B20传感器显示温度)
嵌入式硬件·51单片机·串口通信·异步通信·串行通信·同步通信·并行通信
逼子格5 小时前
【Proteus仿真】定时器控制系列仿真——秒表计数/数码管显示时间
数据库·单片机·嵌入式硬件·51单片机·proteus·定时器·硬件工程师
2401_888423096 小时前
51单片机-按键、蜂鸣器、定时器模块及中断
单片机·嵌入式硬件·51单片机
东亚_劲夫7 小时前
STM32—SPI协议
stm32·单片机·嵌入式硬件
♞沉寂8 小时前
c51串口通信原理及实操
单片机·51单片机·c51
猫头虎9 小时前
2025最新超详细FreeRTOS入门教程:第一章 FreeRTOS移植到STM32
stm32·单片机·嵌入式硬件·机器人·硬件架构·freertos·嵌入式实时数据库
清风66666611 小时前
基于STM32单片机的酒驾检测设计
stm32·单片机·嵌入式硬件·毕业设计·课程设计
恒森宇电子有限公司11 小时前
IP5326_BZ 支持C同口输入输出的移动电源芯片 2.4A的充放电电流 支持4LED指示灯
c语言·开发语言·单片机
涂山苏苏⁠11 小时前
STM32之ADC
stm32·单片机·adc
曙曙学编程12 小时前
stm32——NVIC,EXIT
c语言·c++·stm32·单片机·嵌入式硬件