【ROS、STM32】ROS端发送命令使STM32执行相应命令

一、下载ROS与STM32通信工程文件

博主这边提供了所需要的的STM32与ROS工程文件百度云链接如下
链接: https://pan.baidu.com/s/1oOcfHVRdzKeyQKiBKLHsrg.
提取码：abcd
但是，该文件只是ROS与STM32通信工程文件，要实现通过在ROS端的命令发送使STM32板实现相应的功能还需要以下的步骤，博主这边以ROS端发送指令实现STM32端小灯的亮灭来进行说明介绍

二、通过ROS终端发送不同指令来打印不同信息

在之前的ROS学习笔记四中，谈到了关于服务编程的相关内容，在下面的链接中，可以看到我们是怎么实现在终端输入命令来控制小海龟的运动，通过这个例程，我想可以不可以输入不同的指令来让终端打印不同的信息，如果解决了这个问题，我们就可以通过在ROS端输入不同指令在让单片机做不同的事情，以下是学习笔记四的链接，又不清楚的地方可以去重新回顾一下
链接: https://blog.csdn.net/weixin_45417246/article/details/115490999.
打开vscode，并在vscode界面打开一个终端，输入以下命令进行工作空间的创建

cd ~/fjy_xm
mkdir catkin_command
cd catkin_command
mkdir src
catkin_make
source devel/setup.bash
cd src
catkin_create_pkg learning_command roscpp std_msgs
cd learning_command
cd src
touch command_publish.cpp

command_publish.cpp文件如下

#include <ros/ros.h>
#include <std_srvs/SetBool.h>

bool commandCallback(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res)
{
    if(req.data==1) ROS_INFO("Publish 1.");
    else if(req.data==2) ROS_INFO("Publish 2.");
    else ROS_INFO("Publish else.");
	// 设置反馈数据
	res.success = true;
    res.message = "command is ok";
    return true;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "command_server");
    ros::NodeHandle n;
    ros::ServiceServer command_service = n.advertiseService("/command", commandCallback);
    ROS_INFO("Ready to receive command.");
	ros::Rate loop_rate(10);
	while(ros::ok())
	{
    	ros::spinOnce();
	    loop_rate.sleep();
	}
    return 0;
}

在CMakeLists.txt文件后面再加上以下两句代码，生成可执行文件

add_executable(command_publish src/command_publish.cpp)
target_link_libraries(command_publish ${catkin_LIBRARIES})

编译后运行learning_command功能包下的command_publish节点，运行效果如下

至此，顺利解决了ROS端发送不同指令打印不同的对应信息的功能

三、STM32通信工程测试文件

在完成了ROS端发送不同指令打印不同的对应信息的功能后，在原有串口通信测试程序的基础上，通过接收信号的不同，添加了led灯的亮灭，这样接收到不同的信号时，能够对不同的信号做出不同的反应

main.c文件

#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "led.h"
#include "mbotLinuxUsart.h"

short testSend1   =5000;
short testSend2   =2000;
short testSend3   =1000;
unsigned char testSend4 = 0x05;

int testRece1     =0;
int testRece2     =0;
unsigned char testRece3 = 0x00;

int main(void)
{	
	delay_init();	    	                   
	LED_Init();		  	
	LED0=1;
	LED1=1;	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); 
	uart_init(115200);	                          
	while(1)
	{
		usartSendData(testSend1,testSend2,testSend3,testSend4);
		delay_ms(13);
	} 
}
void USART1_IRQHandler()
{
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
 	 {
		 USART_ClearITPendingBit(USART1,USART_IT_RXNE);
		 usartReceiveOneData(&testRece1,&testRece2,&testRece3);
		 if(testRece1==1000) LED0=!LED0;
		 if(testRece2==1000) LED1=!LED1;
	 }
}

四、ROS端通信功能包测试文件

command_publish.cpp文件

#include "ros/ros.h"
#include "std_msgs/String.h" //use data struct of std_msgs/String  
#include "mbot_linux_serial.h"
#include <std_srvs/SetBool.h>

//test receive value
double testRece1=0.0;
double testRece2=0.0;
double testRece3=0.0;
unsigned char testRece4=0x00;

bool commandCallback(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res)
{
    if(req.data==1) 
    {
        writeSpeed(1000,2000,0x07);
        ROS_INFO("Publish command 1.");
    }
    else if(req.data==2) 
    {
        writeSpeed(2000,1000,0x07);
        ROS_INFO("Publish command 2.");
    }
    else 
    {
        ROS_INFO("Publish command else.");
    }
	// 设置反馈数据
	res.success = true;
    res.message = "command is ok";
    return true;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "public_node");
    ros::NodeHandle n;
    serialInit();
    ros::ServiceServer command_service = n.advertiseService("/command", commandCallback);
    ROS_INFO("Ready to receive command.");
	ros::Rate loop_rate(10);
	while(ros::ok())
	{
    	ros::spinOnce();
        //从STM32接收数据，输入参数依次转化为小车的线速度、角速度、航向角（角度）、预留控制位
	    readSpeed(testRece1,testRece2,testRece3,testRece4);
        //ROS_INFO("%f,%f,%f,%d\n",testRece1,testRece2,testRece3,testRece4);
	    loop_rate.sleep();
	}
    return 0;
}

五、运行效果

通过输入指令，可以使STM32精英板上的黄灯和红灯亮灭

这样，我们就完成了通过ROS端发送不同的指令给STM32，让32单片机执行相应的命令了