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);
     
    
   

    

     

    
    

     

      }
     
    
  
 

代码做稳定性测试，目前就是觉得节点好像不能自动注销呢。

 看这个红红！

这是一个节点，既有收又能发。

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/120351704