mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 18:10:47 +02:00
Removed more old files
This commit is contained in:
Simon Rasmussen
@@ -1,399 +0,0 @@
|
|||||||
/*
|
|
||||||
* robot_state.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.h"
|
|
||||||
|
|
||||||
RobotState::RobotState(std::condition_variable& msg_cond)
|
|
||||||
{
|
|
||||||
version_msg_.major_version = 0;
|
|
||||||
version_msg_.minor_version = 0;
|
|
||||||
new_data_available_ = false;
|
|
||||||
pMsg_cond_ = &msg_cond;
|
|
||||||
RobotState::setDisconnected();
|
|
||||||
robot_mode_running_ = robotStateTypeV30::ROBOT_MODE_RUNNING;
|
|
||||||
}
|
|
||||||
double RobotState::ntohd(uint64_t nf)
|
|
||||||
{
|
|
||||||
double x;
|
|
||||||
nf = be64toh(nf);
|
|
||||||
memcpy(&x, &nf, sizeof(x));
|
|
||||||
return x;
|
|
||||||
}
|
|
||||||
void RobotState::unpack(uint8_t* buf, unsigned int buf_length)
|
|
||||||
{
|
|
||||||
/* Returns missing bytes to unpack a message, or 0 if all data was parsed */
|
|
||||||
unsigned int offset = 0;
|
|
||||||
while (buf_length > offset)
|
|
||||||
{
|
|
||||||
int len;
|
|
||||||
unsigned char message_type;
|
|
||||||
memcpy(&len, &buf[offset], sizeof(len));
|
|
||||||
len = ntohl(len);
|
|
||||||
if (len + offset > buf_length)
|
|
||||||
{
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
memcpy(&message_type, &buf[offset + sizeof(len)], sizeof(message_type));
|
|
||||||
switch (message_type)
|
|
||||||
{
|
|
||||||
case messageType::ROBOT_MESSAGE:
|
|
||||||
RobotState::unpackRobotMessage(buf, offset,
|
|
||||||
len); //'len' is inclusive the 5 bytes from messageSize and messageType
|
|
||||||
break;
|
|
||||||
case messageType::ROBOT_STATE:
|
|
||||||
RobotState::unpackRobotState(buf, offset,
|
|
||||||
len); //'len' is inclusive the 5 bytes from messageSize and messageType
|
|
||||||
break;
|
|
||||||
case messageType::PROGRAM_STATE_MESSAGE:
|
|
||||||
// Don't do anything atm...
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
offset += len;
|
|
||||||
}
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len)
|
|
||||||
{
|
|
||||||
offset += 5;
|
|
||||||
uint64_t timestamp;
|
|
||||||
int8_t source, robot_message_type;
|
|
||||||
memcpy(×tamp, &buf[offset], sizeof(timestamp));
|
|
||||||
offset += sizeof(timestamp);
|
|
||||||
memcpy(&source, &buf[offset], sizeof(source));
|
|
||||||
offset += sizeof(source);
|
|
||||||
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
|
|
||||||
offset += sizeof(robot_message_type);
|
|
||||||
switch (robot_message_type)
|
|
||||||
{
|
|
||||||
case robotMessageType::ROBOT_MESSAGE_VERSION:
|
|
||||||
val_lock_.lock();
|
|
||||||
version_msg_.timestamp = timestamp;
|
|
||||||
version_msg_.source = source;
|
|
||||||
version_msg_.robot_message_type = robot_message_type;
|
|
||||||
RobotState::unpackRobotMessageVersion(buf, offset, len);
|
|
||||||
val_lock_.unlock();
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len)
|
|
||||||
{
|
|
||||||
offset += 5;
|
|
||||||
while (offset < len)
|
|
||||||
{
|
|
||||||
int32_t length;
|
|
||||||
uint8_t package_type;
|
|
||||||
memcpy(&length, &buf[offset], sizeof(length));
|
|
||||||
length = ntohl(length);
|
|
||||||
memcpy(&package_type, &buf[offset + sizeof(length)], sizeof(package_type));
|
|
||||||
switch (package_type)
|
|
||||||
{
|
|
||||||
case packageType::ROBOT_MODE_DATA:
|
|
||||||
val_lock_.lock();
|
|
||||||
RobotState::unpackRobotMode(buf, offset + 5);
|
|
||||||
val_lock_.unlock();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case packageType::MASTERBOARD_DATA:
|
|
||||||
val_lock_.lock();
|
|
||||||
RobotState::unpackRobotStateMasterboard(buf, offset + 5);
|
|
||||||
val_lock_.unlock();
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
offset += length;
|
|
||||||
}
|
|
||||||
new_data_available_ = true;
|
|
||||||
pMsg_cond_->notify_all();
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::unpackRobotMessageVersion(uint8_t* buf, unsigned int offset, uint32_t len)
|
|
||||||
{
|
|
||||||
memcpy(&version_msg_.project_name_size, &buf[offset], sizeof(version_msg_.project_name_size));
|
|
||||||
offset += sizeof(version_msg_.project_name_size);
|
|
||||||
memcpy(&version_msg_.project_name, &buf[offset], sizeof(char) * version_msg_.project_name_size);
|
|
||||||
offset += version_msg_.project_name_size;
|
|
||||||
version_msg_.project_name[version_msg_.project_name_size] = '\0';
|
|
||||||
memcpy(&version_msg_.major_version, &buf[offset], sizeof(version_msg_.major_version));
|
|
||||||
offset += sizeof(version_msg_.major_version);
|
|
||||||
memcpy(&version_msg_.minor_version, &buf[offset], sizeof(version_msg_.minor_version));
|
|
||||||
offset += sizeof(version_msg_.minor_version);
|
|
||||||
memcpy(&version_msg_.svn_revision, &buf[offset], sizeof(version_msg_.svn_revision));
|
|
||||||
offset += sizeof(version_msg_.svn_revision);
|
|
||||||
version_msg_.svn_revision = ntohl(version_msg_.svn_revision);
|
|
||||||
memcpy(&version_msg_.build_date, &buf[offset], sizeof(char) * len - offset);
|
|
||||||
version_msg_.build_date[len - offset] = '\0';
|
|
||||||
if (version_msg_.major_version < 2)
|
|
||||||
{
|
|
||||||
robot_mode_running_ = robotStateTypeV18::ROBOT_RUNNING_MODE;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::unpackRobotMode(uint8_t* buf, unsigned int offset)
|
|
||||||
{
|
|
||||||
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
|
|
||||||
offset += sizeof(robot_mode_.timestamp);
|
|
||||||
uint8_t tmp;
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isRobotConnected = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isRobotConnected = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isRealRobotEnabled = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isRealRobotEnabled = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
// printf("PowerOnRobot: %d\n", tmp);
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isPowerOnRobot = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isPowerOnRobot = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isEmergencyStopped = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isEmergencyStopped = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isProtectiveStopped = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isProtectiveStopped = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isProgramRunning = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isProgramRunning = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&tmp, &buf[offset], sizeof(tmp));
|
|
||||||
if (tmp > 0)
|
|
||||||
robot_mode_.isProgramPaused = true;
|
|
||||||
else
|
|
||||||
robot_mode_.isProgramPaused = false;
|
|
||||||
offset += sizeof(tmp);
|
|
||||||
memcpy(&robot_mode_.robotMode, &buf[offset], sizeof(robot_mode_.robotMode));
|
|
||||||
offset += sizeof(robot_mode_.robotMode);
|
|
||||||
uint64_t temp;
|
|
||||||
if (RobotState::getVersion() > 2.)
|
|
||||||
{
|
|
||||||
memcpy(&robot_mode_.controlMode, &buf[offset], sizeof(robot_mode_.controlMode));
|
|
||||||
offset += sizeof(robot_mode_.controlMode);
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
robot_mode_.targetSpeedFraction = RobotState::ntohd(temp);
|
|
||||||
}
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
robot_mode_.speedScaling = RobotState::ntohd(temp);
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset)
|
|
||||||
{
|
|
||||||
if (RobotState::getVersion() < 3.0)
|
|
||||||
{
|
|
||||||
int16_t digital_input_bits, digital_output_bits;
|
|
||||||
memcpy(&digital_input_bits, &buf[offset], sizeof(digital_input_bits));
|
|
||||||
offset += sizeof(digital_input_bits);
|
|
||||||
memcpy(&digital_output_bits, &buf[offset], sizeof(digital_output_bits));
|
|
||||||
offset += sizeof(digital_output_bits);
|
|
||||||
mb_data_.digitalInputBits = ntohs(digital_input_bits);
|
|
||||||
mb_data_.digitalOutputBits = ntohs(digital_output_bits);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
memcpy(&mb_data_.digitalInputBits, &buf[offset], sizeof(mb_data_.digitalInputBits));
|
|
||||||
offset += sizeof(mb_data_.digitalInputBits);
|
|
||||||
mb_data_.digitalInputBits = ntohl(mb_data_.digitalInputBits);
|
|
||||||
memcpy(&mb_data_.digitalOutputBits, &buf[offset], sizeof(mb_data_.digitalOutputBits));
|
|
||||||
offset += sizeof(mb_data_.digitalOutputBits);
|
|
||||||
mb_data_.digitalOutputBits = ntohl(mb_data_.digitalOutputBits);
|
|
||||||
}
|
|
||||||
|
|
||||||
memcpy(&mb_data_.analogInputRange0, &buf[offset], sizeof(mb_data_.analogInputRange0));
|
|
||||||
offset += sizeof(mb_data_.analogInputRange0);
|
|
||||||
memcpy(&mb_data_.analogInputRange1, &buf[offset], sizeof(mb_data_.analogInputRange1));
|
|
||||||
offset += sizeof(mb_data_.analogInputRange1);
|
|
||||||
uint64_t temp;
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
mb_data_.analogInput0 = RobotState::ntohd(temp);
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
mb_data_.analogInput1 = RobotState::ntohd(temp);
|
|
||||||
memcpy(&mb_data_.analogOutputDomain0, &buf[offset], sizeof(mb_data_.analogOutputDomain0));
|
|
||||||
offset += sizeof(mb_data_.analogOutputDomain0);
|
|
||||||
memcpy(&mb_data_.analogOutputDomain1, &buf[offset], sizeof(mb_data_.analogOutputDomain1));
|
|
||||||
offset += sizeof(mb_data_.analogOutputDomain1);
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
mb_data_.analogOutput0 = RobotState::ntohd(temp);
|
|
||||||
memcpy(&temp, &buf[offset], sizeof(temp));
|
|
||||||
offset += sizeof(temp);
|
|
||||||
mb_data_.analogOutput1 = RobotState::ntohd(temp);
|
|
||||||
|
|
||||||
memcpy(&mb_data_.masterBoardTemperature, &buf[offset], sizeof(mb_data_.masterBoardTemperature));
|
|
||||||
offset += sizeof(mb_data_.masterBoardTemperature);
|
|
||||||
mb_data_.masterBoardTemperature = ntohl(mb_data_.masterBoardTemperature);
|
|
||||||
memcpy(&mb_data_.robotVoltage48V, &buf[offset], sizeof(mb_data_.robotVoltage48V));
|
|
||||||
offset += sizeof(mb_data_.robotVoltage48V);
|
|
||||||
mb_data_.robotVoltage48V = ntohl(mb_data_.robotVoltage48V);
|
|
||||||
memcpy(&mb_data_.robotCurrent, &buf[offset], sizeof(mb_data_.robotCurrent));
|
|
||||||
offset += sizeof(mb_data_.robotCurrent);
|
|
||||||
mb_data_.robotCurrent = ntohl(mb_data_.robotCurrent);
|
|
||||||
memcpy(&mb_data_.masterIOCurrent, &buf[offset], sizeof(mb_data_.masterIOCurrent));
|
|
||||||
offset += sizeof(mb_data_.masterIOCurrent);
|
|
||||||
mb_data_.masterIOCurrent = ntohl(mb_data_.masterIOCurrent);
|
|
||||||
|
|
||||||
memcpy(&mb_data_.safetyMode, &buf[offset], sizeof(mb_data_.safetyMode));
|
|
||||||
offset += sizeof(mb_data_.safetyMode);
|
|
||||||
memcpy(&mb_data_.masterOnOffState, &buf[offset], sizeof(mb_data_.masterOnOffState));
|
|
||||||
offset += sizeof(mb_data_.masterOnOffState);
|
|
||||||
|
|
||||||
memcpy(&mb_data_.euromap67InterfaceInstalled, &buf[offset], sizeof(mb_data_.euromap67InterfaceInstalled));
|
|
||||||
offset += sizeof(mb_data_.euromap67InterfaceInstalled);
|
|
||||||
if (mb_data_.euromap67InterfaceInstalled != 0)
|
|
||||||
{
|
|
||||||
memcpy(&mb_data_.euromapInputBits, &buf[offset], sizeof(mb_data_.euromapInputBits));
|
|
||||||
offset += sizeof(mb_data_.euromapInputBits);
|
|
||||||
mb_data_.euromapInputBits = ntohl(mb_data_.euromapInputBits);
|
|
||||||
memcpy(&mb_data_.euromapOutputBits, &buf[offset], sizeof(mb_data_.euromapOutputBits));
|
|
||||||
offset += sizeof(mb_data_.euromapOutputBits);
|
|
||||||
mb_data_.euromapOutputBits = ntohl(mb_data_.euromapOutputBits);
|
|
||||||
if (RobotState::getVersion() < 3.0)
|
|
||||||
{
|
|
||||||
int16_t euromap_voltage, euromap_current;
|
|
||||||
memcpy(&euromap_voltage, &buf[offset], sizeof(euromap_voltage));
|
|
||||||
offset += sizeof(euromap_voltage);
|
|
||||||
memcpy(&euromap_current, &buf[offset], sizeof(euromap_current));
|
|
||||||
offset += sizeof(euromap_current);
|
|
||||||
mb_data_.euromapVoltage = ntohs(euromap_voltage);
|
|
||||||
mb_data_.euromapCurrent = ntohs(euromap_current);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
memcpy(&mb_data_.euromapVoltage, &buf[offset], sizeof(mb_data_.euromapVoltage));
|
|
||||||
offset += sizeof(mb_data_.euromapVoltage);
|
|
||||||
mb_data_.euromapVoltage = ntohl(mb_data_.euromapVoltage);
|
|
||||||
memcpy(&mb_data_.euromapCurrent, &buf[offset], sizeof(mb_data_.euromapCurrent));
|
|
||||||
offset += sizeof(mb_data_.euromapCurrent);
|
|
||||||
mb_data_.euromapCurrent = ntohl(mb_data_.euromapCurrent);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
double RobotState::getVersion()
|
|
||||||
{
|
|
||||||
double ver;
|
|
||||||
val_lock_.lock();
|
|
||||||
ver = version_msg_.major_version + 0.1 * version_msg_.minor_version + .0000001 * version_msg_.svn_revision;
|
|
||||||
val_lock_.unlock();
|
|
||||||
return ver;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::finishedReading()
|
|
||||||
{
|
|
||||||
new_data_available_ = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool RobotState::getNewDataAvailable()
|
|
||||||
{
|
|
||||||
return new_data_available_;
|
|
||||||
}
|
|
||||||
|
|
||||||
int RobotState::getDigitalInputBits()
|
|
||||||
{
|
|
||||||
return mb_data_.digitalInputBits;
|
|
||||||
}
|
|
||||||
int RobotState::getDigitalOutputBits()
|
|
||||||
{
|
|
||||||
return mb_data_.digitalOutputBits;
|
|
||||||
}
|
|
||||||
double RobotState::getAnalogInput0()
|
|
||||||
{
|
|
||||||
return mb_data_.analogInput0;
|
|
||||||
}
|
|
||||||
double RobotState::getAnalogInput1()
|
|
||||||
{
|
|
||||||
return mb_data_.analogInput1;
|
|
||||||
}
|
|
||||||
double RobotState::getAnalogOutput0()
|
|
||||||
{
|
|
||||||
return mb_data_.analogOutput0;
|
|
||||||
}
|
|
||||||
double RobotState::getAnalogOutput1()
|
|
||||||
{
|
|
||||||
return mb_data_.analogOutput1;
|
|
||||||
}
|
|
||||||
bool RobotState::isRobotConnected()
|
|
||||||
{
|
|
||||||
return robot_mode_.isRobotConnected;
|
|
||||||
}
|
|
||||||
bool RobotState::isRealRobotEnabled()
|
|
||||||
{
|
|
||||||
return robot_mode_.isRealRobotEnabled;
|
|
||||||
}
|
|
||||||
bool RobotState::isPowerOnRobot()
|
|
||||||
{
|
|
||||||
return robot_mode_.isPowerOnRobot;
|
|
||||||
}
|
|
||||||
bool RobotState::isEmergencyStopped()
|
|
||||||
{
|
|
||||||
return robot_mode_.isEmergencyStopped;
|
|
||||||
}
|
|
||||||
bool RobotState::isProtectiveStopped()
|
|
||||||
{
|
|
||||||
return robot_mode_.isProtectiveStopped;
|
|
||||||
}
|
|
||||||
bool RobotState::isProgramRunning()
|
|
||||||
{
|
|
||||||
return robot_mode_.isProgramRunning;
|
|
||||||
}
|
|
||||||
bool RobotState::isProgramPaused()
|
|
||||||
{
|
|
||||||
return robot_mode_.isProgramPaused;
|
|
||||||
}
|
|
||||||
unsigned char RobotState::getRobotMode()
|
|
||||||
{
|
|
||||||
return robot_mode_.robotMode;
|
|
||||||
}
|
|
||||||
bool RobotState::isReady()
|
|
||||||
{
|
|
||||||
if (robot_mode_.robotMode == robot_mode_running_)
|
|
||||||
{
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void RobotState::setDisconnected()
|
|
||||||
{
|
|
||||||
robot_mode_.isRobotConnected = false;
|
|
||||||
robot_mode_.isRealRobotEnabled = false;
|
|
||||||
robot_mode_.isPowerOnRobot = false;
|
|
||||||
}
|
|
||||||
@@ -1,492 +0,0 @@
|
|||||||
/*
|
|
||||||
* 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 = qd_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);
|
|
||||||
|
|
||||||
// Check the correct message length is received
|
|
||||||
bool len_good = true;
|
|
||||||
if (version_ >= 1.6 && version_ < 1.7)
|
|
||||||
{ // v1.6
|
|
||||||
if (len != 756)
|
|
||||||
len_good = false;
|
|
||||||
}
|
|
||||||
else if (version_ >= 1.7 && version_ < 1.8)
|
|
||||||
{ // v1.7
|
|
||||||
if (len != 764)
|
|
||||||
len_good = false;
|
|
||||||
}
|
|
||||||
else if (version_ >= 1.8 && version_ < 1.9)
|
|
||||||
{ // v1.8
|
|
||||||
if (len != 812)
|
|
||||||
len_good = false;
|
|
||||||
}
|
|
||||||
else if (version_ >= 3.0 && version_ < 3.2)
|
|
||||||
{ // v3.0 & v3.1
|
|
||||||
if (len != 1044)
|
|
||||||
len_good = false;
|
|
||||||
}
|
|
||||||
else if (version_ >= 3.2 && version_ < 3.3)
|
|
||||||
{ // v3.2
|
|
||||||
if (len != 1060)
|
|
||||||
len_good = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!len_good)
|
|
||||||
{
|
|
||||||
printf("Wrong length of message on RT interface: %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.9)
|
|
||||||
{
|
|
||||||
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();
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user