micro_ros_setup: some thing wrong with rclc_support_init in appMain

Error output with appMain thread open.

with no idea of gdb in linux with openocd so I add some printf in code to detect where the code had ran.

printf output.

printf is ready 
endif----- 
freeRTOS allocator success 
Into appMain 
test printf 0 
test printf 1 
rclc_support_init counter : 0 
rclc_support_init counter : 1 
rclc_support_init counter : 2 
rclc_support_init counter : 3 
rclc_support_init counter : 4 
get to end rclc_support_init counter : 5 
rclc_support_init_with_options counter : 0 
rclc_support_init_with_options counter : 1 
rclc_support_init_with_options counter : 2 
rclc_support_init_with_options counter : 3 
rclc_support_init_with_options counter : 4 
rclc_support_init_with_options counter : 5 
rclc_support_init_with_options counter : 6

It mean the code run into appMain and stop at this line

RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

and finally it stop here ./mcu_ws/uros/rclc/rclc/src/rclc/init.c

rcl_ret_t
rclc_support_init_with_options(
  rclc_support_t * support,
  int argc,
  char const * const * argv,
  rcl_init_options_t * init_options,
  rcl_allocator_t * allocator)
{
 int counter = 0;
 printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    support, "support is a null pointer", return RCL_RET_INVALID_ARGUMENT);
   printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    init_options, "init_options is a null pointer", return RCL_RET_INVALID_ARGUMENT);
   printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    allocator, "allocator is a null pointer", return RCL_RET_INVALID_ARGUMENT);
  rcl_ret_t rc = RCL_RET_OK;
  printf("%s counter : %d \r\n", __func__, counter++);

  memcpy(&support->init_options, init_options, sizeof(rcl_init_options_t));
  printf("%s counter : %d \r\n", __func__, counter++);

   printf("%s counter : %d \r\n", __func__, counter++);
 support->context = rcl_get_zero_initialized_context();
   printf("%s counter : %d \r\n", __func__, counter++);
 rc = rcl_init(argc, argv, &support->init_options, &support->context);              //stop here
   printf("%s counter : %d \r\n", __func__, counter++);
 if (rc != RCL_RET_OK) {
    PRINT_RCLC_ERROR(rclc_init, rcl_init);
    return rc;
  }
  support->allocator = allocator;
  printf("%s counter : %d \r\n", __func__, counter++);

  rc = rcl_clock_init(RCL_STEADY_TIME, &support->clock, support->allocator);
  if (rc != RCL_RET_OK) {
    PRINT_RCLC_ERROR(rclc_init, rcl_clock_init);
  }
   printf("%s counter : %d \r\n", __func__, counter++);
 return rc;
}

the change I had make.

I just add some printf.

in Ping-Pong app.c

void appMain(void *argument)
{
	int test_printf_count = 0;
	printf("Into %s \r\n", __func__);
	rcl_allocator_t allocator = rcl_get_default_allocator();
	printf("test printf %d \r\n", test_printf_count++);
	rclc_support_t support;

	printf("test printf %d \r\n", test_printf_count++);
// create init_options
	RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

	printf("test printf %d \r\n", test_printf_count++);
// create node
	rcl_node_t node;
	RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support));

	printf("test printf %d \r\n", test_printf_count++);
// Create a reliable ping publisher
	RCCHECK(rclc_publisher_init_default(&ping_publisher, &node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

	printf("test printf %d \r\n", test_printf_count++);
// Create a best effort pong publisher
	RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));

	printf("test printf %d \r\n", test_printf_count++);
// Create a best effort ping subscriber
	RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping"));

	printf("test printf %d \r\n", test_printf_count++);
// Create a best effort  pong subscriber
	RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node,
		ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong"));


	printf("test printf %d \r\n", test_printf_count++);
// Create a 3 seconds ping timer timer,
	rcl_timer_t timer;
	RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback));


	printf("test printf %d \r\n", test_printf_count++);
// Create executor
	rclc_executor_t executor;
	RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator));
	RCCHECK(rclc_executor_add_timer(&executor, &timer));
	RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping,
		&ping_subscription_callback, ON_NEW_DATA));
	RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong,
		&pong_subscription_callback, ON_NEW_DATA));

	printf("test printf %d \r\n", test_printf_count++);
