<commit_before>// Sends a single topic
// Author: <NAME> <<EMAIL>>

#include "topic_sender.h"
#include "udp_sender.h"
#include "udp_packet.h"

#include <bzlib.h>

#include <nimbro_topic_transport/CompressedMsg.h>

#include <boost/algorithm/string/replace.hpp>

#include <random>
#include <algorithm>
#include <chrono>

#if WITH_OPENFEC
extern "C"
{
#include <of_openfec_api.h>
}
#endif

#include <fcntl.h>

namespace nimbro_topic_transport
{

TopicSender::TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable)
 : m_sender(sender)
 , m_flags(flags)
 , m_updateBuf(true)
 , m_msgCounter(0)
 , m_inputMsgCounter(0)
 , m_directTransmission(true)
 , m_enable(escapeTopicName(topic), enable)
{
	ROS_INFO_STREAM("Subscribing to" << topic);
	m_subscriber = nh->subscribe(topic, 1, &TopicSender::handleData, this);
	m_topicName = topic;

	m_durationBetweenPackets = ros::Duration(1.0 / rate);

	if(resend)
	{
		m_resendTimer = nh->createTimer(m_durationBetweenPackets, boost::bind(&TopicSender::resend, this));
		m_resendTimer.start();
	}
}

TopicSender::~TopicSender()
{
	ROS_DEBUG("Topic '%s': Sent %d messages", m_topicName.c_str(), m_msgCounter);
}

void TopicSender::send()
{
	if(m_updateBuf)
	{
		boost::lock_guard<boost::mutex> lock(m_dataMutex);

		if(!m_lastData)
			return;

		// If the data was sent over our CompressedMsg format, do not recompress it
		if((m_flags & UDP_FLAG_COMPRESSED) && m_lastData->getDataType() == "nimbro_topic_transport/CompressedMsg")
		{
			CompressedMsg::Ptr compressed = m_lastData->instantiate<CompressedMsg>();
			if(!compressed)
			{
				ROS_ERROR("Could not instantiate CompressedMsg");
				return;
			}

			m_buf.swap(compressed->data);
			memcpy(m_md5, compressed->md5.data(), sizeof(m_md5));
			m_topicType = compressed->type;
		}
		else
		{
			m_buf.resize(m_lastData->size());
			m_lastData->write(*this);

			if(m_flags & UDP_FLAG_COMPRESSED)
			{
				unsigned int len = m_buf.size() + m_buf.size() / 100 + 1200;
				m_compressionBuf.resize(len);
				int ret = BZ2_bzBuffToBuffCompress((char*)m_compressionBuf.data(), &len, (char*)m_buf.data(), m_buf.size(), 3, 0, 30);
				if(ret == BZ_OK)
				{
					m_buf.swap(m_compressionBuf);
					m_buf.resize(len);
				}
				else
				{
					ROS_ERROR("Could not compress data, sending uncompressed");
				}
			}

			std::string md5 = m_lastData->getMD5Sum();
			for(int i = 0; i < 4; ++i)
			{
				std::string md5_part = md5.substr(8*i, 8);
				uint32_t md5_num = strtol(md5_part.c_str(), 0, 16);
				m_md5[i] = md5_num;
			}

			m_topicType = m_lastData->getDataType();
		}

		m_updateBuf = false;
	}

	// Do we want to do forward error correction?
	if(m_sender->fec() != 0.0)
	{
		sendWithFEC();
	}
	else
	{
		sendWithoutFEC();
	}

	m_msgCounter++;
}

