From 68fb5ad0bf737d66ef15ea9513d034c4bec2ff02 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Jun 2021 16:21:37 +0200 Subject: [PATCH] WIP: Create an interface for registering a trajectoryDone callback for passthrough --- examples/full_driver.cpp | 8 +++++-- include/ur_client_library/comm/tcp_server.h | 4 ++-- .../control/reverse_interface.h | 2 +- .../control/trajectory_point_interface.h | 21 ++++++++++++++++++- include/ur_client_library/ur/ur_driver.h | 15 +++++++------ resources/external_control.urscript | 6 ++++++ src/comm/tcp_server.cpp | 2 +- src/control/reverse_interface.cpp | 6 +++--- src/control/trajectory_point_interface.cpp | 19 ++++++++++++++--- src/ur/ur_driver.cpp | 13 +++++++----- 10 files changed, 72 insertions(+), 24 deletions(-) diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 45213d4c..4e9f27b3 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -53,12 +53,16 @@ void handleRobotProgramState(bool program_running) // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; } +void trajectoryDoneCb(urcl::control::TrajectoryResult result) +{ + std::cout << "Trajectory finished in state " << toUnderlying(result) << std::endl; +} int main(int argc, char* argv[]) { std::unique_ptr tool_comm_setup; - g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, false, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, + &trajectoryDoneCb, false, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. diff --git a/include/ur_client_library/comm/tcp_server.h b/include/ur_client_library/comm/tcp_server.h index 6e478717..20cda613 100644 --- a/include/ur_client_library/comm/tcp_server.h +++ b/include/ur_client_library/comm/tcp_server.h @@ -90,7 +90,7 @@ class TCPServer * \param func Function handling the event information. The file client's file_descriptor will be * passed to the function as well as the actual message received from the client. */ - void setMessageCallback(std::function func) + void setMessageCallback(std::function func) { message_callback_ = func; } @@ -184,7 +184,7 @@ class TCPServer std::function new_connection_callback_; std::function disconnect_callback_; - std::function message_callback_; + std::function message_callback_; }; } // namespace comm diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index 4f090768..618be2d4 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -109,7 +109,7 @@ class ReverseInterface virtual void disconnectionCallback(const int filedescriptor); - virtual void messageCallback(const int filedescriptor, char* buffer); + virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv); int client_fd_; comm::TCPServer server_; diff --git a/include/ur_client_library/control/trajectory_point_interface.h b/include/ur_client_library/control/trajectory_point_interface.h index 1df40bf1..88a5c82f 100644 --- a/include/ur_client_library/control/trajectory_point_interface.h +++ b/include/ur_client_library/control/trajectory_point_interface.h @@ -37,6 +37,17 @@ namespace urcl { namespace control { +/*! + * \brief Types for encoding trajectory execution result + */ +enum class TrajectoryResult : int32_t +{ + + TRAJECTORY_RESULT_SUCCESS = 0, ///< Successful execution + TRAJECTORY_RESULT_CANCELED = 1, ///< Canceled by user + TRAJECTORY_RESULT_FAILURE = 2 ///< Aborted due to error during execution +}; + /*! * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full * trajectories are forwarded to the robot controller and are executed there. @@ -72,12 +83,20 @@ class TrajectoryPointInterface : public ReverseInterface bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, const bool cartesian); + void setTrajectoryEndCallback(std::function callback) + { + handle_trajectory_end_ = callback; + } + protected: virtual void connectionCallback(const int filedescriptor) override; virtual void disconnectionCallback(const int filedescriptor) override; - virtual void messageCallback(const int filedescriptor, char* buffer) override; + virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override; + +private: + std::function handle_trajectory_end_; }; } // namespace control diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 5ec84753..43abeb8c 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -87,7 +87,8 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -116,7 +117,8 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = ""); @@ -144,13 +146,14 @@ class UrDriver * address of the interface that is used for connecting to the robot's RTDE port will be used. */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, - const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + const std::string& input_recipe_file, std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "") - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, + headless_mode, std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, + servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) { } diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 5fd2a2a1..8ed48208 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -27,6 +27,10 @@ TRAJECTORY_POINT_JOINT = 0 TRAJECTORY_POINT_CARTESIAN = 1 TRAJECTORY_DATA_DIMENSION = 7 +TRAJECTORY_RESULT_SUCCESS = 0 +TRAJECTORY_RESULT_CANCELED = 1 +TRAJECTORY_RESULT_FAILURE = 2 + #Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -150,6 +154,7 @@ thread jointTrajectoryThread(): #add some form of error handling here end end + socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") textmsg("Trajectory finished") end @@ -212,6 +217,7 @@ while keepalive > 0 and control_mode > MODE_STOPPED: textmsg("cancel received") kill thread_trajectory clear_remaining_trajectory_points() + socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket") end end else: diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp index 3443c223..59aba63d 100644 --- a/src/comm/tcp_server.cpp +++ b/src/comm/tcp_server.cpp @@ -272,7 +272,7 @@ void TCPServer::readData(const int fd) { if (message_callback_) { - message_callback_(fd, input_buffer_); + message_callback_(fd, input_buffer_, nbytesrecv); } } else diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index 008392c1..ae1d0de1 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -35,8 +35,8 @@ ReverseInterface::ReverseInterface(uint32_t port, std::function hand : client_fd_(-1), server_(port), handle_program_state_(handle_program_state), keepalive_count_(1) { handle_program_state_(false); - server_.setMessageCallback( - std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2)); + 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)); server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1)); server_.setMaxClientsAllowed(1); @@ -135,7 +135,7 @@ void ReverseInterface::disconnectionCallback(const int filedescriptor) handle_program_state_(false); } -void ReverseInterface::messageCallback(const int filedescriptor, char* buffer) +void ReverseInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) { URCL_LOG_WARN("Message on ReverseInterface received. The reverse interface currently does not support any message " "handling. This message will be ignored."); diff --git a/src/control/trajectory_point_interface.cpp b/src/control/trajectory_point_interface.cpp index 28fafd95..cf8a5a54 100644 --- a/src/control/trajectory_point_interface.cpp +++ b/src/control/trajectory_point_interface.cpp @@ -105,10 +105,23 @@ void TrajectoryPointInterface::disconnectionCallback(const int filedescriptor) client_fd_ = -1; } -void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer) +void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) { - // TODO: Send up. This will be things like "finished trajectory" - URCL_LOG_INFO("Received message %s on TrajectoryPointInterface", buffer); + if (nbytesrecv == 4) + { + int32_t* status = reinterpret_cast(buffer); + URCL_LOG_DEBUG("Received message %d on TrajectoryPointInterface", be32toh(*status)); + + if (handle_trajectory_end_) + { + handle_trajectory_end_(static_cast(*status)); + } + } + else + { + URCL_LOG_WARN("Received %d bytes on TrajectoryPointInterface. Expecting 4 bytes, so ignoring this message", + nbytesrecv); + } } } // namespace control } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 185af308..fbb1a373 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -49,7 +49,8 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, + std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) @@ -156,19 +157,21 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state)); // TODO swap to configurable or static port trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10)); + trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); URCL_LOG_DEBUG("Initialization done"); } urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, + std::function handle_program_state, + std::function trajectory_done_cb, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip) + : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb, + headless_mode, std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, + servoj_lookahead_time, non_blocking_read, reverse_ip) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This "