micro-ROS之esp32与ros2发布pub与接收sub
Posted zhangrelay
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了micro-ROS之esp32与ros2发布pub与接收sub相关的知识,希望对你有一定的参考价值。
先看源码
#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_system.h"
#include <uros_network_interfaces.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <std_msgs/msg/int32.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rmw_microros/rmw_microros.h>
#include "uxr/client/config.h"
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\\n",__LINE__,(int)temp_rc);vTaskDelete(NULL);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\\n",__LINE__,(int)temp_rc);}}
rcl_publisher_t publisher;
rcl_subscription_t subscriber;
std_msgs__msg__Int32 send_msg;
std_msgs__msg__Int32 recv_msg;
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
(void) last_call_time;
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &send_msg, NULL));
printf("Sent: %d\\n", send_msg.data);
send_msg.data++;
}
}
void subscription_callback(const void * msgin)
{
const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin;
printf("Received: %d\\n", msg->data);
}
void micro_ros_task(void * arg)
{
rcl_allocator_t allocator = rcl_get_default_allocator();
rclc_support_t support;
// Create init_options.
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
RCCHECK(rcl_init_options_init(&init_options, allocator));
rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
// Use static agent IP and port.
RCCHECK(rmw_uros_options_set_udp_address(CONFIG_MICRO_ROS_AGENT_IP, CONFIG_MICRO_ROS_AGENT_PORT, rmw_options));
// Setup support structure.
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
// Create node.
rcl_node_t node = rcl_get_zero_initialized_node();
RCCHECK(rclc_node_init_default(&node, "esp32_int32_publisher_subscriber_rclc", "", &support));
// Create publisher.
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"esp32_int32_publisher"));
// Create subscriber.
RCCHECK(rclc_subscription_init_default(
&subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"esp32_int32_subscriber"));
// Create timer.
rcl_timer_t timer = rcl_get_zero_initialized_timer();
const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// Create executor.
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
unsigned int rcl_wait_timeout = 1000; // in ms
RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout)));
// Add timer and subscriber to executor.
RCCHECK(rclc_executor_add_timer(&executor, &timer));
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &recv_msg, &subscription_callback, ON_NEW_DATA));
// Spin forever.
send_msg.data = 0;
while(1){
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
usleep(100000);
}
// Free resources.
RCCHECK(rcl_subscription_fini(&subscriber, &node));
RCCHECK(rcl_publisher_fini(&publisher, &node));
RCCHECK(rcl_node_fini(&node));
vTaskDelete(NULL);
}
void app_main(void)
{
#ifdef UCLIENT_PROFILE_UDP
// Start the networking if required
ESP_ERROR_CHECK(uros_network_interface_initialize());
#endif // UCLIENT_PROFILE_UDP
//pin micro-ros task in APP_CPU to make PRO_CPU to deal with wifi:
xTaskCreate(micro_ros_task,
"uros_task",
CONFIG_MICRO_ROS_APP_STACK,
NULL,
CONFIG_MICRO_ROS_APP_TASK_PRIO,
NULL);
}
代码做稳定性测试,目前就是觉得节点好像不能自动注销呢。
看这个红红!
这是一个节点,既有收又能发。
以上是关于micro-ROS之esp32与ros2发布pub与接收sub的主要内容,如果未能解决你的问题,请参考以下文章
micro-ROS之esp32与ros2资料(freertos)
micro-ROS之esp32与ros2资料(freertos)
esp32发布机器人电池电压到ros2(micro-ros+CoCube)