mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
420 lines
10 KiB
C++
420 lines
10 KiB
C++
/*
|
|
* robotStateRT.cpp
|
|
*
|
|
* Copyright 2015 Thomas Timm Andersen
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#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);
|
|
data_published_ = false;
|
|
controller_updated_ = false;
|
|
pMsg_cond_ = &msg_cond;
|
|
}
|
|
|
|
RobotStateRT::~RobotStateRT() {
|
|
/* Make sure nobody is waiting after this thread is destroyed */
|
|
data_published_ = true;
|
|
controller_updated_ = true;
|
|
pMsg_cond_->notify_all();
|
|
}
|
|
|
|
|
|
void RobotStateRT::setDataPublished() {
|
|
data_published_ = false;
|
|
}
|
|
bool RobotStateRT::getDataPublished() {
|
|
return data_published_;
|
|
}
|
|
|
|
void RobotStateRT::setControllerUpdated() {
|
|
controller_updated_ = false;
|
|
}
|
|
bool RobotStateRT::getControllerUpdated() {
|
|
return controller_updated_;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
void RobotStateRT::setVersion(double ver) {
|
|
val_lock_.lock();
|
|
version_ = ver;
|
|
val_lock_.unlock();
|
|
}
|
|
|
|
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();
|
|
int len;
|
|
memcpy(&len, &buf[offset], sizeof(len));
|
|
|
|
offset += sizeof(len);
|
|
len = ntohl(len);
|
|
if (version_ > 3. & version_ < 3.1 & len != 1044) {
|
|
//In 3.0, every 4th? package is malformed...?
|
|
//printf("Len: %i\n", len);
|
|
val_lock_.unlock();
|
|
return;
|
|
}
|
|
memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
|
|
time_ = RobotStateRT::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();
|
|
controller_updated_ = true;
|
|
data_published_ = true;
|
|
pMsg_cond_->notify_all();
|
|
|
|
}
|
|
|