void TopicSender::sendWithFEC()
{
#if WITH_OPENFEC
uint16_t msg_id = m_sender->allocateMessageID();
uint64_t dataSize = sizeof(FECHeader) + m_buf.size();
// If the message fits in a single packet, use that as the buffer size
uint64_t symbolSize;
uint64_t sourceSymbols;
if(dataSize <= FECPacket::MaxDataSize)
{
sourceSymbols = 1;
symbolSize = dataSize;
}
else
{
// We need to pad the data to a multiple of our packet payload size.
sourceSymbols = div_round_up(dataSize, FECPacket::MaxDataSize);
symbolSize = FECPacket::MaxDataSize;
}
ROS_DEBUG("dataSize: %lu, symbol size: %lu, sourceSymbols: %lu", dataSize, symbolSize, sourceSymbols);
uint64_t packetSize = sizeof(FECPacket::Header) + symbolSize;
ROS_DEBUG("=> packetSize: %lu", packetSize);
uint64_t repairSymbols = std::ceil(m_sender->fec() * sourceSymbols);
uint64_t numPackets = sourceSymbols + repairSymbols;
of_session_t* ses = 0;
uint32_t prng_seed = rand();
if(sourceSymbols >= MIN_PACKETS_LDPC)
{
ROS_DEBUG("%s: Choosing LDPC-Staircase codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 1) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create LDPC codec instance", m_topicName.c_str());
return;
}
of_ldpc_parameters_t params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.prng_seed = prng_seed;
params.N1 = 7;
ROS_DEBUG("LDPC seed: 7, 0x%X", params.prng_seed);
if(of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
else
{
ROS_DEBUG("%s: Choosing Reed-Solomon codec", m_topicName.c_str());
if(of_create_codec_instance(&ses, OF_CODEC_REED_SOLOMON_GF_2_M_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not create REED_SOLOMON codec instance", m_topicName.c_str());
return;
}
of_rs_2_m_parameters params;
params.nb_source_symbols = sourceSymbols;
params.nb_repair_symbols = std::ceil(m_sender->fec() * sourceSymbols);
params.encoding_symbol_length = symbolSize;
params.m = 8;
if(of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not set FEC parameters", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
std::vector<uint8_t> packetBuffer(numPackets * packetSize);
std::vector<void*> symbols(sourceSymbols + repairSymbols);
uint64_t writtenData = 0;
// Fill the source packets
for(uint64_t i = 0; i < sourceSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
uint64_t remainingSpace = symbolSize;
symbols[i] = dataPtr;
if(i == 0)
{
// First packet includes the FECHeader
FECHeader* msgHeader = reinterpret_cast<FECHeader*>(dataPtr);
// Fill in header fields
msgHeader->flags = m_flags;
msgHeader->topic_msg_counter = m_inputMsgCounter;
strncpy(msgHeader->topic_name, m_topicName.c_str(), sizeof(msgHeader->topic_name));
if(msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] != 0)
{
ROS_ERROR("Topic '%s' is too long. Please shorten the name.", m_topicName.c_str());
msgHeader->topic_name[sizeof(msgHeader->topic_name)-1] = 0;
}
strncpy(msgHeader->topic_type, m_topicType.c_str(), sizeof(msgHeader->topic_type));
if(msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] != 0)
{
ROS_ERROR("Topic type '%s' is too long. Please shorten the name.", m_topicType.c_str());
msgHeader->topic_type[sizeof(msgHeader->topic_type)-1] = 0;
}
for(int i = 0; i < 4; ++i)
msgHeader->topic_md5[i] = m_md5[i];
dataPtr += sizeof(FECHeader);
remainingSpace -= sizeof(FECHeader);
}
uint64_t chunkSize = std::min(remainingSpace, m_buf.size() - writtenData);
memcpy(dataPtr, m_buf.data() + writtenData, chunkSize);
writtenData += chunkSize;
// Set any padding to zero
if(chunkSize < remainingSpace)
memset(dataPtr + chunkSize, 0, remainingSpace - chunkSize);
}
// Fill the repair packets
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
uint8_t* packetPtr = packetBuffer.data() + i * packetSize;
FECPacket::Header* header = reinterpret_cast<FECPacket::Header*>(packetPtr);
header->msg_id = msg_id;
header->symbol_id = i;
header->symbol_length = symbolSize;
header->source_symbols = sourceSymbols;
header->repair_symbols = repairSymbols;
header->prng_seed = prng_seed;
uint8_t* dataPtr = packetPtr + sizeof(FECPacket::Header);
symbols[i] = dataPtr;
}
for(uint64_t i = sourceSymbols; i < sourceSymbols + repairSymbols; ++i)
{
if(of_build_repair_symbol(ses, symbols.data(), i) != OF_STATUS_OK)
{
ROS_ERROR("%s: Could not build repair symbol", m_topicName.c_str());
of_release_codec_instance(ses);
return;
}
}
// FEC work is done
of_release_codec_instance(ses);
std::vector<unsigned int> packetOrder(numPackets);
std::iota(packetOrder.begin(), packetOrder.end(), 0);
// Send the packets in random order
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::mt19937 mt(seed);
std::shuffle(packetOrder.begin(), packetOrder.end(), mt);
ROS_DEBUG("Sending %d packets", (int)packetOrder.size());
for(unsigned int idx : packetOrder)
{
if(!m_sender->send(packetBuffer.data() + idx * packetSize, packetSize, m_topicName))
return;
}
#else
throw std::runtime_error("Forward error correction requested, but I was not compiled with FEC support...");
#endif
}
int main()
{
int context;
int i,j,k=OF_CODEC_LDPC_STAIRCASE_STABLE;
of_session_t *ses;
of_status_t ret;
// LDPC parameters
of_ldpc_parameters_t params;
params.nb_source_symbols = 1000;
params.nb_repair_symbols = 500;
params.encoding_symbol_length = 1024;
params.prng_seed = rand();
params.N1 = 5;
char **orig_symb_tab;
void **encoding_symb_tab;
//for (k=0;k<CODEC_MAX;k++)
{
// create an instance of an encoder
if ((ret = of_create_codec_instance(&ses,k,OF_ENCODER,1)) != OF_STATUS_OK)
{
printf("error in of_create_codec_instance\n");
goto error;
}
if (of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK) {
printf("error in of_set_fec_parameters\n");
goto error;
}
if( of_set_callback_functions(ses,source_cb,repair_cb,&context) != OF_STATUS_OK) {
printf ("error in of_set_fec_parameters\n");
}
orig_symb_tab = (char**)calloc(params.nb_source_symbols,sizeof(char*));
encoding_symb_tab = (void*)calloc(params.nb_source_symbols+params.nb_repair_symbols,sizeof(void*));
for(i=0;i<params.nb_source_symbols;i++)
{
orig_symb_tab[i] = (char*)calloc(1,params.encoding_symbol_length);
encoding_symb_tab[i] = (void*)orig_symb_tab[i];
for (j=0;j<params.encoding_symbol_length;j++)
{
orig_symb_tab[i][j] = rand();
}
}
for(i=params.nb_source_symbols;i<params.nb_source_symbols+params.nb_repair_symbols;i++)
{
encoding_symb_tab[i] = (void*)calloc(1,params.encoding_symbol_length);
if (of_build_repair_symbol(ses,encoding_symb_tab,i) != OF_STATUS_OK)
{
printf("of_build_repair_symbol: ");;
//goto error;
}
}
if ((ret = of_release_codec_instance(ses)) != OF_STATUS_OK)
{
printf("of_release_codec_instance: ");;
//goto error;
}
//create a decoder instance
if ((ret = of_create_codec_instance(&ses,k,OF_DECODER,1)) != OF_STATUS_OK)
{
printf("of_create_codec_instance: ");
;
//goto error;
}
if (of_set_fec_parameters(ses, (of_parameters_t*)¶ms) != OF_STATUS_OK) {
printf(("of_set_fec_parameters(): "));;
// goto error;
}
if( of_set_callback_functions(ses,source_cb,repair_cb,&context) != OF_STATUS_OK) {
printf (("of_set_fec_parameters(): "));;
}
for (i=380;i<1400;i++) //take 1020 symbols
{
of_decode_with_new_symbol(ses,encoding_symb_tab[i],i);
}
of_finish_decoding(ses);
printf("fini\n");
if ((ret = of_release_codec_instance(ses)) != OF_STATUS_OK)
{
printf("of_release_codec_instance: ");;
//goto error;
}
free(encoding_symb_tab);
free(orig_symb_tab);
}
printf ("nb : %i,%i\n",nb_source,nb_repair);
printf("OK\n");
return 0;
error:
printf("error\n");
return -1;
}
int
main(int argc, char* argv[])
{
of_codec_id_t codec_id; /* identifier of the codec to use */
of_session_t *ses = NULL; /* openfec codec instance identifier */
of_parameters_t *params = NULL; /* structure used to initialize the openfec session */
void** enc_symbols_tab = NULL; /* table containing pointers to the encoding (i.e. source + repair) symbols buffers */
UINT32 symb_sz_32 = SYMBOL_SIZE / 4; /* symbol size in units of 32 bit words */
UINT32 k; /* number of source symbols in the block */
UINT32 n; /* number of encoding symbols (i.e. source + repair) in the block */
UINT32 esi; /* Encoding Symbol ID, used to identify each encoding symbol */
UINT32 i;
UINT32* rand_order = NULL; /* table used to determine a random transmission order. This randomization process
* is essential for LDPC-Staircase optimal performance */
SOCKET so = INVALID_SOCKET; /* UDP socket for server => client communications */
char *pkt_with_fpi = NULL; /* buffer containing a fixed size packet plus a header consisting only of the FPI */
fec_oti_t fec_oti; /* FEC Object Transmission Information as sent to the client */
INT32 lost_after_index= -1; /* all the packets to send after this index are considered as lost during transmission */
SOCKADDR_IN dst_host;
UINT32 ret = -1;
if (argc == 1)
{
/* k value is ommited, so use default */
k = DEFAULT_K;
}
else
{
k = atoi(argv[1]);
}
n = (UINT32)floor((double)k / (double)CODE_RATE);
/* Choose which codec is the most appropriate. If small enough, choose Reed-Solomon (with m=8), otherwise LDPC-Staircase.
* Then finish the openfec session initialization accordingly */
if (n <= 255)
{
/* fill in the code specific part of the of_..._parameters_t structure */
of_rs_2_m_parameters_t *my_params;
printf("\nInitialize a Reed-Solomon over GF(2^m) codec instance, (n, k)=(%u, %u)...\n", n, k);
codec_id = OF_CODEC_REED_SOLOMON_GF_2_M_STABLE;
if ((my_params = (of_rs_2_m_parameters_t *)calloc(1, sizeof(* my_params))) == NULL)
{
OF_PRINT_ERROR(("no memory for codec %d\n", codec_id))
ret = -1;
goto end;
}
my_params->m = 8;
params = (of_parameters_t *) my_params;
}
else
{
/* fill in the code specific part of the of_..._parameters_t structure */
of_ldpc_parameters_t *my_params;
printf("\nInitialize an LDPC-Staircase codec instance, (n, k)=(%u, %u)...\n", n, k);
codec_id = OF_CODEC_LDPC_STAIRCASE_STABLE;
if ((my_params = (of_ldpc_parameters_t *)calloc(1, sizeof(* my_params))) == NULL)
{
OF_PRINT_ERROR(("no memory for codec %d\n", codec_id))
ret = -1;
goto end;
}
my_params->prng_seed = rand();
my_params->N1 = 7;
params = (of_parameters_t *) my_params;
}
params->nb_source_symbols = k; /* fill in the generic part of the of_parameters_t structure */
params->nb_repair_symbols = n - k;
params->encoding_symbol_length = SYMBOL_SIZE;
/* Open and initialize the openfec session now... */
if (ret = of_create_codec_instance(&ses, codec_id, OF_ENCODER, VERBOSITY) != OF_STATUS_OK)
{
OF_PRINT_ERROR(("of_create_codec_instance() failed\n"))
ret = -1;
goto end;
}
if (of_set_fec_parameters(ses, params) != OF_STATUS_OK)
{
OF_PRINT_ERROR(("of_set_fec_parameters() failed for codec_id %d\n", codec_id))
ret = -1;
goto end;
}
/* Allocate and initialize our source symbols...
* In case of a file transmission, the opposite takes place: the file is read and partitionned into a set of k source symbols.
* At the end, it's just equivalent since there is a set of k source symbols that need to be sent reliably thanks to an FEC
* encoding. */
printf("\nFilling source symbols...\n");
if ((enc_symbols_tab = (void**) calloc(n, sizeof(void*))) == NULL) {
OF_PRINT_ERROR(("no memory (calloc failed for enc_symbols_tab, n=%u)\n", n))
ret = -1;
goto end;
}
/* In order to detect corruption, the first symbol is filled with 0x1111..., the second with 0x2222..., etc.
* NB: the 0x0 value is avoided since it is a neutral element in the target finite fields, i.e. it prevents the detection
* of symbol corruption */
for (esi = 0; esi < k; esi++ )
{
if ((enc_symbols_tab[esi] = calloc(symb_sz_32, sizeof(UINT32))) == NULL)
{
OF_PRINT_ERROR(("no memory (calloc failed for enc_symbols_tab[%d])\n", esi))
ret = -1;
goto end;
}
memset(enc_symbols_tab[esi], (char)(esi + 1), SYMBOL_SIZE);
if (VERBOSITY > 1)
{
printf("src[%03d]= ", esi);
dump_buffer_32(enc_symbols_tab[esi], 1);
}
}
/* Now build the n-k repair symbols... */
printf("\nBuilding repair symbols...\n");
for (esi = k; esi < n; esi++)
{
if ((enc_symbols_tab[esi] = (char*)calloc(symb_sz_32, sizeof(UINT32))) == NULL)
{
OF_PRINT_ERROR(("no memory (calloc failed for enc_symbols_tab[%d])\n", esi))
ret = -1;
goto end;
}
if (of_build_repair_symbol(ses, enc_symbols_tab, esi) != OF_STATUS_OK) {
OF_PRINT_ERROR(("ERROR: of_build_repair_symbol() failed for esi=%u\n", esi))
ret = -1;
goto end;
}
if (VERBOSITY > 1)
{
printf("repair[%03d]= ", esi);
dump_buffer_32(enc_symbols_tab[esi], 4);
}
}
/* Randomize the packet order, it's important for LDPC-Staircase codes for instance... */
printf("\nRandomizing transmit order...\n");
if ((rand_order = (UINT32*)calloc(n, sizeof(UINT32))) == NULL)
{
OF_PRINT_ERROR(("no memory (calloc failed for rand_order)\n"))
ret = -1;
goto end;
}
randomize_array(&rand_order, n);
/* Finally initialize the UDP socket and throw our packets... */
if ((so = init_socket(&dst_host)) == INVALID_SOCKET)
{
OF_PRINT_ERROR(("Error initializing socket!\n"))
ret = -1;
goto end;
}
printf("First of all, send the FEC OTI for this object to %s/%d\n", DEST_IP, DEST_PORT);
/* Initialize and send the FEC OTI to the client */
/* convert back to host endianess */
fec_oti.codec_id = htonl(codec_id);
fec_oti.k = htonl(k);
fec_oti.n = htonl(n);
if ((ret = sendto(so, (void*)&fec_oti, sizeof(fec_oti), 0, (SOCKADDR *)&dst_host, sizeof(dst_host))) != sizeof(fec_oti)) {
OF_PRINT_ERROR(("Error while sending the FEC OTI\n"))
ret = -1;
goto end;
}
lost_after_index = n * (1 - LOSS_RATE);
if (lost_after_index < k)
{
OF_PRINT_ERROR(("The loss rate %f is to high: only %u packets will be sent, whereas k=%u\n", LOSS_RATE, lost_after_index, k))
ret = -1;
goto end;
}
printf("Sending %u source and repair packets to %s/%d. All packets sent at index %u and higher are considered as lost\n",
n, DEST_IP, DEST_PORT, lost_after_index);
/* Allocate a buffer where we'll copy each symbol plus its simplistif FPI (in this example consisting only of the ESI).
* This needs to be fixed in real applications, with the actual FPI required for this code. Also doing a memcpy is
* rather suboptimal in terms of performance! */
if ((pkt_with_fpi = malloc(4 + SYMBOL_SIZE)) == NULL)
{
OF_PRINT_ERROR(("no memory (malloc failed for pkt_with_fpi)\n"))
ret = -1;
goto end;
}
for (i = 0; i < n; i++)
{
if (i == lost_after_index)
{
/* the remaining packets are considered as lost, exit loop */
break;
}
/* Add a pkt header wich only countains the ESI, i.e. a 32bits sequence number, in network byte order in order
* to be portable regardless of the local and remote byte endian representation (the receiver will do the
* opposite with ntohl()...) */
*(UINT32*)pkt_with_fpi = htonl(rand_order[i]);
memcpy(4 + pkt_with_fpi, enc_symbols_tab[rand_order[i]], SYMBOL_SIZE);
printf("%05d => sending symbol %u (%s)\n", i + 1, rand_order[i], (rand_order[i] < k) ? "src" : "repair");
if ((ret = sendto(so, pkt_with_fpi, SYMBOL_SIZE + 4, 0, (SOCKADDR *)&dst_host, sizeof(dst_host))) == SOCKET_ERROR)
{
OF_PRINT_ERROR(("sendto() failed!\n"))
ret = -1;
goto end;
}
}
printf( "\nCompleted! %d packets sent successfully.\n", i);
ret = 1;
end:
/* Cleanup everything... */
if (so!= INVALID_SOCKET)
{
close(so);
}
if (ses)
{
of_release_codec_instance(ses);
}
if (params)
{
free(params);
}
if (rand_order) {
free(rand_order);
}
if (enc_symbols_tab)
{
for (esi = 0; esi < n; esi++)
{
if (enc_symbols_tab[esi])
{
free(enc_symbols_tab[esi]);
}
}
free(enc_symbols_tab);
}
return ret;
}