Infinite loop 揭示

以定位模块为例

1．  在定位文件localize.c的main函数中

  carmen_robot_subscribe_frontlaser_message(&front_laser, (carmen_handler_t)

                                       robot_frontlaser_handler,

                                       CARMEN_SUBSCRIBE_LATEST);

2．  在robot/robot_interface.c文件中

carmen_robot_subscribe_frontlaser_message(carmen_robot_laser_message *laser,

                                     carmen_handler_t handler,

                                     carmen_subscribe_t subscribe_how)

{

  carmen_subscribe_message(CARMEN_ROBOT_FRONTLASER_NAME,

                        CARMEN_ROBOT_FRONTLASER_FMT,

                        laser, sizeof(carmen_robot_laser_message),

                        handler, subscribe_how);

}

3．  在global/ipc_wrapper.c文件中

  /** carmen_subscribe_message - generic IPC subscribe function.  It attaches

     a callback and a memory destination to a particular IPC message. **/

void

carmen_subscribe_message(char *message_name, char *message_fmt,

                      void *message_mem, int message_size,

                      carmen_handler_t handler,

                      carmen_subscribe_t subscribe_how)

{

  IPC_RETURN_TYPE err = IPC_OK;

  IPC_CONTEXT_PTR context;

  carmen_message_list_p mark;

  int i;

  /* define the message, just in case */

  err = IPC_defineMsg(message_name, IPC_VARIABLE_LENGTH, message_fmt);

  carmen_test_ipc_exit(err, "Could not define message", message_name);

  /* look for a matching message name */

  mark = message_list;

  while(mark != NULL && strcmp(message_name, mark->message_name))

    mark = mark->next;

  /* if no match, add message name to the list */

  if(mark == NULL) {

    mark = (carmen_message_list_p)calloc(1, sizeof(carmen_message_list_t));

    carmen_test_alloc(mark);

    mark->message_name = (char *)calloc(1, strlen(message_name) + 1);

    carmen_test_alloc(mark->message_name);

    strcpy(mark->message_name, message_name);

    mark->num_callbacks = 0;

    mark->callback = NULL;

    mark->next = message_list;

    message_list = mark;

    /* NEW LOGGER BEGIN */

    if(outfile != NULL)

      carmen_write_formatter_message(message_name, message_fmt, message_size);

    /* NEW LOGGER END */

  }

  /* make sure that the callback isn't already in there */

  context = IPC_getContext();

  i = 0;

  while(i < mark->num_callbacks && (mark->callback[i].handler != handler ||

                                mark->callback[i].context != context))

    i++;

  if(i == mark->num_callbacks) {

    mark->num_callbacks++;

    mark->callback = (carmen_callback_p)realloc(mark->callback,

                                        mark->num_callbacks *

                                        sizeof(carmen_callback_t));

    carmen_test_alloc(mark->callback);

    /* allocate message memory if necessary */

    if(message_mem) {

      mark->callback[mark->num_callbacks - 1].data = message_mem;

      memset(message_mem, 0, message_size);

    }

    else {

      mark->callback[mark->num_callbacks - 1].data = calloc(1, message_size);

      carmen_test_alloc(mark->callback[mark->num_callbacks - 1].data);

    }

    mark->callback[mark->num_callbacks - 1].context = context;

    mark->callback[mark->num_callbacks - 1].handler = handler;

    mark->callback[mark->num_callbacks - 1].first = 1;

    mark->callback[mark->num_callbacks - 1].message_size = message_size;

  }

  if(infile == NULL) {

    err = IPC_subscribe(message_name, carmen_generic_handler, mark);

    if(subscribe_how == CARMEN_SUBSCRIBE_LATEST)

      IPC_setMsgQueueLength(message_name, 1);

    else

      IPC_setMsgQueueLength(message_name, 100);

    carmen_test_ipc(err, "Could not subscribe", message_name);

  }

}

4．  其中IPC_subscribe(message_name, carmen_generic_handler, mark)触发carmen_generic_handler回调函数如下：将接受的数据转换成原来的格式。

static void

carmen_generic_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData,

                     void *clientData)

{

  IPC_RETURN_TYPE err = IPC_OK;

  IPC_CONTEXT_PTR context;

  FORMATTER_PTR formatter;

  carmen_message_list_p mark = (carmen_message_list_p)clientData;

  int i, n;

  current_msgRef = msgRef;

  /* NEW LOGGER BEGIN */

  if(outfile != NULL)

    carmen_write_data_message(callData, IPC_dataLength(msgRef),

                           mark->message_name);

  /* NEW LOGGER END */

  i = 0;

  context = IPC_getContext();

  while(i < mark->num_callbacks) {

    if(mark->callback[i].context == context) {

      if(mark->callback[i].data) {

       formatter = IPC_msgInstanceFormatter(msgRef);

       if(!mark->callback[i].first)

         IPC_freeDataElements(formatter, mark->callback[i].data);

       err = IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData,

                             mark->callback[i].data,

                             mark->callback[i].message_size);

       mark->callback[i].first = 0;

      }

      n = mark->num_callbacks;

      if(mark->callback[i].handler && carmen_use_handlers)

       mark->callback[i].handler(mark->callback[i].data);

      if(mark->num_callbacks >= n)

       i++;

    }

    else

      i++;

  }

  IPC_freeByteArray(callData);

  carmen_test_ipc_return(err, "Could not unmarshall",

                    IPC_msgInstanceName(msgRef));

}