Cleaned up a bit and removed a few global vars

src/robot_state_RT.cpp

@@ -0,0 +1,392 @@
+/*
+ * robotStateRT.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> 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.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/robot_state_RT.h"
+
+RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) {
+	version_ = 0.0;
+	time_ = 0.0;
+	q_target_.assign(6, 0.0);
+	qd_target_.assign(6, 0.0);
+	qdd_target_.assign(6, 0.0);
+	i_target_.assign(6, 0.0);
+	m_target_.assign(6, 0.0);
+	q_actual_.assign(6, 0.0);
+	qd_actual_.assign(6, 0.0);
+	i_actual_.assign(6, 0.0);
+	i_control_.assign(6, 0.0);
+	tool_vector_actual_.assign(6, 0.0);
+	tcp_speed_actual_.assign(6, 0.0);
+	tcp_force_.assign(6, 0.0);
+	tool_vector_target_.assign(6, 0.0);
+	tcp_speed_target_.assign(6, 0.0);
+	digital_input_bits_.assign(64, false);
+	motor_temperatures_.assign(6, 0.0);
+	controller_timer_ = 0.0;
+	robot_mode_ = 0.0;
+	joint_modes_.assign(6, 0.0);
+	safety_mode_ = 0.0;
+	tool_accelerometer_values_.assign(3, 0.0);
+	speed_scaling_ = 0.0;
+	linear_momentum_norm_ = 0.0;
+	v_main_ = 0.0;
+	v_robot_ = 0.0;
+	i_robot_ = 0.0;
+	v_actual_.assign(6, 0.0);
+	new_data_available_ = false;
+	pMsg_cond_ = &msg_cond;
+}
+
+bool RobotStateRT::getNewDataAvailable() {
+	return new_data_available_;
+}
+
+void RobotStateRT::finishedReading() {
+	new_data_available_ = false;
+}
+
+double RobotStateRT::ntohd(uint64_t nf) {
+	double x;
+	nf = be64toh(nf);
+	memcpy(&x, &nf, sizeof(x));
+	return x;
+}
+
+std::vector<double> RobotStateRT::unpackVector(uint8_t * buf, int start_index,
+		int nr_of_vals) {
+	uint64_t q;
+	std::vector<double> ret;
+	for (int i = 0; i < nr_of_vals; i++) {
+		memcpy(&q, &buf[start_index + i * sizeof(q)], sizeof(q));
+		ret.push_back(ntohd(q));
+	}
+	return ret;
+}
+
+std::vector<bool> RobotStateRT::unpackDigitalInputBits(int64_t data) {
+	std::vector<bool> ret;
+	for (int i = 0; i < 64; i++) {
+		ret.push_back((data & (1 << i)) >> i);
+	}
+	return ret;
+}
+
+double RobotStateRT::getVersion() {
+	double ret;
+	val_lock_.lock();
+	ret = version_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getTime() {
+	double ret;
+	val_lock_.lock();
+	ret = time_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = q_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQdTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = q_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQddTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = qdd_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getITarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getMTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = m_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = q_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getQdActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = qd_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getIActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getIControl() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = i_control_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolVectorActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_vector_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpSpeedActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_speed_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpForce() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_force_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolVectorTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_vector_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getTcpSpeedTarget() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tcp_speed_target_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<bool> RobotStateRT::getDigitalInputBits() {
+	std::vector<bool> ret;
+	val_lock_.lock();
+	ret = digital_input_bits_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getMotorTemperatures() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = motor_temperatures_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getControllerTimer() {
+	double ret;
+	val_lock_.lock();
+	ret = controller_timer_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getRobotMode() {
+	double ret;
+	val_lock_.lock();
+	ret = robot_mode_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getJointModes() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = joint_modes_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getSafety_mode() {
+	double ret;
+	val_lock_.lock();
+	ret = safety_mode_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getToolAccelerometerValues() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = tool_accelerometer_values_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getSpeedScaling() {
+	double ret;
+	val_lock_.lock();
+	ret = speed_scaling_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getLinearMomentumNorm() {
+	double ret;
+	val_lock_.lock();
+	ret = linear_momentum_norm_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getVMain() {
+	double ret;
+	val_lock_.lock();
+	ret = v_main_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getVRobot() {
+	double ret;
+	val_lock_.lock();
+	ret = v_robot_;
+	val_lock_.unlock();
+	return ret;
+}
+double RobotStateRT::getIRobot() {
+	double ret;
+	val_lock_.lock();
+	ret = i_robot_;
+	val_lock_.unlock();
+	return ret;
+}
+std::vector<double> RobotStateRT::getVActual() {
+	std::vector<double> ret;
+	val_lock_.lock();
+	ret = v_actual_;
+	val_lock_.unlock();
+	return ret;
+}
+void RobotStateRT::unpack(uint8_t * buf) {
+	int64_t digital_input_bits;
+	uint64_t unpack_to;
+	uint16_t offset = 0;
+	val_lock_.lock();
+	if (version_ == 0.0) {
+		uint32_t len;
+		memcpy(&len, &buf[offset], sizeof(len));
+		if (len <= 756) {
+			version_ = 1.6;
+		} else if (len <= 764) {
+			version_ = 1.7;
+		} else if (len <= 812) {
+			version_ = 1.8;
+		} else if (len <= 1044) {
+			version_ = 3.0;
+		}
+	}
+	offset += 4;
+	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+	time_ = ntohd(unpack_to);
+	offset += sizeof(double);
+	q_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qd_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qdd_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	i_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	m_target_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	q_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	qd_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	i_actual_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	if (version_ <= 1.8) {
+		if (version_ != 1.6)
+			tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+		offset += sizeof(double) * (3 + 15);
+		tcp_force_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_actual_ = unpackVector(buf, offset, 6);
+	} else {
+		i_control_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_actual_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_force_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tool_vector_target_ = unpackVector(buf, offset, 6);
+		offset += sizeof(double) * 6;
+		tcp_speed_target_ = unpackVector(buf, offset, 6);
+	}
+	offset += sizeof(double) * 6;
+
+	memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
+	digital_input_bits_ = unpackDigitalInputBits(be64toh(digital_input_bits));
+	offset += sizeof(double);
+	motor_temperatures_ = unpackVector(buf, offset, 6);
+	offset += sizeof(double) * 6;
+	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+	controller_timer_ = ntohd(unpack_to);
+	if (version_ > 1.6) {
+		offset += sizeof(double) * 2;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		robot_mode_ = ntohd(unpack_to);
+		if (version_ > 1.7) {
+			offset += sizeof(double);
+			joint_modes_ = unpackVector(buf, offset, 6);
+		}
+	}
+	if (version_ > 1.8) {
+		offset += sizeof(double) * 6;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		safety_mode_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		tool_accelerometer_values_ = unpackVector(buf, offset, 3);
+		offset += sizeof(double) * 3;
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		speed_scaling_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		linear_momentum_norm_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		v_main_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		v_robot_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
+		i_robot_ = ntohd(unpack_to);
+		offset += sizeof(double);
+		v_actual_ = unpackVector(buf, offset, 6);
+	}
+	val_lock_.unlock();
+	new_data_available_ = true;
+	pMsg_cond_->notify_all();
+
+}
+

src/ur_driver.cpp

@@ -0,0 +1,33 @@
+/*
+ * ur_driver.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> 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.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/ur_driver.h"
+
+UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host,
+		uint safety_count_max) {
+	rt_interface_ = new UrRealtimeCommunication(msg_cond, host,
+			safety_count_max);
+
+}
+
+void UrDriver::start() {
+	rt_interface_->start();
+}
+
+void UrDriver::halt() {
+	rt_interface_->halt();
+}
+
+void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
+		double q5, double acc) {
+	rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
+}
+

src/ur_realtime_communication.cpp

@@ -0,0 +1,107 @@
+/*
+ * ur_realtime_communication.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> 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.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ur_modern_driver/ur_realtime_communication.h"
+
+UrRealtimeCommunication::UrRealtimeCommunication(
+		std::condition_variable& msg_cond, std::string host,
+		uint 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_ = 0;
+	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");
+	printf("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("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;
+}
+

src/ur_ros_wrapper.cpp

@@ -0,0 +1,93 @@
+/*
+ * ur_driver.cpp
+ *
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <thomas.timm.dk@gmail.com> 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.   Thomas Timm Andersen
+ * ----------------------------------------------------------------------------
+ */
+
+#include "ros/ros.h"
+#include <ros/console.h>
+#include "ur_modern_driver/ur_driver.h"
+#include <string.h>
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/WrenchStamped.h"
+#include <thread>
+
+std::condition_variable g_msg_cond;
+
+void publishRTMsg(UrDriver robot, std::vector<std::string> joint_names) {
+	ros::NodeHandle n_rt;
+	ros::Publisher joint_pub = n_rt.advertise<sensor_msgs::JointState>("/joint_states", 1);
+	ros::Publisher wrench_pub = n_rt.advertise<geometry_msgs::WrenchStamped>("/wrench", 1);
+	while (ros::ok()) {
+		sensor_msgs::JointState joint_msg;
+		joint_msg.name = joint_names;
+		geometry_msgs::WrenchStamped wrench_msg;
+		std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
+		std::unique_lock<std::mutex> locker(msg_lock);
+		while (!robot.rt_interface_->robot_state_->getNewDataAvailable()) {
+			g_msg_cond.wait(locker);
+		}
+		joint_msg.header.stamp = ros::Time::now();
+		joint_msg.position = robot.rt_interface_->robot_state_->getQActual();
+		joint_msg.velocity = robot.rt_interface_->robot_state_->getQdActual();
+		joint_msg.effort = robot.rt_interface_->robot_state_->getIActual();
+		joint_pub.publish(joint_msg);
+		std::vector<double> tcp_force = robot.rt_interface_->robot_state_->getTcpForce();
+		wrench_msg.header.stamp = joint_msg.header.stamp;
+		wrench_msg.wrench.force.x = tcp_force[0];
+		wrench_msg.wrench.force.y = tcp_force[1];
+		wrench_msg.wrench.force.z = tcp_force[2];
+		wrench_msg.wrench.torque.x = tcp_force[3];
+		wrench_msg.wrench.torque.y = tcp_force[4];
+		wrench_msg.wrench.torque.z = tcp_force[5];
+		wrench_pub.publish(wrench_msg);
+
+		robot.rt_interface_->robot_state_->finishedReading();
+
+	}
+}
+
+int main(int argc, char **argv) {
+	bool use_sim_time = false;
+	std::string joint_prefix ="";
+	std::string host;
+	std::vector<std::string> joint_names;
+	UrDriver robot(g_msg_cond, host);
+
+
+	ros::init(argc, argv, "ur_driver");
+	ros::NodeHandle n;
+	if (ros::param::get("use_sim_time", use_sim_time)) {
+		ROS_WARN("use_sim_time is set!!");
+	}
+	if (ros::param::get("~prefix", joint_prefix)) {
+		ROS_INFO("Setting prefix to %s", joint_prefix.c_str());
+	}
+	joint_names.push_back(joint_prefix + "shoulder_pan_joint");
+	joint_names.push_back(joint_prefix + "should_lift_joint");
+	joint_names.push_back(joint_prefix + "elbow_joint");
+	joint_names.push_back(joint_prefix + "wrist_1_joint");
+	joint_names.push_back(joint_prefix + "wrist_2_joint");
+	joint_names.push_back(joint_prefix + "wrist_3_joint");
+
+	if (argc > 1) {
+		host = argv[1];
+	} else if (!(ros::param::get("~robot_ip", host))) {
+		ROS_FATAL("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
+		exit(1);
+	}
+	robot.start();
+
+	std::thread rt_publish_thread(publishRTMsg, robot, joint_names);
+	ros::spin();
+	robot.halt();
+	exit(0);
+}