Infinite loop 揭示

lzhw_lucky

以定位模块为例
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 num_callbacks && (mark->callback.handler != handler ||
                                mark->callback.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 num_callbacks) {
    if(mark->callback.context == context) {
      if(mark->callback.data) {
       formatter = IPC_msgInstanceFormatter(msgRef);
       if(!mark->callback.first)
         IPC_freeDataElements(formatter, mark->callback.data);
       err = IPC_unmarshallData(IPC_msgInstanceFormatter(msgRef), callData,
                             mark->callback.data,
                             mark->callback.message_size);
      
       mark->callback.first = 0;
      }
      n = mark->num_callbacks;
      if(mark->callback.handler && carmen_use_handlers)
       mark->callback.handler(mark->callback.data);
      if(mark->num_callbacks >= n)
       i++;
    }
    else
      i++;
  }
  IPC_freeByteArray(callData);
  carmen_test_ipc_return(err, "Could not unmarshall",
                    IPC_msgInstanceName(msgRef));
}



本文来自ChinaUnix博客，如果查看原文请点：http://blog.chinaunix.net/u/16942/showart_271262.html