// Create and allocate the pingpong messages
	char outcoming_ping_buffer[STRING_BUFFER_LEN];
	outcoming_ping.frame_id.data = outcoming_ping_buffer;
	outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

	char incoming_ping_buffer[STRING_BUFFER_LEN];
	incoming_ping.frame_id.data = incoming_ping_buffer;
	incoming_ping.frame_id.capacity = STRING_BUFFER_LEN;

	char incoming_pong_buffer[STRING_BUFFER_LEN];
	incoming_pong.frame_id.data = incoming_pong_buffer;
	incoming_pong.frame_id.capacity = STRING_BUFFER_LEN;

	device_id = rand();
	
	printf("test printf %d \r\n", test_printf_count++);

	while(1){
		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
		usleep(10000);
	}

	// Free resources
	RCCHECK(rcl_publisher_fini(&ping_publisher, &node));
	RCCHECK(rcl_publisher_fini(&pong_publisher, &node));
	RCCHECK(rcl_subscription_fini(&ping_subscriber, &node));
	RCCHECK(rcl_subscription_fini(&pong_subscriber, &node));
	RCCHECK(rcl_node_fini(&node));
}

firmware/mcu_ws/uros/rclc/rclc/src/rclc/init.c

rcl_ret_t
rclc_support_init(
  rclc_support_t * support,
  int argc,
  char const * const * argv,
  rcl_allocator_t * allocator)
{
  int counter = 0;
  printf("%s counter : %d \r\n", __func__, counter++);
  RCL_CHECK_FOR_NULL_WITH_MSG(
    support, "support is a null pointer", return RCL_RET_INVALID_ARGUMENT);
  printf("%s counter : %d \r\n", __func__, counter++);
RCL_CHECK_FOR_NULL_WITH_MSG(
    allocator, "allocator is a null pointer", return RCL_RET_INVALID_ARGUMENT);
  rcl_ret_t rc = RCL_RET_OK;
printf("%s counter : %d \r\n", __func__, counter++);

 printf("%s counter : %d \r\n", __func__, counter++);
 rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  printf("%s counter : %d \r\n", __func__, counter++);
rc = rcl_init_options_init(&init_options, (*allocator) );
  if (rc != RCL_RET_OK) {
    PRINT_RCLC_ERROR(rclc_support_init, rcl_init_options_init);
    return rc;
  }
printf("get to end %s counter : %d \r\n", __func__, counter++);

  rc = rclc_support_init_with_options(support, argc, argv, &init_options, allocator);

  printf("get to end %s counter : %d \r\n", __func__, counter++);

  return rc;
}

firmware/mcu_ws/uros/rclc/rclc/src/rclc/init.c

rcl_ret_t
rclc_support_init_with_options(
  rclc_support_t * support,
  int argc,
  char const * const * argv,
  rcl_init_options_t * init_options,
  rcl_allocator_t * allocator)
{
 int counter = 0;	
 printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    support, "support is a null pointer", return RCL_RET_INVALID_ARGUMENT);
   printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    init_options, "init_options is a null pointer", return RCL_RET_INVALID_ARGUMENT);
   printf("%s counter : %d \r\n", __func__, counter++);
 RCL_CHECK_FOR_NULL_WITH_MSG(
    allocator, "allocator is a null pointer", return RCL_RET_INVALID_ARGUMENT);
  rcl_ret_t rc = RCL_RET_OK;
  printf("%s counter : %d \r\n", __func__, counter++);

  memcpy(&support->init_options, init_options, sizeof(rcl_init_options_t));
  printf("%s counter : %d \r\n", __func__, counter++);

   printf("%s counter : %d \r\n", __func__, counter++);
 support->context = rcl_get_zero_initialized_context();
   printf("%s counter : %d \r\n", __func__, counter++);
 rc = rcl_init(argc, argv, &support->init_options, &support->context);
   printf("%s counter : %d \r\n", __func__, counter++);
 if (rc != RCL_RET_OK) {
    PRINT_RCLC_ERROR(rclc_init, rcl_init);
    return rc;
  }
  support->allocator = allocator;
  printf("%s counter : %d \r\n", __func__, counter++);

  rc = rcl_clock_init(RCL_STEADY_TIME, &support->clock, support->allocator);
  if (rc != RCL_RET_OK) {
    PRINT_RCLC_ERROR(rclc_init, rcl_clock_init);
  }
   printf("%s counter : %d \r\n", __func__, counter++);
 return rc;
}

Project Link

https://github.com/Needrom/Microros-on-DIscovery-board

any thing help?

About this issue

Original URL
State: closed
Created 3 years ago
Comments: 22 (10 by maintainers)

Most upvoted comments

yeah thanks a lot .I fix that with add global interrupt for usart, and Then it won’t stuck in the state of TX.

setting

and it can connect now

Needrom on Mar 3, 2021