diff --git a/CMakeLists.txt b/CMakeLists.txt index e9f3b83..7f1badf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(ur_modern_driver) + + add_definitions( -DROS_BUILD ) + + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -130,6 +134,8 @@ else() message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ") endif() +set(CMAKE_CXX_FLAGS "-g -Wall -Wextra ${CMAKE_CXX_FLAGS}") + ## Specify additional locations of header files ## Your package locations should be listed before other locations # include_directories(include) @@ -152,18 +158,26 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES - src/ur_ros_wrapper.cpp + src/ros/rt_publisher.cpp + src/ur/stream.cpp + src/ur/robot_mode.cpp + src/ur/master_board.cpp + src/ur/rt_state.cpp + src/ur/messages.cpp + #src/ros_main.cpp + #src/ur_ros_wrapper.cpp src/ur_driver.cpp src/ur_realtime_communication.cpp src/ur_communication.cpp src/robot_state.cpp src/robot_state_RT.cpp src/do_output.cpp) -add_executable(ur_driver ${${PROJECT_NAME}_SOURCES}) + +add_executable(ur_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) ## Add cmake target dependencies of the executable ## same as for the library above - add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(ur_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(ur_driver @@ -200,11 +214,12 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Testing ## ############# -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_modern_driver.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +set(${PROJECT_NAME}_TEST_SOURCES + tests/ur/rt_state.cpp + tests/ur/robot_mode.cpp) + +if (CATKIN_ENABLE_TESTING) + catkin_add_gtest(ur_modern_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp) + target_link_libraries(ur_modern_driver_test ur_hardware_interface ${catkin_LIBRARIES}) +endif() diff --git a/include/ur_modern_driver/test/random_data.h b/include/ur_modern_driver/test/random_data.h new file mode 100644 index 0000000..14ae65b --- /dev/null +++ b/include/ur_modern_driver/test/random_data.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/bin_parser.h" + +class RandomDataTest +{ +private: + using random_bytes_engine = std::independent_bits_engine; + uint8_t* _buf; + BinParser bp_; + size_t n_; + +public: + RandomDataTest(size_t n) : _buf(new uint8_t[n]), bp_(_buf, n), n_(n) + { + random_bytes_engine rbe; + std::generate(_buf, _buf + n, std::ref(rbe)); + } + + ~RandomDataTest() + { + delete _buf; + } + + BinParser getParser(bool skip = false) + { + return BinParser(_buf, n_ - (skip ? sizeof(int32_t) : 0)); + } + + template + T getNext() + { + T actual; + bp_.parse(actual); + return actual; + } + void skip(size_t n) + { + bp_.consume(n); + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/test/utils.h b/include/ur_modern_driver/test/utils.h new file mode 100644 index 0000000..edbb531 --- /dev/null +++ b/include/ur_modern_driver/test/utils.h @@ -0,0 +1,8 @@ +#pragma once + +#define ASSERT_DOUBLE_ARRAY_EQ(fn, name) \ + for (auto const& v : name) \ + { \ + ASSERT_EQ(fn, v) << #name " failed parsing"; \ + } + diff --git a/tests/main.cpp b/tests/main.cpp new file mode 100644 index 0000000..92cbc41 --- /dev/null +++ b/tests/main.cpp @@ -0,0 +1,7 @@ +#include "gtest/gtest.h" + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/ur/master_board.cpp b/tests/ur/master_board.cpp new file mode 100644 index 0000000..e69de29 diff --git a/tests/ur/robot_mode.cpp b/tests/ur/robot_mode.cpp new file mode 100644 index 0000000..170e0cd --- /dev/null +++ b/tests/ur/robot_mode.cpp @@ -0,0 +1,105 @@ +#include +#include "ur_modern_driver/ur/robot_mode.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/test/random_data.h" + +TEST(RobotModeData_V1_X, testRandomDataParsing) +{ + RandomDataTest rdt(24); + BinParser bp = rdt.getParser(); + RobotModeData_V1_X state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.security_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.speed_fraction); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + +TEST(RobotModeData_V3_0__1, testRandomDataParsing) +{ + RandomDataTest rdt(33); + BinParser bp = rdt.getParser(); + RobotModeData_V3_0__1 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.control_mode); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + +TEST(RobotModeData_V3_2, testRandomDataParsing) +{ + RandomDataTest rdt(41); + BinParser bp = rdt.getParser(); + RobotModeData_V3_2 state; + ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; + + ASSERT_EQ(rdt.getNext(), state.timestamp); + ASSERT_EQ(rdt.getNext(), state.physical_robot_connected); + ASSERT_EQ(rdt.getNext(), state.real_robot_enabled); + ASSERT_EQ(rdt.getNext(), state.robot_power_on); + ASSERT_EQ(rdt.getNext(), state.emergency_stopped); + ASSERT_EQ(rdt.getNext(), state.protective_stopped); + ASSERT_EQ(rdt.getNext(), state.program_running); + ASSERT_EQ(rdt.getNext(), state.program_paused); + ASSERT_EQ(rdt.getNext(), state.robot_mode); + ASSERT_EQ(rdt.getNext(), state.control_mode); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction); + ASSERT_EQ(rdt.getNext(), state.speed_scaling); + ASSERT_EQ(rdt.getNext(), state.target_speed_fraction_limit); + + ASSERT_TRUE(bp.empty()) << "Did not consume all data"; +} + + + +TEST(RobotModeData_V1_X, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V1_X state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + + +TEST(RobotModeData_V3_0__1, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V3_0__1 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} + + +TEST(RobotModeData_V3_2, testTooSmallBuffer) +{ + RandomDataTest rdt(10); + BinParser bp = rdt.getParser(); + RobotModeData_V3_2 state; + EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough"; +} diff --git a/tests/ur/rt_state.cpp b/tests/ur/rt_state.cpp new file mode 100644 index 0000000..c24c72a --- /dev/null +++ b/tests/ur/rt_state.cpp @@ -0,0 +1,192 @@ +#include +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/bin_parser.h" +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/types.h" +#include "ur_modern_driver/test/utils.h" +#include "ur_modern_driver/test/random_data.h" + +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"; +} \ No newline at end of file