#include “FreeRTOS.h”
#include “task.h”
#include “main.h”
#include “cmsis_os.h”
#include “usart.h”
#include “u_microros.h”
#include “canopen_app.h”
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rcl/error_handling.h>
#include <uxr/client/transport.h>
#include <rmw_microxrcedds_c/config.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/string.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/int32_multi_array.h>
#include <rcl/error_handling.h>
#include “example_interfaces/srv/add_two_ints.h”
#include <stdio.h>
#include <unistd.h>
#include <rcl/subscription.h>
bool cubemx_transport_open(struct uxrCustomTransport *transport);
bool cubemx_transport_close(struct uxrCustomTransport *transport);
size_t cubemx_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *err);
size_t cubemx_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout,
uint8_t *err);
void* microros_allocate(size_t size, void *state);
void microros_deallocate(void *pointer, void state);
void microros_reallocate(void *pointer, size_t size, void state);
void microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void *state);
#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);}}
#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_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
rclc_executor_t executor;
void microros_init()
{
rmw_uros_set_custom_transport(
true, (void*) &huart2, cubemx_transport_open, cubemx_transport_close, cubemx_transport_write,
cubemx_transport_read);
rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator(); freeRTOS_allocator.allocate = microros_allocate; freeRTOS_allocator.deallocate = microros_deallocate; freeRTOS_allocator.reallocate = microros_reallocate; freeRTOS_allocator.zero_allocate = microros_zero_allocate; if (!rcutils_set_default_allocator(&freeRTOS_allocator)) { printf(“Error on default allocators (line %d)\n”, LINE); } allocator = rcl_get_default_allocator(); //create init_options rclc_support_init(&support, 0, NULL, &allocator);
}
attribute((used)) void service_callback(const void *req, void *res)
{
example_interfaces__srv__AddTwoInts_Request req_in = (example_interfaces__srv__AddTwoInts_Request) req;
example_interfaces__srv__AddTwoInts_Response res_in = (example_interfaces__srv__AddTwoInts_Response) res;
printf(“Service request value: %d + %d.\n”, (int) req_in->a, (int) req_in->b); res_in->sum = req_in->a + req_in->b;
}
void callback_led_control(const void *msgin)
{
const std_msgs__msg__Int32 msg = (const std_msgs__msg__Int32) msgin;
if (msg->data == 0)
{
// control_word = 0x06;
// sendPDOevent(My_Data);
HAL_GPIO_WritePin(LED0_GPIO_Port, LED0_Pin, GPIO_PIN_SET);
}
else if (msg->data == 1)
{
// control_word = 0x0f;
// sendPDOevent(My_Data);
HAL_GPIO_WritePin(LED0_GPIO_Port, LED0_Pin, GPIO_PIN_RESET);
}
}
void callback_canopen_start(const void *msgin)
{
const std_msgs__msg__Int32 msg = (const std_msgs__msg__Int32) msgin;
if ((msg->data >= 1) ||(msg->data <= 2))
{
if (SDO_Config_slave(msg->data))
{
return;
}
check_and_start_node(My_Data, msg->data);
uint32_t SendData = 0x3; writeNetworkDict(My_Data, msg->data, 0x6060, 0x00, 0x01, uint8, &SendData, 0); //模式 MyFun_Wait_SDO(msg->data); SendData = 0xf; writeNetworkDict(My_Data, msg->data, 0x6040, 0x00, 0x02, uint16, &SendData, 0); //控制字 MyFun_Wait_SDO(msg->data); SendData = 0x010000; writeNetworkDict(My_Data, msg->data, 0x60ff, 0x00, 0x04, int32, &SendData, 0); //速度 MyFun_Wait_SDO(msg->data); }
}
void callback_canopen_pdo(const void *msgin)
{
const std_msgs__msg__Int32MultiArray msg = (const std_msgs__msg__Int32MultiArray) msgin;
struct nodeid_var *pNode = &(masterVar.nodeid1);
int32_t *pdodata = msg->data.data;
uint8_t nodeid = pdodata[0] - 1;
*pNode[nodeid].W_controlWord = pdodata[1];
int32_t speed = pdodata[2];
if(speed >500 )
{
speed = 500;
}else if(speed <-500)
{
speed = -500;
}
speed *=2730;
*pNode[nodeid].W_speed = speed;
sendPDOevent(My_Data);
}
void my_microros_service()
{
microros_init();
// micro-ROS app
rcl_service_t service; allocator = rcl_get_default_allocator(); //create init_options rclc_support_init(&support, 0, NULL, &allocator); // create node rclc_node_init_default(&node, “cubemx_node”, “”, &support); rclc_executor_init(&executor, &support.context, 1, &allocator); rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts), “add_two_ints”); example_interfaces__srv__AddTwoInts_Response response_msg; example_interfaces__srv__AddTwoInts_Request request_msg; rclc_executor_add_service(&executor, &service, &request_msg, &response_msg, service_callback); //rclc_executor_spin(&executor); while (1) { vTaskDelay(100); rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); }
}
rcl_publisher_t pub;
rcl_subscription_t sub_pdo;
rcl_subscription_t sub_start;
std_msgs__msg__Int32 sub_msg_start;
std_msgs__msg__Int32MultiArray sub_msg_pdo;
std_msgs__msg__Int32MultiArray pubArray_msg;
void my_microros_run()
{
microros_init();
rclc_node_init_default(&node, “cubemx_node”, “”, &support);
std_msgs__msg__Int32__init(&sub_msg_start); std_msgs__msg__Int32MultiArray__init(&pubArray_msg); std_msgs__msg__Int32MultiArray__init(&sub_msg_pdo);
// rclc_publisher_init_default(&pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
// “dataArray”);
rcl_ret_t rc;
rc=rclc_subscription_init_default(&sub_start, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
“pub_start”);
rc=rclc_subscription_init_default(&sub_pdo, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
“pub_pdo”);
rclc_executor_init(&executor, &support.context, 2, &allocator); rc = rclc_executor_add_subscription(&executor, &sub_pdo, &sub_msg_pdo, &callback_canopen_pdo, ON_NEW_DATA); rc=rclc_executor_add_subscription(&executor, &sub_start, &sub_msg_start, &callback_canopen_start, ON_NEW_DATA);
// int32_t tee[] = { 1, 2, 3 };
while (1)
{
vTaskDelay(10);
// Set message value
// pubArray_msg.data.data = tee;
// pubArray_msg.data.size = 3;
// pubArray_msg.data.capacity = 3;
// Publish message
//rcl_publish(&pub, &pubArray_msg, NULL);
rclc_executor_spin(&executor);
//rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
}
}
话题都正常发布，结合这整段代码分析一下这段代码为什么pub_start话题能进回调，而pub_pdo不能进回调
本文探讨了在网络编程中常见的TIME_WAIT状态及其解决方案, 对于如何避免和处理TIME_WAIT状态进行了深入分析。
扫一扫
229
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
