学做网站论坛vip学员码,泉州关键词排名,交换友情链接的条件,购物网站建设所需软件文章目录 下位机上位机自定义msg消息发布订阅 ROS与STM32通信一般分为两种#xff0c;
STM32上运行ros节点实现通信使用普通的串口库进行通信#xff0c;然后以话题方式发布
第一种方式具体实现过程可参考上篇文章ROS与STM32通信-rosserial#xff0c;上述文章中的收发频率… 文章目录 下位机上位机自定义msg消息发布订阅 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 yawpackage.xml添加依赖 build_dependmessage_generation/build_dependexec_dependmessage_runtime/exec_dependCMakeList.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, timeout0.5)imu_pub rospy.Publisher(imu, Imu, queue_size10)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 /imupython实现
#! /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_size10)rospy.spin()运行 roscoresource ./devel/setup.bash rosrun hello_vscode ros_pyserial_sub.py rqt_graph
