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*)&params) != 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*)&params) != 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
}
Beispiel #2
0
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*)&params) != 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*)&params) != 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;
}
Beispiel #3
0
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;
}