前言
ROS与STM32的串口通信
之前写过一篇用boost
的串口通信, 本篇写下ros
的serial
的通信方式.
serial安装
sudo apt-get install ros-<distro>-serial
, 由于是Ubuntu 18, 那么就是:
sudo apt install ros-melodic-serial
STM32工程
串口的使用参考 STM32CubeMX_UART_printf_接收中断_DMA空闲中断_LPUART 一文, 主要代码如下:
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
static uint8_t count = 0;
++count;
printf("Hello ROS |%3d\r\n", count);
HAL_Delay(10);
}
/* USER CODE END 3 */
以100Hz的频率打印字符串, 波特率115200-8-N-1.
ROS程序
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_create_pkg serial_port roscpp std_msgs
cd serial_port/src
touch serial_port.cpp
gedit serial_port.cpp
在打开的serial_port.cpp
放入以下代码:
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "serial_port");
ros::NodeHandle n;
serial::Serial sp("/dev/ttyACM0",115200,serial::Timeout::simpleTimeout(1000));
if(sp.isOpen()) {
ROS_INFO_STREAM("/dev/ttyACM0 is opened.");
} else {
return -1;
}
ros::Rate loop_rate(100);
while (ros::ok())
{
if(sp.available()) {
std_msgs::String result;
result.data = sp.read(sp.available());
ROS_INFO_STREAM(result.data);
}
ros::spinOnce();
loop_rate.sleep();
}
sp.close();
return 0;
}
打开~/catkin_make/src/serial_port/CMakeLists.txt
, 填入:
add_executable(serial_port src/serial_port.cpp)
add_dependencies(serial_port ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(serial_port
${catkin_LIBRARIES}
)
编译运行:
roscore
cd ~/catkin_ws
catkin_make
source devel/setup.bash
sudo chmod 666 /dev/ttyACM0 #加权限才能正常打开
rosrun serial_port serial_port
可以看到打印, 前面是缓存的, 一直到126才算正常:
关于发送和读取, 详细可以参考这篇文章: https://blog.csdn.net/m0_37598942/article/details/80713512
微信公众号
欢迎扫描关注我的微信公众号, 及时获取最新文章: