<commit_before>/*
 * ur_realtime_communication.cpp
 *
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <<EMAIL>> wrote this file.  As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.   <NAME>
 * ----------------------------------------------------------------------------
 */

#include "ur_modern_driver/ur_realtime_communication.h"

UrRealtimeCommunication::UrRealtimeCommunication(
		std::condition_variable& msg_cond, std::string host,
		unsigned int safety_count_max) :
		SAMPLETIME_(0.008) {
	robot_state_ = new RobotStateRT(msg_cond);
	bzero((char *) &serv_addr_, sizeof(serv_addr_));
	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd_ < 0) {
		printf("ERROR opening socket");
		exit(0);
	}
	server_ = gethostbyname(host.c_str());
	if (server_ == NULL) {
		printf("ERROR, no such host\n");
		exit(0);
	}
	serv_addr_.sin_family = AF_INET;
	bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
	serv_addr_.sin_port = htons(30003);
	flag_ = 1;
	setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
	setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
	connected_ = false;
	keepalive_ = false;
	safety_count_ = safety_count_max+1;
	safety_count_max_ = safety_count_max;
}

void UrRealtimeCommunication::start() {
	keepalive_ = true;
	if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
			< 0)
		printf("Error connecting to RT port 30003");
	printf("Realtime port: Connecting...\n");
	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
}

void UrRealtimeCommunication::halt() {
	keepalive_ = false;
	comThread_.join();
}

void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
	if (inp.back() != '\n') {
		inp.append("\n");
	}
	command_string_lock_.lock();
	command_ += inp;
	command_string_lock_.unlock();
}

void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
		double q3, double q4, double q5, double acc) {
	char cmd[1024];
	sprintf(cmd,
			"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %1.5f,0.02)\n",
			q0, q1, q2, q3, q4, q5, acc);
	addCommandToQueue((std::string) (cmd));
	if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
		//If a joint speed is set, make sure we stop it again after some time if the user doesn't
		safety_count_ = 0;
	}
}

void UrRealtimeCommunication::run() {
	uint8_t buf[2048];
	bzero(buf, 2048);
	printf("Realtime port: Got connection\n");
	connected_ = true;
	while (keepalive_) {
		read(sockfd_, buf, 2048);
		setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
				sizeof(int));
		robot_state_->unpack(buf);
		command_string_lock_.lock();
		if (command_.length() != 0) {
			write(sockfd_, command_.c_str(), command_.length());
			command_ = "";
		}
		if (safety_count_ == safety_count_max_) {
			setSpeed(0., 0., 0., 0., 0., 0.);
			write(sockfd_, command_.c_str(), command_.length());
			command_ = "";
		}
		safety_count_ += 1;
		command_string_lock_.unlock();

	}
}

void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
	safety_count_max_ = inp;
}
<commit_msg>Close RT socket<commit_after>/*
 * ur_realtime_communication.cpp
 *
 * ----------------------------------------------------------------------------
 * "THE BEER-WARE LICENSE" (Revision 42):
 * <<EMAIL>> wrote this file.  As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return.   <NAME>
 * ----------------------------------------------------------------------------
 */

#include "ur_modern_driver/ur_realtime_communication.h"

UrRealtimeCommunication::UrRealtimeCommunication(
		std::condition_variable& msg_cond, std::string host,
		unsigned int safety_count_max) :
		SAMPLETIME_(0.008) {
	robot_state_ = new RobotStateRT(msg_cond);
	bzero((char *) &serv_addr_, sizeof(serv_addr_));
	sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd_ < 0) {
		printf("ERROR opening socket");
		exit(0);
	}
	server_ = gethostbyname(host.c_str());
	if (server_ == NULL) {
		printf("ERROR, no such host\n");
		exit(0);
	}
	serv_addr_.sin_family = AF_INET;
	bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length);
	serv_addr_.sin_port = htons(30003);
	flag_ = 1;
	setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int));
	setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int));
	connected_ = false;
	keepalive_ = false;
	safety_count_ = safety_count_max+1;
	safety_count_max_ = safety_count_max;
}

void UrRealtimeCommunication::start() {
	keepalive_ = true;
	if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_))
			< 0)
		printf("Error connecting to RT port 30003");
	printf("Realtime port: Connecting...\n");
	comThread_ = std::thread(&UrRealtimeCommunication::run, this);
}

void UrRealtimeCommunication::halt() {
	keepalive_ = false;
	comThread_.join();
}

void UrRealtimeCommunication::addCommandToQueue(std::string inp) {
	if (inp.back() != '\n') {
		inp.append("\n");
	}
	command_string_lock_.lock();
	command_ += inp;
	command_string_lock_.unlock();
}

void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2,
		double q3, double q4, double q5, double acc) {
	char cmd[1024];
	sprintf(cmd,
			"speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %1.5f,0.02)\n",
			q0, q1, q2, q3, q4, q5, acc);
	addCommandToQueue((std::string) (cmd));
	if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) {
		//If a joint speed is set, make sure we stop it again after some time if the user doesn't
		safety_count_ = 0;
	}
}

void UrRealtimeCommunication::run() {
	uint8_t buf[2048];
	bzero(buf, 2048);
	printf("Realtime port: Got connection\n");
	connected_ = true;
	while (keepalive_) {
		read(sockfd_, buf, 2048);
		setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_,
				sizeof(int));
		robot_state_->unpack(buf);
		command_string_lock_.lock();
		if (command_.length() != 0) {
			write(sockfd_, command_.c_str(), command_.length());
			command_ = "";
		}
		if (safety_count_ == safety_count_max_) {
			setSpeed(0., 0., 0., 0., 0., 0.);
			write(sockfd_, command_.c_str(), command_.length());
			command_ = "";
		}
		safety_count_ += 1;
		command_string_lock_.unlock();

	}
	close(sockfd_);
}

void UrRealtimeCommunication::setSafetyCountMax(uint inp) {
	safety_count_max_ = inp;
}

{'fraction_non_alphanumeric': 0.19361157149314448, 'fraction_numerical': 0.027120687057405453, 'mean_word_length': 3.6582456140350876, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1059928', 'n_tokens_mistral': 2640, 'n_tokens_neox': 2427, 'n_words': 569}