From d7f065a22d1ab596d7b48a409658a5cbba6920e7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 11 Apr 2019 18:08:34 +0200 Subject: [PATCH] Upload URScript via primary interface Note: This will probably be changed later, but currently that's the easiest way to handle this. --- CMakeLists.txt | 2 +- include/ur_rtde_driver/comm/stream.h | 2 +- include/ur_rtde_driver/ur/ur_driver.h | 4 ++ launch/ur10_ros_control.launch | 2 +- src/comm/server.cpp | 2 +- src/ros/hardware_interface.cpp | 2 +- src/ur/ur_driver.cpp | 94 +++++++++++++++++++++++++++ 7 files changed, 103 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aebf6a2..9fe8476 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,13 +71,13 @@ include_directories( add_library(ur_rtde_driver src/comm/tcp_socket.cpp + src/comm/server.cpp #src/ros/service_stopper.cpp #src/ur/commander.cpp #src/ur/master_board.cpp #src/ur/messages.cpp #src/ur/robot_mode.cpp #src/ur/rt_state.cpp - #src/ur/server.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_message/version_message.cpp diff --git a/include/ur_rtde_driver/comm/stream.h b/include/ur_rtde_driver/comm/stream.h index 1db9ae8..cca63f0 100644 --- a/include/ur_rtde_driver/comm/stream.h +++ b/include/ur_rtde_driver/comm/stream.h @@ -49,7 +49,7 @@ public: * \param host IP address of the remote host * \param port Port on which the socket shall be connected */ - URStream(std::string& host, int port) : host_(host), port_(port) + URStream(const std::string& host, int port) : host_(host), port_(port) { } diff --git a/include/ur_rtde_driver/ur/ur_driver.h b/include/ur_rtde_driver/ur/ur_driver.h index 33a4bf7..b548815 100644 --- a/include/ur_rtde_driver/ur/ur_driver.h +++ b/include/ur_rtde_driver/ur/ur_driver.h @@ -26,6 +26,7 @@ //---------------------------------------------------------------------- #include "ur_rtde_driver/rtde/rtde_client.h" +#include "ur_rtde_driver/comm/reverse_interface.h" namespace ur_driver { @@ -60,10 +61,13 @@ public: return rtde_frequency_; } + bool writeJointCommand(const vector6d_t& values); + private: //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series. uint32_t rtde_frequency_; comm::INotifier notifier_; std::unique_ptr rtde_client_; + std::unique_ptr reverse_interface_; }; } // namespace ur_driver diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index 4c6a5b4..5a67b19 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -16,7 +16,7 @@ - + diff --git a/src/comm/server.cpp b/src/comm/server.cpp index 0232f14..1acc3d3 100644 --- a/src/comm/server.cpp +++ b/src/comm/server.cpp @@ -18,7 +18,7 @@ * limitations under the License. */ -#include "ur_rtde_driver/ur/server.h" +#include "ur_rtde_driver/comm/server.h" #include #include #include diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp index 27f81f6..be0d0b6 100644 --- a/src/ros/hardware_interface.cpp +++ b/src/ros/hardware_interface.cpp @@ -101,7 +101,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period) { - // TODO: Implement + ur_driver_->writeJointCommand(joint_position_command_); } bool HardwareInterface ::prepareSwitch(const std::list& start_list, diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index d85dbcd..485239b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -30,6 +30,66 @@ namespace ur_driver { +static const std::string POSITION_PROGRAM = R"( +def myProg(): + textmsg("hello") + MULT_jointstate = 1000000 + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, t=0.008, lookahead_time=0.03, gain=300) + else: + sync() + end + end + end + socket_open("192.168.56.1", 50001) + + thread_servo = run servoThread() + keepalive = -2 + params_mult = socket_read_binary_integer(6+1) + keepalive = params_mult[7] + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1) + keepalive = params_mult[7] + if keepalive > 0: + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + set_servo_setpoint(q) + end + end + end + sleep(.1) + socket_close() + kill thread_servo +end +)"; + ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125) // conservative CB3 default. { ROS_INFO_STREAM("Initializing RTDE client"); @@ -39,6 +99,26 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125 { throw std::runtime_error("initialization went wrong"); // TODO: be less harsh } + + comm::URStream stream(ROBOT_IP, 30001); + stream.connect(); + + size_t len = POSITION_PROGRAM.size(); + const uint8_t* data = reinterpret_cast(POSITION_PROGRAM.c_str()); + size_t written; + + if (stream.write(data, len, written)) + { + LOG_INFO("Sent program to robot"); + } + else + { + LOG_ERROR("Could not send program to robot"); + } + + uint32_t reverse_port = 50001; // TODO: Make this a parameter + reverse_interface_.reset(new comm::ReverseInterface(reverse_port)); + rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface) } @@ -59,4 +139,18 @@ std::unique_ptr ur_driver::UrDriver::getDataPackage } return nullptr; } + +bool UrDriver::writeJointCommand(const vector6d_t& values) +{ + if (reverse_interface_) + { + reverse_interface_->write(values); + } + else + { + return false; + } + + return true; +} } // namespace ur_driver