FreeRTPS Publisher

frudp_publish_user_msg.c

bool frudp_publish_user_msg(frudp_pub_t *pub,
    const uint8_t *payload, const uint32_t payload_len)
{
  if (pub->reliable)
  {
    // 高信頼通信は未対応
    FREERTPS_ERROR("user reliable publishing not quite done yet.\n");
    return false;
  }

  // 送信（TX）パケットの準備
  frudp_msg_t *msg = frudp_init_msg((frudp_msg_t *)g_pub_user_tx_buf);
  fr_time_t t = fr_time_now();
  uint16_t submsg_wpos = 0;
  frudp_submsg_t *ts_submsg = (frudp_submsg_t *)&msg->submsgs[submsg_wpos];
  // ヘッダー設定
  ts_submsg->header.id = FRUDP_SUBMSG_ID_INFO_TS;
  ts_submsg->header.flags = FRUDP_FLAGS_LITTLE_ENDIAN;
  ts_submsg->header.len = 8;
  memcpy(ts_submsg->contents, &t, 8);
  submsg_wpos += 4 + 8;

  // メッセージデータのパケットへの埋め込み
  frudp_submsg_data_t *d = (frudp_submsg_data_t *)&msg->submsgs[submsg_wpos];
  d->header.id = FRUDP_SUBMSG_ID_DATA;
  d->header.flags = FRUDP_FLAGS_LITTLE_ENDIAN |
                    FRUDP_FLAGS_DATA_PRESENT;
  d->header.len = sizeof(frudp_submsg_data_t) /*+ 4*/ + payload_len;
  d->extraflags = 0;
  d->octets_to_inline_qos = 16;
  d->writer_sn = pub->next_sn;
  frudp_encapsulation_scheme_t *scheme =
    (frudp_encapsulation_scheme_t *)((uint8_t *)d->data);
  scheme->scheme = freertps_htons(FRUDP_SCHEME_CDR_LE);
  scheme->options = 0;
  uint8_t *outbound_payload = (uint8_t *)(&d->data[4]);

  // メッセージデータのコピー
  memcpy(outbound_payload, payload, payload_len);
  submsg_wpos += 4 + d->header.len;

  const int udp_payload_len = 
    (uint8_t *)&msg->submsgs[submsg_wpos] - (uint8_t *)msg;

  // 複数の送信パケットのライタから該当を探し、そのライタで送信
  for (int i = 0; i < g_frudp_num_writers; i++)
  {
    frudp_writer_t *w = &g_frudp_writers[i];
    if (w->writer_eid.u == pub->writer_eid.u)
    {
      // 現状、マルチキャストとユニキャストの送信に違いなし
      d->reader_id = w->reader_guid.eid;
      d->writer_id = w->writer_eid;
      frudp_part_t *part = frudp_part_find(&w->reader_guid.prefix);
      if (!part)
        continue;
      // パケットの送信
      frudp_tx(part->default_unicast_locator.addr.udp4.addr,
               part->default_unicast_locator.port,
               (const uint8_t *)msg,
               udp_payload_len);
    }
  }

  pub->next_sn.low++;
  return true;
}