Cleaned up a bit and removed a few global vars

include/ur_modern_driver/robot_state_RT.h

@@ -0,0 +1,101 @@
+/*
+ * robotStateRT.h
+ *
+ * ----------------------------------------------------------------------------
+ * "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
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef ROBOTSTATERT_H_
+#define ROBOTSTATERT_H_
+
+#include <inttypes.h>
+#include <vector>
+#include <stdlib.h>
+#include <string.h>
+#include <mutex>
+#include <condition_variable>
+
+class RobotStateRT {
+private:
+	double version_; //protocol version
+
+	double time_; //Time elapsed since the controller was started
+	std::vector<double> q_target_; //Target joint positions
+	std::vector<double> qd_target_; //Target joint velocities
+	std::vector<double> qdd_target_; //Target joint accelerations
+	std::vector<double> i_target_; //Target joint currents
+	std::vector<double> m_target_; //Target joint moments (torques)
+	std::vector<double> q_actual_; //Actual joint positions
+	std::vector<double> qd_actual_; //Actual joint velocities
+	std::vector<double> i_actual_; //Actual joint currents
+	std::vector<double> i_control_; //Joint control currents
+	std::vector<double> tool_vector_actual_; //Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+	std::vector<double> tcp_speed_actual_; //Actual speed of the tool given in Cartesian coordinates
+	std::vector<double> tcp_force_; //Generalised forces in the TC
+	std::vector<double> tool_vector_target_; //Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+	std::vector<double> tcp_speed_target_; //Target speed of the tool given in Cartesian coordinates
+	std::vector<bool> digital_input_bits_; //Current state of the digital inputs. NOTE: these are bits encoded as int64_t, e.g. a value of 5 corresponds to bit 0 and bit 2 set high
+	std::vector<double> motor_temperatures_; //Temperature of each joint in degrees celsius
+	double controller_timer_; //Controller realtime thread execution time
+	double robot_mode_; //Robot mode
+	std::vector<double> joint_modes_; //Joint control modes
+	double safety_mode_; //Safety mode
+	std::vector<double> tool_accelerometer_values_; //Tool x,y and z accelerometer values (software version 1.7)
+	double speed_scaling_; //Speed scaling of the trajectory limiter
+	double linear_momentum_norm_; //Norm of Cartesian linear momentum
+	double v_main_; //Masterboard: Main voltage
+	double v_robot_; //Matorborad: Robot voltage (48V)
+	double i_robot_; //Masterboard: Robot current
+	std::vector<double> v_actual_; //Actual joint voltages
+
+	std::mutex val_lock_; // Locks the variables while unpack parses data;
+
+	std::condition_variable* pMsg_cond_; //Signals that new vars are available
+	bool new_data_available_; //to avoid spurious wakes
+
+	std::vector<double> unpackVector(uint8_t * buf, int start_index,
+			int nr_of_vals);
+	std::vector<bool> unpackDigitalInputBits(int64_t data);
+	double ntohd(uint64_t nf);
+
+public:
+	RobotStateRT(std::condition_variable& msg_cond);
+	double getVersion();
+	double getTime();
+	std::vector<double> getQTarget();
+	std::vector<double> getQdTarget();
+	std::vector<double> getQddTarget();
+	std::vector<double> getITarget();
+	std::vector<double> getMTarget();
+	std::vector<double> getQActual();
+	std::vector<double> getQdActual();
+	std::vector<double> getIActual();
+	std::vector<double> getIControl();
+	std::vector<double> getToolVectorActual();
+	std::vector<double> getTcpSpeedActual();
+	std::vector<double> getTcpForce();
+	std::vector<double> getToolVectorTarget();
+	std::vector<double> getTcpSpeedTarget();
+	std::vector<bool> getDigitalInputBits();
+	std::vector<double> getMotorTemperatures();
+	double getControllerTimer();
+	double getRobotMode();
+	std::vector<double> getJointModes();
+	double getSafety_mode();
+	std::vector<double> getToolAccelerometerValues();
+	double getSpeedScaling();
+	double getLinearMomentumNorm();
+	double getVMain();
+	double getVRobot();
+	double getIRobot();
+	bool getNewDataAvailable();
+	void finishedReading();
+	std::vector<double> getVActual();
+	void unpack(uint8_t * buf);
+};
+
+#endif /* ROBOTSTATERT_H_ */

include/ur_modern_driver/ur_driver.h

@@ -0,0 +1,32 @@
+/*
+ * ur_driver
+ *
+ * ----------------------------------------------------------------------------
+ * "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
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef UR_DRIVER_H_
+#define UR_DRIVER_H_
+
+#include <mutex>
+#include <condition_variable>
+#include "ur_realtime_communication.h"
+
+class UrDriver {
+public:
+	UrRealtimeCommunication* rt_interface_;
+
+	UrDriver(std::condition_variable& msg_cond, std::string host,
+			uint safety_count_max = 12);
+	void start();
+	void halt();
+	void setSpeed(double q0, double q1, double q2, double q3, double q4,
+			double q5, double acc = 100.);
+
+};
+
+#endif /* UR_DRIVER_H_ */

include/ur_modern_driver/ur_realtime_communication.h

@@ -0,0 +1,62 @@
+/*
+ * ur_realtime_communication.h
+ *
+ * ----------------------------------------------------------------------------
+ * "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
+ * ----------------------------------------------------------------------------
+ */
+
+#ifndef UR_REALTIME_COMMUNICATION_H_
+#define UR_REALTIME_COMMUNICATION_H_
+
+#include "robot_state_RT.h"
+#include <vector>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/time.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <netdb.h>
+#include <iostream>
+#include <unistd.h>
+
+class UrRealtimeCommunication {
+private:
+	const double SAMPLETIME_;
+	uint safety_count_max_;
+	int sockfd_;
+	struct sockaddr_in serv_addr_;
+	struct hostent *server_;
+	bool keepalive_;
+	std::thread comThread_;
+	int flag_;
+	std::recursive_mutex command_string_lock_;
+	std::string command_;
+	uint safety_count_;
+	void run();
+
+public:
+	bool connected_;
+	RobotStateRT* robot_state_;
+
+	UrRealtimeCommunication(std::condition_variable& msg_cond, std::string host,
+			uint safety_count_max = 12);
+	void start();
+	void halt();
+	void setSpeed(double q0, double q1, double q2, double q3, double q4,
+			double q5, double acc = 100.);
+	void addCommandToQueue(std::string inp);
+	void setSafetyCountMax(uint inp);
+
+};
+
+#endif /* UR_REALTIME_COMMUNICATION_H_ */