esp32(ROS2foxy)从字符串发布到速度发布turtlesim
2023-03-31 10:32:32 时间
直播课演示如下,从点亮一个小灯,发送字符串到发布速度指令控制turtlesim。
字符串参考:
eps32和ros2之稳稳点亮一个LED灯(IO4)_zhangrelay的专栏-CSDN博客
速度代码如下:
#include <ros2arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID "***"
#define SSID_PW "***"
#define AGENT_IP "***"
#define AGENT_PORT 2021 //AGENT port number
#define PUBLISH_FREQUENCY 2 //hz
void publishVel(geometry_msgs::Twist* vel, void* arg)
{
(void)(arg);
static int cnt = 0;
vel->linear.x = 0.1+0.01*cnt; //线速度
vel->angular.z = 0.1+0.01*cnt; //角速度
// vel->linear.x = ((double)rand()/(RAND_MAX)); //随机线速度
// vel->angular.z = ((double)rand()/(RAND_MAX)); //随机角速度
// vel->linear.x = 0.2; //固定线速度
// vel->angular.z = 1.0 - 0.001*cnt; //变化角速度
cnt++;
}
class VelPub : public ros2::Node
{
public:
VelPub()
: Node("esp32_cmdvel")
{
ros2::Publisher<geometry_msgs::Twist>* publisher_ = this->createPublisher<geometry_msgs::Twist>("turtle1/cmd_vel");
this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishVel, nullptr, publisher_);
}
};
WiFiUDP udp;
void setup()
{
WiFi.begin(SSID, SSID_PW);
while(WiFi.status() != WL_CONNECTED);
ros2::init(&udp, AGENT_IP, AGENT_PORT);
}
void loop()
{
static VelPub VelNode;
ros2::spin(&VelNode);
}
相关文章
- 助力企业数字化升级,火山引擎发布云上增长解决方案
- Web 3.0世界中的嬉皮士、先知与造物主
- 五款经典代码阅读器的使用方案对比
- 思迈特软件Smartbi完成C轮融资,推动国产BI加速进入智能化时代
- 是什么让 Flutter 与众不同
- 2022年软件开发的趋势
- Windows环境安装Flutter
- macOS 环境安装Flutter
- 代理服务器软件如何理解?
- linux 环境安装Flutter
- Vue 3.0 有哪些新特性值得我们提前了解
- Flutter 初学者的简单例子充分解释
- Python open()函数详解
- AndroidStudio创建第一个 Flutter 应用程序
- python pyqt5系统中查找文件
- 不盲追大模型与堆算力!沈向洋、曹颖与马毅提出理解 AI 的两个基本原理:简约性与自一致性
- open()文件对象常用的属性
- Flutter 找不到 android sdk(图文详解)记一次安装中错误的过程
- 自动驾驶汽车的软件升级技术管理与监管策略分析
- Vue.js的图片加载性能优化你可以试试