Added tests

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()

include/ur_modern_driver/test/random_data.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <inttypes.h>
+#include <algorithm>
+#include <climits>
+#include <cstddef>
+#include <functional>
+#include <random>
+#include "ur_modern_driver/bin_parser.h"
+
+class RandomDataTest
+{
+private:
+  using random_bytes_engine = std::independent_bits_engine<std::default_random_engine, CHAR_BIT, uint8_t>;
+  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 <typename T>
+  T getNext()
+  {
+    T actual;
+    bp_.parse(actual);
+    return actual;
+  }
+  void skip(size_t n)
+  {
+    bp_.consume(n);
+  }
+};

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";                                                        \
+  }
+

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();
+}

tests/ur/robot_mode.cpp

@@ -0,0 +1,105 @@
+#include <gtest/gtest.h>
+#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<uint64_t>(), state.timestamp);
+  ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
+  ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
+  ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
+  ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.security_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
+  ASSERT_EQ(rdt.getNext<robot_mode_V1_X>(), state.robot_mode);
+  ASSERT_EQ(rdt.getNext<double>(), 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<uint64_t>(), state.timestamp);
+  ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
+  ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
+  ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
+  ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
+  ASSERT_EQ(rdt.getNext<robot_mode_V3_X>(), state.robot_mode);
+  ASSERT_EQ(rdt.getNext<robot_control_mode_V3_X>(), state.control_mode);
+  ASSERT_EQ(rdt.getNext<double>(), state.target_speed_fraction);
+  ASSERT_EQ(rdt.getNext<double>(), 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<uint64_t>(), state.timestamp);
+  ASSERT_EQ(rdt.getNext<bool>(), state.physical_robot_connected);
+  ASSERT_EQ(rdt.getNext<bool>(), state.real_robot_enabled);
+  ASSERT_EQ(rdt.getNext<bool>(), state.robot_power_on);
+  ASSERT_EQ(rdt.getNext<bool>(), state.emergency_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.protective_stopped);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_running);
+  ASSERT_EQ(rdt.getNext<bool>(), state.program_paused);
+  ASSERT_EQ(rdt.getNext<robot_mode_V3_X>(), state.robot_mode);
+  ASSERT_EQ(rdt.getNext<robot_control_mode_V3_X>(), state.control_mode);
+  ASSERT_EQ(rdt.getNext<double>(), state.target_speed_fraction);
+  ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
+  ASSERT_EQ(rdt.getNext<double>(), 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";
+}

tests/ur/rt_state.cpp

@@ -0,0 +1,192 @@
+#include <gtest/gtest.h>
+#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<double>(), state.time);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
+  rdt.skip(sizeof(double) * 15);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
+  ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
+  rdt.skip(sizeof(double));  // skip unused value
+  ASSERT_EQ(rdt.getNext<double>(), 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<double>(), state.time);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
+  rdt.skip(sizeof(double) * 15);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
+  ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
+  rdt.skip(sizeof(double));  // skip unused value
+  ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), 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<double>(), state.time);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
+
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_control);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_target);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_target);
+
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
+  ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
+  rdt.skip(sizeof(double));  // skip unused value
+  ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
+  ASSERT_EQ(rdt.getNext<double>(), state.safety_mode);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
+  ASSERT_EQ(rdt.getNext<double>(), state.linear_momentum_norm);
+  rdt.skip(sizeof(double) * 2);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_main);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_robot);
+  ASSERT_EQ(rdt.getNext<double>(), state.i_robot);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), 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<double>(), state.time);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
+
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_control);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_target);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_target);
+
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
+  ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
+  rdt.skip(sizeof(double));  // skip unused value
+  ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
+  ASSERT_EQ(rdt.getNext<double>(), state.safety_mode);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
+  ASSERT_EQ(rdt.getNext<double>(), state.linear_momentum_norm);
+  rdt.skip(sizeof(double) * 2);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_main);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_robot);
+  ASSERT_EQ(rdt.getNext<double>(), state.i_robot);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.v_actual);
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_outputs);
+  ASSERT_EQ(rdt.getNext<double>(), 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";
+}