diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 36831a12..38b422e9 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -76,6 +76,26 @@ class ExampleRobotWrapper ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file, const std::string& input_recipe_file, const bool headless_mode = true, const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE); + /*! + * \brief Construct a new Example Robot Wrapper object + * + * This will connect to a robot and initialize it. In headless mode the program will be running + * instantly, in teach pendant mode the from \p autostart_program will be started. + * + * Note: RTDE communication has to be started separately. + * + * \param robot_ip IP address of the robot to connect to + * \param output_recipe Output recipe vector for RTDE communication + * \param input_recipe Input recipe vector for RTDE communication + * \param headless_mode Should the driver be started in headless mode or not? + * \param autostart_program Program to start automatically after initialization when not in + * headless mode. This flag is ignored in headless mode. + * \param script_file URScript file to send to the robot. That should be script code + * communicating to the driver's reverse interface and trajectory interface. + */ + ExampleRobotWrapper(const std::string& robot_ip, const std::vector output_recipe, + const std::vector input_recipe, const bool headless_mode = true, + const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE); ~ExampleRobotWrapper(); /** diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index af152e4c..5ebc76ce 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -222,6 +222,19 @@ class RTDEClient return output_recipe_; } + /*! + * \brief Getter for the RTDE input recipe. + * + * \returns The input recipe + */ + std::vector getInputRecipe() + { + return input_recipe_; + } + + // Reads output or input recipe from a file + static std::vector readRecipe(const std::string& recipe_file); + private: comm::URStream stream_; std::vector output_recipe_; @@ -247,9 +260,6 @@ class RTDEClient constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; - // Reads output or input recipe from a file - std::vector readRecipe(const std::string& recipe_file) const; - // Helper function to ensure that timestamp is present in the output recipe. The timestamp is needed to ensure that // the robot is booted. std::vector ensureTimestampIsPresent(const std::vector& output_recipe) const; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 15a4fd25..5aaf5314 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -52,10 +52,12 @@ namespace urcl */ struct UrDriverConfiguration { - std::string robot_ip; //!< IP-address under which the robot is reachable. - std::string script_file; //!< URScript file that should be sent to the robot. - std::string output_recipe_file; //!< Filename where the output recipe is stored in. - std::string input_recipe_file; //!< Filename where the input recipe is stored in. + std::string robot_ip; //!< IP-address under which the robot is reachable. + std::string script_file; //!< URScript file that should be sent to the robot. + std::string output_recipe_file; //!< Filename where the output recipe is stored in. + std::string input_recipe_file; //!< Filename where the input recipe is stored in. + std::vector output_recipe; //!< Vector with the output recipe fields. + std::vector input_recipe; //!< Vector with the input recipe fields. /*! * \brief Function handle to a callback on program state changes. @@ -63,7 +65,7 @@ struct UrDriverConfiguration * For this to work, the URScript program will have to send keepalive signals to the \p * reverse_port. */ - std::function handle_program_state; + std::function handle_program_state = nullptr; bool headless_mode; //!< Parameter to control if the driver should be started in headless mode. std::unique_ptr tool_comm_setup = nullptr; //!< Configuration for using the tool communication. @@ -837,6 +839,13 @@ class UrDriver */ std::vector getRTDEOutputRecipe(); + /*! + * \brief Getter for the RTDE input recipe used in the RTDE client. + * + * \returns The used RTDE input recipe + */ + std::vector getRTDEInputRecipe(); + /*! * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot. * @@ -987,6 +996,7 @@ class UrDriver private: void init(const UrDriverConfiguration& config); + void setupRTDEClient(const UrDriverConfiguration& config); void initRTDE(); void setupReverseInterface(const uint32_t reverse_port); @@ -1023,4 +1033,4 @@ class UrDriver VersionInformation robot_version_; }; } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED \ No newline at end of file diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index d9fec1c7..356cd09a 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -47,7 +47,10 @@ ReverseInterface::ReverseInterface(const ReverseInterfaceConfig& config) , step_time_(config.step_time) , keep_alive_count_modified_deprecated_(false) { - handle_program_state_(false); + if (handle_program_state_) + { + handle_program_state_(false); + } server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1)); @@ -224,7 +227,10 @@ void ReverseInterface::connectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands."); client_fd_ = filedescriptor; - handle_program_state_(true); + if (handle_program_state_) + { + handle_program_state_(true); + } } else { @@ -237,7 +243,10 @@ void ReverseInterface::disconnectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor); client_fd_ = INVALID_SOCKET; - handle_program_state_(false); + if (handle_program_state_) + { + handle_program_state_(false); + } for (auto handler : disconnect_callbacks_) { handler.function(filedescriptor); @@ -251,4 +260,4 @@ void ReverseInterface::messageCallback(const socket_t filedescriptor, char* buff } } // namespace control -} // namespace urcl +} // namespace urcl \ No newline at end of file diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 6a230684..4ed2f3cd 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -664,7 +664,7 @@ bool RTDEClient::sendPause() throw UrException(ss.str()); } -std::vector RTDEClient::readRecipe(const std::string& recipe_file) const +std::vector RTDEClient::readRecipe(const std::string& recipe_file) { std::vector recipe; std::ifstream file(recipe_file); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e860722e..e1dd6e0b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -32,6 +32,7 @@ //---------------------------------------------------------------------- #include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/control/script_reader.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" @@ -39,6 +40,8 @@ #include "ur_client_library/helpers.h" #include #include +#include +#include #include @@ -76,14 +79,12 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset( - new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); get_packet_timeout_ = non_blocking_read_ ? 0 : 100; - initRTDE(); + setupRTDEClient(config); setupReverseInterface(config.reverse_port); // Figure out the ip automatically if the user didn't provide it @@ -623,6 +624,11 @@ std::vector UrDriver::getRTDEOutputRecipe() return rtde_client_->getOutputRecipe(); } +std::vector UrDriver::getRTDEInputRecipe() +{ + return rtde_client_->getInputRecipe(); +} + void UrDriver::setKeepaliveCount(const uint32_t count) { URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the " @@ -684,4 +690,40 @@ std::deque UrDriver::getErrorCodes() { return primary_client_->getErrorCodes(); } + +void UrDriver::setupRTDEClient(const UrDriverConfiguration& config) +{ + auto output_recipe = config.output_recipe; + if (config.output_recipe_file.empty() && config.output_recipe.size() == 0) + { + throw UrException("Neither output recipe file nor output recipe have been defined. An output recipe is required."); + } + if (!config.output_recipe_file.empty()) + { + if (config.output_recipe.size() != 0) + { + URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe vector"); + } + else + { + output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); + } + } + + auto input_recipe = config.input_recipe; + if (!config.input_recipe_file.empty()) + { + if (config.input_recipe.size() != 0) + { + URCL_LOG_WARN("Both input recipe file and input recipe vector are used. Defaulting to input recipe vector."); + } + else + { + input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); + } + } + + resetRTDEClient(output_recipe, input_recipe); +} + } // namespace urcl diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 80713157..ccf2af2f 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include "test_utils.h" using namespace urcl; @@ -40,6 +41,43 @@ using namespace urcl; const std::string SCRIPT_FILE = "../resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +const std::vector OUTPUT_RECIPE_VECTOR = { "timestamp", + "actual_qd", + "speed_scaling", + "target_speed_fraction", + "runtime_state", + "actual_TCP_force", + "actual_TCP_pose", + "actual_digital_input_bits", + "actual_digital_output_bits", + "standard_analog_input0", + "standard_analog_input1", + "standard_analog_output0", + "standard_analog_output1", + "analog_io_types", + "tool_mode", + "tool_analog_input_types", + "tool_analog_input0", + "tool_analog_input1", + "tool_output_voltage", + "tool_output_current", + "tool_temperature", + "robot_mode", + "safety_mode", + "robot_status_bits", + "safety_status_bits", + "actual_current", + "tcp_offset" }; +const std::vector INPUT_RECIPE_VECTOR = { + "speed_slider_mask", "standard_digital_output_mask", + "standard_digital_output", "configurable_digital_output_mask", + "configurable_digital_output", "tool_digital_output_mask", + "tool_digital_output", "standard_analog_output_mask", + "standard_analog_output_type", "standard_analog_output_0", + "standard_analog_output_1" +}; +const std::string OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "actual_q"; +const std::string INPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "speed_slider_fraction"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; @@ -310,6 +348,64 @@ TEST(UrDriverInitTest, setting_connection_limits_works_correctly) EXPECT_THROW(UrDriver ur_driver(config), UrException); } +TEST(UrDriverInitTest, no_recipe_throws_error) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); +} + +TEST(UrDriverInitTest, non_existing_output_recipe_file_throws_exception) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = " "; + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); +} + +TEST(UrDriverInitTest, non_existing_input_recipe_file_does_not_throw_exception) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.output_recipe_file = OUTPUT_RECIPE; + config.headless_mode = g_HEADLESS; + config.script_file = SCRIPT_FILE; + + EXPECT_NO_THROW(UrDriver ur_driver(config)); +} + +TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = OUTPUT_RECIPE; + config.input_recipe = INPUT_RECIPE_VECTOR; + config.output_recipe = OUTPUT_RECIPE_VECTOR; + config.headless_mode = g_HEADLESS; + config.script_file = SCRIPT_FILE; + + auto driver = UrDriver(config); + auto output_recipe = driver.getRTDEOutputRecipe(); + EXPECT_TRUE(std::find(output_recipe.begin(), output_recipe.end(), OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE) == + output_recipe.end()); + auto input_recipe = driver.getRTDEInputRecipe(); + EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) == + input_recipe.end()); +} // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[])