micro-ROS / micro_ros_setup

Support macros for building micro-ROS-based firmware.
Apache License 2.0
357 stars 132 forks source link

action server #644

Open martantoine opened 1 year ago

martantoine commented 1 year ago

Hello

Issue template

Steps to reproduce the issue

  1. Create a custom action in a package following ROS2 humble instructions
  2. copy the package src folder in the extra packages following instructions on this page
  3. take code inspiration from ros-demo-actio-server
  4. "Final" code, I only put part of the code that are revelant:

    void StartDefaultTask(void *argument)
    {
    /* USER CODE BEGIN StartDefaultTask */
    /* Infinite loop */
    // micro-ROS configuration
    
    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__); 
    }
    
    // micro-ROS node
    rcl_allocator_t allocator = rcl_get_default_allocator();
    
    rclc_support_t support;
    RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
    rcl_node_t node;
    RCCHECK(rclc_node_init_default(&node, "DuploBuster", "", &support)); 
    
    // micro-ROS action service
    while(ROSIDL_GET_ACTION_TYPE_SUPPORT(duplobuster_interface, Order) == NULL);
    rclc_action_server_t action_server;
    volatile int qqq = rclc_action_server_init_default(
    &action_server,
    &node,
    &support,
    ROSIDL_GET_ACTION_TYPE_SUPPORT(duplobuster_interface, Order),
    "Order"
    );
    
    // Create executor
    rclc_executor_t executor;
    volatile wtf2 = rclc_executor_init(&executor, &support.context, 1, &allocator);
    
    duplobuster_interface__action__Order_SendGoal_Request ros_goal_request[10];
    volatile int wtf3 =  rclc_executor_add_action_server(
    &executor,
    &action_server,
    10,
    ros_goal_request,
    sizeof(duplobuster_interface__action__Order_SendGoal_Request),
    handle_goal,
    handle_cancel,
    (void *) &action_server
    );
    
    // create publisher
    std_msgs__msg__Int32 msg;
    rcl_publisher_t publisher;
    rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "cubemx_publisher");
    
    msg.data = 0;
    
    for(;;)
    {
      HAL_GPIO_TogglePin(PIN_TEST_GPIO_Port, PIN_TEST_Pin);
    
      rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    if (ret != RCL_RET_OK)
    {
      printf("Error publishing (line %d)\n", __LINE__); 
    }
    
    msg.data++;
    osDelay(10);
    }
    
    rcl_node_fini(&node);
    /* USER CODE END StartDefaultTask */
    }

    Expected behavior

    I should be able to discover the action wen runing ros2 run action list

Actual behavior

I don't have anyting when running ros2 run action list even though I still can see the node and the publisher

Additional information

The code compiles without any error / warning, it's a runtime issue

Thank you

Acuadros95 commented 1 year ago

You need to modify the values on the colcon.meta file and then rebuild the library, just as detailed on the Customizing the micro-ROS library section.

For an action server you will need atleast 3 services and 2 publishers, that are used internally by the library.