From c640badfb4139057af371e211665e37235420d44 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 6 Jun 2019 11:09:19 +0200 Subject: [PATCH] removed legacy datatypes --- ur_rtde_driver/CMakeLists.txt | 1 - .../include/ur_rtde_driver/comm/bin_parser.h | 15 -- ur_rtde_driver/include/ur_rtde_driver/types.h | 22 -- .../include/ur_rtde_driver/ur/rt_state.h | 140 ------------ ur_rtde_driver/src/ur/rt_state.cpp | 138 ------------ ur_rtde_driver/tests/ur/rt_state.cpp | 210 ------------------ 6 files changed, 526 deletions(-) delete mode 100644 ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h delete mode 100644 ur_rtde_driver/src/ur/rt_state.cpp delete mode 100644 ur_rtde_driver/tests/ur/rt_state.cpp diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index e9ef869..d4185be 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -81,7 +81,6 @@ add_library(ur_rtde_driver #src/ur/master_board.cpp #src/ur/messages.cpp #src/ur/robot_mode.cpp - #src/ur/rt_state.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_message/version_message.cpp diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h index 7bf825d..d9c838c 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/bin_parser.h @@ -126,14 +126,6 @@ public: val = inner != 0; } - // Explicit parsing order of fields to avoid issues with struct layout - void parse(double3_t& val) - { - parse(val.x); - parse(val.y); - parse(val.z); - } - void parse(vector3d_t& val) { for (size_t i = 0; i < val.size(); ++i) @@ -166,13 +158,6 @@ public: } } - // Explicit parsing order of fields to avoid issues with struct layout - void parse(cartesian_coord_t& val) - { - parse(val.position); - parse(val.rotation); - } - void rawData(std::unique_ptr& buffer, size_t& buffer_length) { buffer_length = buf_end_ - buf_pos_; diff --git a/ur_rtde_driver/include/ur_rtde_driver/types.h b/ur_rtde_driver/include/ur_rtde_driver/types.h index 173ecb3..adc5fdf 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/types.h +++ b/ur_rtde_driver/include/ur_rtde_driver/types.h @@ -24,33 +24,11 @@ namespace ur_driver { -// TODO:Replace -struct double3_t -{ - double x, y, z; -}; - using vector3d_t = std::array; using vector6d_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; -struct cartesian_coord_t -{ - double3_t position; - double3_t rotation; -}; - -inline bool operator==(const double3_t& lhs, const double3_t& rhs) -{ - return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z; -} - -inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rhs) -{ - return lhs.position == rhs.position && lhs.rotation == rhs.rotation; -} - template std::ostream& operator<<(std::ostream& out, const std::array& item) { diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h b/ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h deleted file mode 100644 index 2e0e441..0000000 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/rt_state.h +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * 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. - */ - -#pragma once - -#include -#include -#include "ur_rtde_driver/comm/bin_parser.h" -#include "ur_rtde_driver/comm/comm/pipeline.h" -#include "ur_rtde_driver/types.h" - -namespace ur_driver -{ -class URRTPacketConsumer; - -class RTPacket -{ -public: - virtual bool parseWith(BinParser& bp) = 0; - virtual bool consumeWith(URRTPacketConsumer& consumer) = 0; -}; - -class RTShared -{ -protected: - void parse_shared1(BinParser& bp); - void parse_shared2(BinParser& bp); - -public: - double time; - std::array q_target; - std::array qd_target; - std::array qdd_target; - std::array i_target; - std::array m_target; - std::array q_actual; - std::array qd_actual; - std::array i_actual; - - // gap here depending on version - - std::array tcp_force; - - // does not contain "_actual" postfix in V1_X but - // they're the same fields so share anyway - cartesian_coord_t tool_vector_actual; - cartesian_coord_t tcp_speed_actual; - - // gap here depending on version - - uint64_t digital_inputs; - std::array motor_temperatures; - double controller_time; - double robot_mode; - - static const size_t SIZE = - sizeof(double) * 3 + sizeof(double[6]) * 10 + sizeof(cartesian_coord_t) * 2 + sizeof(uint64_t); -}; - -class RTState_V1_6__7 : public RTShared, public RTPacket -{ -public: - bool parseWith(BinParser& bp); - virtual bool consumeWith(URRTPacketConsumer& consumer); - - double3_t tool_accelerometer_values; - - static const size_t SIZE = RTShared::SIZE + sizeof(double3_t); - - static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!"); -}; - -class RTState_V1_8 : public RTState_V1_6__7 -{ -public: - bool parseWith(BinParser& bp); - virtual bool consumeWith(URRTPacketConsumer& consumer); - - std::array joint_modes; - - static const size_t SIZE = RTState_V1_6__7::SIZE + sizeof(double[6]); - - static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!"); -}; - -class RTState_V3_0__1 : public RTShared, public RTPacket -{ -public: - bool parseWith(BinParser& bp); - virtual bool consumeWith(URRTPacketConsumer& consumer); - - std::array i_control; - cartesian_coord_t tool_vector_target; - cartesian_coord_t tcp_speed_target; - - std::array joint_modes; - double safety_mode; - double3_t tool_accelerometer_values; - double speed_scaling; - double linear_momentum_norm; - double v_main; - double v_robot; - double i_robot; - std::array v_actual; - - static const size_t SIZE = - RTShared::SIZE + sizeof(double[6]) * 3 + sizeof(double3_t) + sizeof(cartesian_coord_t) * 2 + sizeof(double) * 6; - - static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!"); -}; - -class RTState_V3_2__3 : public RTState_V3_0__1 -{ -public: - bool parseWith(BinParser& bp); - virtual bool consumeWith(URRTPacketConsumer& consumer); - - uint64_t digital_outputs; - double program_state; - - static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double); - - static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!"); -}; -} // namespace ur_driver diff --git a/ur_rtde_driver/src/ur/rt_state.cpp b/ur_rtde_driver/src/ur/rt_state.cpp deleted file mode 100644 index e7b15ed..0000000 --- a/ur_rtde_driver/src/ur/rt_state.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * 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_rtde_driver/ur/rt_state.h" -#include "ur_rtde_driver/ur/consumer.h" - -namespace ur_driver -{ -void RTShared::parse_shared1(BinParser& bp) -{ - bp.parse(time); - bp.parse(q_target); - bp.parse(qd_target); - bp.parse(qdd_target); - bp.parse(i_target); - bp.parse(m_target); - bp.parse(q_actual); - bp.parse(qd_actual); - bp.parse(i_actual); -} - -void RTShared::parse_shared2(BinParser& bp) -{ - bp.parse(digital_inputs); - bp.parse(motor_temperatures); - bp.parse(controller_time); - bp.consume(sizeof(double)); // Unused "Test value" field - bp.parse(robot_mode); -} - -bool RTState_V1_6__7::parseWith(BinParser& bp) -{ - if (!bp.checkSize()) - return false; - - parse_shared1(bp); - - bp.parse(tool_accelerometer_values); - bp.consume(sizeof(double) * 15); - bp.parse(tcp_force); - bp.parse(tool_vector_actual); - bp.parse(tcp_speed_actual); - - parse_shared2(bp); - - return true; -} - -bool RTState_V1_8::parseWith(BinParser& bp) -{ - if (!bp.checkSize()) - return false; - - RTState_V1_6__7::parseWith(bp); - - bp.parse(joint_modes); - - return true; -} - -bool RTState_V3_0__1::parseWith(BinParser& bp) -{ - if (!bp.checkSize()) - return false; - - parse_shared1(bp); - - bp.parse(i_control); - bp.parse(tool_vector_actual); - bp.parse(tcp_speed_actual); - bp.parse(tcp_force); - bp.parse(tool_vector_target); - bp.parse(tcp_speed_target); - - parse_shared2(bp); - - bp.parse(joint_modes); - bp.parse(safety_mode); - bp.consume(sizeof(double[6])); // skip undocumented - bp.parse(tool_accelerometer_values); - bp.consume(sizeof(double[6])); // skip undocumented - bp.parse(speed_scaling); - bp.parse(linear_momentum_norm); - bp.consume(sizeof(double)); // skip undocumented - bp.consume(sizeof(double)); // skip undocumented - bp.parse(v_main); - bp.parse(v_robot); - bp.parse(i_robot); - bp.parse(v_actual); - - return true; -} - -bool RTState_V3_2__3::parseWith(BinParser& bp) -{ - if (!bp.checkSize()) - return false; - - RTState_V3_0__1::parseWith(bp); - - bp.parse(digital_outputs); - bp.parse(program_state); - - return true; -} - -bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer) -{ - return consumer.consume(*this); -} -bool RTState_V1_8::consumeWith(URRTPacketConsumer& consumer) -{ - return consumer.consume(*this); -} -bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer) -{ - return consumer.consume(*this); -} -bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer) -{ - return consumer.consume(*this); -} -} // namespace ur_driver diff --git a/ur_rtde_driver/tests/ur/rt_state.cpp b/ur_rtde_driver/tests/ur/rt_state.cpp deleted file mode 100644 index 4fd0a38..0000000 --- a/ur_rtde_driver/tests/ur/rt_state.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * 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_rtde_driver/comm/ur/rt_state.h" -#include -#include "ur_rtde_driver/comm/bin_parser.h" -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/comm/test/random_data.h" -#include "ur_rtde_driver/comm/test/utils.h" -#include "ur_rtde_driver/comm/types.h" - -using namespace ur_driver; - -TEST(RTState_V1_6__7, testRandomDataParsing) -{ - RandomDataTest rdt(764); - BinParser bp = rdt.getParser(true); - RTState_V1_6__7 state; - EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; - - ASSERT_EQ(rdt.getNext(), state.time); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); - ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); - rdt.skip(sizeof(double) * 15); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); - ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); - ASSERT_EQ(rdt.getNext(), state.digital_inputs); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); - ASSERT_EQ(rdt.getNext(), state.controller_time); - rdt.skip(sizeof(double)); // skip unused value - ASSERT_EQ(rdt.getNext(), state.robot_mode); - - EXPECT_TRUE(bp.empty()) << "Did not consume all data"; -} - -TEST(RTState_V1_8, testRandomDataParsing) -{ - RandomDataTest rdt(812); - BinParser bp = rdt.getParser(true); - RTState_V1_8 state; - EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; - - ASSERT_EQ(rdt.getNext(), state.time); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); - ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); - rdt.skip(sizeof(double) * 15); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); - ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); - ASSERT_EQ(rdt.getNext(), state.digital_inputs); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); - ASSERT_EQ(rdt.getNext(), state.controller_time); - rdt.skip(sizeof(double)); // skip unused value - ASSERT_EQ(rdt.getNext(), state.robot_mode); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); - - EXPECT_TRUE(bp.empty()) << "Did not consume all data"; -} - -TEST(RTState_V3_0__1, testRandomDataParsing) -{ - RandomDataTest rdt(1044); - BinParser bp = rdt.getParser(true); - RTState_V3_0__1 state; - EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; - - ASSERT_EQ(rdt.getNext(), state.time); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); - - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_control); - ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); - ASSERT_EQ(rdt.getNext(), state.tool_vector_target); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_target); - - ASSERT_EQ(rdt.getNext(), state.digital_inputs); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); - ASSERT_EQ(rdt.getNext(), state.controller_time); - rdt.skip(sizeof(double)); // skip unused value - ASSERT_EQ(rdt.getNext(), state.robot_mode); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); - ASSERT_EQ(rdt.getNext(), state.safety_mode); - rdt.skip(sizeof(double) * 6); - ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); - rdt.skip(sizeof(double) * 6); - ASSERT_EQ(rdt.getNext(), state.speed_scaling); - ASSERT_EQ(rdt.getNext(), state.linear_momentum_norm); - rdt.skip(sizeof(double) * 2); - ASSERT_EQ(rdt.getNext(), state.v_main); - ASSERT_EQ(rdt.getNext(), state.v_robot); - ASSERT_EQ(rdt.getNext(), state.i_robot); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.v_actual); - - EXPECT_TRUE(bp.empty()) << "Did not consume all data"; -} - -TEST(RTState_V3_2__3, testRandomDataParsing) -{ - RandomDataTest rdt(1060); - BinParser bp = rdt.getParser(true); - RTState_V3_2__3 state; - EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false"; - - ASSERT_EQ(rdt.getNext(), state.time); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qdd_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.m_target); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.q_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.qd_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_actual); - - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.i_control); - ASSERT_EQ(rdt.getNext(), state.tool_vector_actual); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_actual); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.tcp_force); - ASSERT_EQ(rdt.getNext(), state.tool_vector_target); - ASSERT_EQ(rdt.getNext(), state.tcp_speed_target); - - ASSERT_EQ(rdt.getNext(), state.digital_inputs); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.motor_temperatures); - ASSERT_EQ(rdt.getNext(), state.controller_time); - rdt.skip(sizeof(double)); // skip unused value - ASSERT_EQ(rdt.getNext(), state.robot_mode); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.joint_modes); - ASSERT_EQ(rdt.getNext(), state.safety_mode); - rdt.skip(sizeof(double) * 6); - ASSERT_EQ(rdt.getNext(), state.tool_accelerometer_values); - rdt.skip(sizeof(double) * 6); - ASSERT_EQ(rdt.getNext(), state.speed_scaling); - ASSERT_EQ(rdt.getNext(), state.linear_momentum_norm); - rdt.skip(sizeof(double) * 2); - ASSERT_EQ(rdt.getNext(), state.v_main); - ASSERT_EQ(rdt.getNext(), state.v_robot); - ASSERT_EQ(rdt.getNext(), state.i_robot); - ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext(), state.v_actual); - ASSERT_EQ(rdt.getNext(), state.digital_outputs); - ASSERT_EQ(rdt.getNext(), state.program_state); - - EXPECT_TRUE(bp.empty()) << "did not consume all data"; -} - -TEST(RTState_V1_6__7, testTooSmallBuffer) -{ - RandomDataTest rdt(10); - BinParser bp = rdt.getParser(true); - RTState_V1_6__7 state; - EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; -} - -TEST(RTState_V1_8, testTooSmallBuffer) -{ - RandomDataTest rdt(10); - BinParser bp = rdt.getParser(true); - RTState_V1_8 state; - EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; -} - -TEST(RTState_V3_0__1, testTooSmallBuffer) -{ - RandomDataTest rdt(10); - BinParser bp = rdt.getParser(true); - RTState_V3_0__1 state; - EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; -} - -TEST(RTState_V3_2__3, testTooSmallBuffer) -{ - RandomDataTest rdt(10); - BinParser bp = rdt.getParser(true); - RTState_V3_2__3 state; - EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; -}