micro_ros_arduino icon indicating copy to clipboard operation
micro_ros_arduino copied to clipboard

Esp32 does not create a parameter and action server

Open shanmugamIoT opened this issue 1 year ago • 10 comments

Issue template

  • Hardware description: ESP32
  • Installation type:github,linux command line
  • Version or commit hash: humble

i use in esp32 as a hardware upload a code with arduino ide and connect a micro_ros_agent via wifi transport.I successfully run a publisher and subscriber nodes and service nodes also but i cant to create a parameter and server in esp32.I have no idea about the error so please help anyone

source code and error is given below

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <unistd.h>
#include <pthread.h>

#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <example_interfaces/action/fibonacci.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); } \
  }
#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); } \
  }

#define MAX_FIBONACCI_ORDER 500
bool goal_completed = false;
int goal_order = 10;

rclc_executor_t executor;


void goal_request_callback(rclc_action_goal_handle_t *goal_handle, bool accepted, void *context) {
  (void)context;

  example_interfaces__action__Fibonacci_SendGoal_Request *request =
    (example_interfaces__action__Fibonacci_SendGoal_Request *)goal_handle->ros_goal_request;
  printf(
    "Goal request (order: %d): %s\n",
    request->goal.order,
    accepted ? "Accepted" : "Rejected");

  if (!accepted) {
    goal_completed = true;
  }
}


void feedback_callback(rclc_action_goal_handle_t *goal_handle, void *ros_feedback, void *context) {
  (void)context;

  example_interfaces__action__Fibonacci_SendGoal_Request *request =
    (example_interfaces__action__Fibonacci_SendGoal_Request *)goal_handle->ros_goal_request;

  printf(
    "Goal Feedback (order: %d) [",
    request->goal.order);

  example_interfaces__action__Fibonacci_FeedbackMessage *feedback =
    (example_interfaces__action__Fibonacci_FeedbackMessage *)ros_feedback;

  for (size_t i = 0; i < feedback->feedback.sequence.size; i++) {
    printf("%d ", feedback->feedback.sequence.data[i]);
  }
  printf("\b]\n");
}

void result_request_callback(
  rclc_action_goal_handle_t *goal_handle, void *ros_result_response,
  void *context) {
  (void)context;

  static size_t goal_count = 1;

  example_interfaces__action__Fibonacci_SendGoal_Request *request =
    (example_interfaces__action__Fibonacci_SendGoal_Request *)goal_handle->ros_goal_request;

  printf(
    "Goal Result (order: %d) [",
    request->goal.order);

  example_interfaces__action__Fibonacci_GetResult_Response *result =
    (example_interfaces__action__Fibonacci_GetResult_Response *)ros_result_response;

  if (result->status == GOAL_STATE_SUCCEEDED) {
    for (size_t i = 0; i < result->result.sequence.size; i++) {
      printf("%d ", result->result.sequence.data[i]);
    }
  } else if (result->status == GOAL_STATE_CANCELED) {
    printf("CANCELED ");
  } else {
    printf("ABORTED ");
  }

  printf("\b]\n");

  goal_completed = true;
}


void cancel_request_callback(
  rclc_action_goal_handle_t *goal_handle, bool cancelled,
  void *context) {
  (void)context;

  example_interfaces__action__Fibonacci_SendGoal_Request *request =
    (example_interfaces__action__Fibonacci_SendGoal_Request *)goal_handle->ros_goal_request;

  printf(
    "Goal cancel request (order: %d): %s\n",
    request->goal.order,
    cancelled ? "Accepted" : "Rejected");

  if (cancelled) {
    goal_completed = true;
  }
}

void setup() {
  // put your setup code here, to run once:
  set_microros_wifi_transports("Kevell-iot","Enter@123","10.10.11.78",8888);
  Serial.begin(115200);
  Serial.println("Starting....");

  rcl_allocator_t allocator = rcl_get_default_allocator();

  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
  rcl_init_options_init(&init_options, allocator);
  rcl_init_options_set_domain_id(&init_options, 10);


  rclc_support_t support;

  rcl_ret_t rf = rclc_support_init_with_options(&support, 0, NULL,&init_options, &allocator);
  if(rf!=RCL_RET_OK)
  {
    Serial.println("support not create");
  }
  else
  {
    Serial.println("support created successfully...");
  }
  rcl_node_t node;
  rcl_ret_t rc=rclc_node_init_default(&node, "fibonacci_action_client_rcl", "", &support);
  if(rc!=RCL_RET_OK)
  {
    Serial.println("node did not create");
  }
  else
  {
    Serial.println("node created successfully...");
  }


  rclc_action_client_t action_client;
  rcl_ret_t rb=rclc_action_client_init_default(
      &action_client,
      &node,
      ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci),
      "fibonacci");

  if(rb!=RCL_RET_OK)
  {
    Serial.println("action did not create....");
    Serial.println(rb);
  }
  else
  {
    Serial.println("action created successfully....");
  }    

  
  rclc_executor_init(&executor, &support.context, 1, &allocator);

  example_interfaces__action__Fibonacci_FeedbackMessage ros_feedback;
  example_interfaces__action__Fibonacci_GetResult_Response ros_result_response;
  example_interfaces__action__Fibonacci_SendGoal_Request ros_goal_request;


  ros_feedback.feedback.sequence.capacity = MAX_FIBONACCI_ORDER;
  ros_feedback.feedback.sequence.size = 0;
  ros_feedback.feedback.sequence.data = (int32_t *)malloc(
    ros_feedback.feedback.sequence.capacity * sizeof(int32_t));


  ros_result_response.result.sequence.capacity = MAX_FIBONACCI_ORDER;
  ros_result_response.result.sequence.size = 0;
  ros_result_response.result.sequence.data = (int32_t *)malloc(
    ros_result_response.result.sequence.capacity * sizeof(int32_t));


  RCCHECK(
    rclc_executor_add_action_client(
      &executor,
      &action_client,
      10,
      &ros_result_response,
      &ros_feedback,
      goal_request_callback,
      feedback_callback,
      result_request_callback,
      cancel_request_callback,

      (void *)&action_client));


  sleep(2);
  ros_goal_request.goal.order = goal_order;
  RCCHECK(rclc_action_send_goal_request(&action_client, &ros_goal_request, NULL));

  while (!goal_completed) {
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    usleep(100000);
  }

  RCCHECK(rclc_action_client_fini(&action_client, &node))
  RCCHECK(rcl_node_fini(&node))

  return 0;
}

void loop() 
{
  // put your main code here, to run repeatedly:
    rcl_ret_t rv=rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10));
    usleep(100000);
    if(rv!=RCL_RET_OK)
    {
      Serial.println("execute did not happen");
    }
    else
    {
      Serial.println("excute happen....");
    }
}

above source code give a run time error

error : action did not create and error code of rcl_ret_t = 1

shanmugamIoT avatar May 20 '24 13:05 shanmugamIoT