RT-Thread-英飞凌PSOC62+CYW43012--rosserial实验RT-Thread问答社区

英飞凌PSOC62+CYW43012--rosserial实验

发布于 2024-02-02 16:49:47 浏览：288 订阅该版

[tocm]

使用CYW43012模块的WIFI功能，可使用rosserial软件包接入ros系统。本次实验实现了发布按键状态的话题，和订阅LED状态话题来控制LED灯的亮灭
串口输出可参考上篇文章：https://club.rt-thread.org/ask/article/b4a8ddc2844d79f3.html
## 一、RT-studio创建项目
选择wifi_demo
![微信截图_20240201162043.png](https://oss-club.rt-thread.org/uploads/20240201/21031be30344079a82303e8e6cc8e613.png.webp)
## 二、rosserial配置
软件包路径：RT-Thread online packages → peripheral libraries and drivers
![微信截图_20240201162125.png](https://oss-club.rt-thread.org/uploads/20240201/e8f238a2751dfb80925915a34ef4362a.png.webp)
## 三、上位机准备
linux上ROS的安装网上有很多教程这里就不多赘述了
运行ros主机
```shell
roscore
```
后另开一个终端开启rosserial，等待设备连接
```shell
rosrun rosserial_python serial_node.py tcp
```
## 四、开发板WIFI连接+连接rosserial
![微信截图_20240201170728.png](https://oss-club.rt-thread.org/uploads/20240201/1a770edbe252d1d382fbc4e3c2faed31.png.webp)

## 五、实验结果
![微信截图_20240201112738.png](https://oss-club.rt-thread.org/uploads/20240201/dd141c9a8184e6e5e7698e8114ee6e75.png.webp)

```shell
#输出/btn_status话题信息
rostopic echo /btn_status
#发布/led_status话题控制LED
rostopic pub -1 /led_status std_msgs/Bool "data: true"
rostopic pub -1 /led_status std_msgs/Bool "data: false"
```
![微信截图_20240201135310.png](https://oss-club.rt-thread.org/uploads/20240201/6652751a947f2633cd681df02a6c690b.png)

hello_world_tcp.cpp 实验代码

```cpp
#include <rtthread.h>
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
extern "C" {
#include <rtdevice.h>
#include "drv_gpio.h"
}

#define LED_PIN     GET_PIN(0, 1)
#define BTN_PIN     GET_PIN(6, 2)

static ros::NodeHandle  nh;
static std_msgs::Bool btn_status;
static ros::Publisher btn("btn_status", &btn_status);
static bool led_status;
static struct rt_completion completion;

static void messageCb( const std_msgs::Bool &led_msg)
{
    rt_pin_write(LED_PIN,!led_msg.data);    //低电平亮灯
}

ros::Subscriber<std_msgs::Bool> sub("led_status", messageCb );

void btn_exti_cb(void *args)    //中断回调
{
    if(rt_pin_read(BTN_PIN))
    {
       btn_status.data = false;     //按键加了上拉电阻，按下为低电平
       rt_completion_done(&completion); //释放完成量
    }
    else
    {
       btn_status.data = true;
       rt_completion_done(&completion);
    }
}

static void rosserial_thread_entry(void *parameter)
{
    int last_status = 1;
    rt_completion_init(&completion);    //完成量初始化
    rt_pin_mode(LED_PIN, PIN_MODE_OUTPUT);
    rt_pin_mode(BTN_PIN, PIN_MODE_INPUT);
    rt_pin_attach_irq(BTN_PIN, PIN_IRQ_MODE_RISING_FALLING , btn_exti_cb, RT_NULL);
    rt_pin_irq_enable(BTN_PIN, PIN_IRQ_ENABLE);
    nh.getHardware()->setConnection("192.168.0.197", 11411);

nh.initNode();
    nh.advertise(btn);
    nh.subscribe(sub);
    rt_thread_mdelay(5000);
    nh.spinOnce();

while (1)
    {
        if ( nh.connected() )
        {
            if(rt_completion_wait(&completion, 10) == RT_EOK)   //等待完成量信号10ms
                btn.publish( &btn_status );
        }
        else
        {
            rt_kprintf("[rosserial] Not Connected \n");
        }
    }
}

static void spinonce_thread_entry(void *parameter)
{
    while(1)
    {
        if ( nh.connected() )
            nh.spinOnce();
        rt_thread_mdelay(100);
    }
}

static void rosserial_hello_world_tcp_example(int argc,char *argv[])
{
    rt_thread_t thread = rt_thread_create("rosserial", rosserial_thread_entry, RT_NULL, 2048, 25, 10);
    if(thread != RT_NULL)
    {
        rt_thread_startup(thread);
        rt_kprintf("[rosserial] New thread rosserial\n");
    }
    else
    {
        rt_kprintf("[rosserial] Failed to create thread rosserial\n");
    }

rt_thread_t tid = rt_thread_create("spinonce", spinonce_thread_entry, RT_NULL, 2048, 30, 10);   //spinonce运行时间长，另外开线程运行
    if(tid != RT_NULL)
    {
     rt_thread_startup(tid);
     rt_kprintf("[spinonce] New thread spinonce\n");
    }
    else
    {
     rt_kprintf("[spinonce] Failed to create thread spinonce\n");
    }
}
MSH_CMD_EXPORT(rosserial_hello_world_tcp_example, roserial hello world example with TCP);

// If you are using Keil, you can ignore everything below

// This is required
// If you'd like to compile with scons which uses arm-none-eabi-gcc
// extern "C" void __cxa_pure_virtual()
// {
//     while (1);
// }

// Moreover, you need to add:
// CXXFLAGS = CFLAGS + ' -fno-rtti'
// in rtconfig.py for arm-none-eabi-gcc

```