inline uint64_t div_round_up(uint64_t a, uint64_t b)
{
	return (a + b - 1) / b;
}

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_INFO("=> packetSize: %lu", packetSize);

	uint64_t repairSymbols = std::ceil(m_sender->fec() * sourceSymbols);

	uint64_t numPackets = sourceSymbols + repairSymbols;

	of_session_t* ses = 0;
	if(sourceSymbols >= MIN_PACKETS_LDPC)
	{
		if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
		{
			ROS_ERROR("Could not create LDPC codec instance");
			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;<commit_msg>nimbro_topic_transport: disable debug msgs<commit_after>// Sends a single topic
// Author: <NAME> <<EMAIL>>

#include "topic_sender.h"
#include "udp_sender.h"
#include "udp_packet.h"

#include <bzlib.h>

#include <nimbro_topic_transport/CompressedMsg.h>

#include <boost/algorithm/string/replace.hpp>

#include <random>
#include <algorithm>
#include <chrono>

#if WITH_OPENFEC
extern "C"
{
#include <of_openfec_api.h>
}
#endif

#include <fcntl.h>

namespace nimbro_topic_transport
{

TopicSender::TopicSender(UDPSender* sender, ros::NodeHandle* nh, const std::string& topic, double rate, bool resend, int flags, bool enable)
 : m_sender(sender)
 , m_flags(flags)
 , m_updateBuf(true)
 , m_msgCounter(0)
 , m_inputMsgCounter(0)
 , m_directTransmission(true)
 , m_enable(escapeTopicName(topic), enable)
{
	ROS_INFO_STREAM("Subscribing to" << topic);
	m_subscriber = nh->subscribe(topic, 1, &TopicSender::handleData, this);
	m_topicName = topic;

	m_durationBetweenPackets = ros::Duration(1.0 / rate);

	if(resend)
	{
		m_resendTimer = nh->createTimer(m_durationBetweenPackets, boost::bind(&TopicSender::resend, this));
		m_resendTimer.start();
	}
}

TopicSender::~TopicSender()
{
	ROS_DEBUG("Topic '%s': Sent %d messages", m_topicName.c_str(), m_msgCounter);
}

void TopicSender::send()
{
	if(m_updateBuf)
	{
		boost::lock_guard<boost::mutex> lock(m_dataMutex);

		if(!m_lastData)
			return;

		// If the data was sent over our CompressedMsg format, do not recompress it
		if((m_flags & UDP_FLAG_COMPRESSED) && m_lastData->getDataType() == "nimbro_topic_transport/CompressedMsg")
		{
			CompressedMsg::Ptr compressed = m_lastData->instantiate<CompressedMsg>();
			if(!compressed)
			{
				ROS_ERROR("Could not instantiate CompressedMsg");
				return;
			}

			m_buf.swap(compressed->data);
			memcpy(m_md5, compressed->md5.data(), sizeof(m_md5));
			m_topicType = compressed->type;
		}
		else
		{
			m_buf.resize(m_lastData->size());
			m_lastData->write(*this);

			if(m_flags & UDP_FLAG_COMPRESSED)
			{
				unsigned int len = m_buf.size() + m_buf.size() / 100 + 1200;
				m_compressionBuf.resize(len);
				int ret = BZ2_bzBuffToBuffCompress((char*)m_compressionBuf.data(), &len, (char*)m_buf.data(), m_buf.size(), 3, 0, 30);
				if(ret == BZ_OK)
				{
					m_buf.swap(m_compressionBuf);
					m_buf.resize(len);
				}
				else
				{
					ROS_ERROR("Could not compress data, sending uncompressed");
				}
			}

			std::string md5 = m_lastData->getMD5Sum();
			for(int i = 0; i < 4; ++i)
			{
				std::string md5_part = md5.substr(8*i, 8);
				uint32_t md5_num = strtol(md5_part.c_str(), 0, 16);
				m_md5[i] = md5_num;
			}

			m_topicType = m_lastData->getDataType();
		}

		m_updateBuf = false;
	}

	// Do we want to do forward error correction?
	if(m_sender->fec() != 0.0)
	{
		sendWithFEC();
	}
	else
	{
		sendWithoutFEC();
	}

	m_msgCounter++;
}

inline uint64_t div_round_up(uint64_t a, uint64_t b)
{
	return (a + b - 1) / b;
}

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;
	if(sourceSymbols >= MIN_PACKETS_LDPC)
	{
		if(of_create_codec_instance(&ses, OF_CODEC_LDPC_STAIRCASE_STABLE, OF_ENCODER, 0) != OF_STATUS_OK)
		{
			ROS_ERROR("Could not create LDPC codec instance");
			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;
{'fraction_non_alphanumeric': 0.13722953904045154, 'fraction_numerical': 0.014816556914393226, 'mean_word_length': 4.685160427807487, 'pattern_counts': {'":': 0, '<': 37, '<?xml version=': 0, '>': 63, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3306127', 'n_tokens_mistral': 3627, 'n_tokens_neox': 3230, 'n_words': 632}