/uros_ws/esp32/firmware/freertos/apps/joint_states_subscriber
#include <rcl/rcl.h> #include <rcl/error_handling.h> #include <rclc/rclc.h> #include <rclc/executor.h> #include <sensor_msgs/msg/joint_state.h> #include <stdio.h> #include "freertos/FreeRTOS.h" #define ARRAY_LEN 200 #define JOINT_DOUBLE_LEN 20 #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); \ return 1; \ } \ } #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_subscription_t joint_states_subscriber; sensor_msgs__msg__JointState joint_states_msg; void subscription_joint_state_callback(const void *msgin) { const sensor_msgs__msg__JointState *msg = (const sensor_msgs__msg__JointState *)msgin; // int joy_btn_num = 0; joy_btn_num = sizeof(msg->position.data); int joy_val[joy_btn_num]; for (int i = 0; i < joy_btn_num; i++) { joy_val[i] = msg->position.data[i]; } printf("I get joint_state topic msg.pos:"); for (int l = 0; l < joy_btn_num; l++) { printf(" %d", joy_val[l]); } printf("\r\n"); // %d \r\n", (int)(msg->position.data[i] * 100)); // } /* joy_msg.header.stamp = ros::Time().now(); */ /*--- header: stamp: sec: 0 nanosec: 0 frame_id: '' axes: [] buttons: [] ---*/ int appMain(void *argument) { rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; // create init_options RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // create node rcl_node_t node; RCCHECK(rclc_node_init_default(&node, "string_node", "", &support)); // create joint_state subscriber RCCHECK(rclc_subscription_init_default( &joint_states_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy), "/joy")); // create executor rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_subscription(&executor, &joint_states_subscriber, &joint_states_msg, &subscription_joint_state_callback, ON_NEW_DATA)); char joint_states_msg_buffer[ARRAY_LEN]; joint_states_msg.header.frame_id.data = joint_states_msg_buffer; joint_states_msg.header.frame_id.size = 20; joint_states_msg.header.frame_id.capacity = ARRAY_LEN; rosidl_runtime_c__String string_buffer[JOINT_DOUBLE_LEN]; joint_states_msg.name.data = string_buffer; joint_states_msg.name.size = 0; joint_states_msg.name.capacity = JOINT_DOUBLE_LEN; for (int i = 0; i < JOINT_DOUBLE_LEN; i++) { joint_states_msg.name.data[i].data = (char *)malloc(ARRAY_LEN); joint_states_msg.name.data[i].size = 0; joint_states_msg.name.data[i].capacity = ARRAY_LEN; } double joint_states_position_buffer[JOINT_DOUBLE_LEN]; joint_states_msg.position.data = joint_states_position_buffer; joint_states_msg.position.size = 7; joint_states_msg.position.capacity = JOINT_DOUBLE_LEN; double joint_states_velocity_buffer[JOINT_DOUBLE_LEN]; joint_states_msg.velocity.data = joint_states_velocity_buffer; joint_states_msg.velocity.size = 7; joint_states_msg.velocity.capacity = JOINT_DOUBLE_LEN; double joint_states_effort_buffer[JOINT_DOUBLE_LEN]; joint_states_msg.effort.data = joint_states_effort_buffer; joint_states_msg.effort.size = 7; joint_states_msg.effort.capacity = JOINT_DOUBLE_LEN; rclc_executor_spin(&executor); RCCHECK(rcl_subscription_fini(&joint_states_subscriber, &node)); RCCHECK(rcl_node_fini(&node)); return 0; }
micro-ROS-demos/rclcpp/string_subscriber/main.cpp
#include <std_msgs/msg/string.hpp> #include <rclcpp/rclcpp.hpp> #include <memory> #include <cstdio> using std::placeholders::_1; class string_subscriber_cpp_node : public rclcpp::Node { public: string_subscriber_cpp_node() : Node("string_subscriber_cpp") { rclcpp::QoS qos(10); subscription_ = this->create_subscription<std_msgs::msg::String>("std_msgs_msg_String", qos, std::bind(&string_subscriber_cpp_node::topic_callback, this, _1)); } private: rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; void topic_callback(const std_msgs::msg::String::SharedPtr msg) { std::cout << "I heard: " << msg->data << std::endl; } }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<string_subscriber_cpp_node>()); rclcpp::shutdown(); return 0; }
ESP32(Micro-ROS)ーPublisherSample
~$ cat uros_ws/esp32/firmware/freertos_apps/apps/int32_publisher/app.c
#include <stdio.h> #include <unistd.h> #include <rcl/rcl.h> #include <rcl/error_handling.h> #include <std_msgs/msg/int32.h> #include <rclc/rclc.h> #include <rclc/executor.h> #ifdef ESP_PLATFORM #include "freertos/FreeRTOS.h" #include "freertos/task.h" #endif #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; std_msgs__msg__Int32 msg; void timer_callback(rcl_timer_t * timer, int64_t last_call_time) { RCLC_UNUSED(last_call_time); if (timer != NULL) { RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); msg.data++; } } void appMain(void * arg) { rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; // create init_options RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // create node rcl_node_t node; RCCHECK(rclc_node_init_default(&node, "freertos_int32_publisher", "", &support)); // create publisher RCCHECK(rclc_publisher_init_default( &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "freertos_int32_publisher")); // create timer, rcl_timer_t 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; RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_timer(&executor, &timer)); msg.data = 0; while(1){ rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); usleep(100000); } // free resources RCCHECK(rcl_publisher_fini(&publisher, &node)) RCCHECK(rcl_node_fini(&node)) vTaskDelete(NULL); }