From 649f49949901fdff2864f557fdc23a34b2a39be0 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Thu, 23 May 2019 15:03:39 +0200 Subject: [PATCH] moved rtde recipe to external text file --- .../include/ur_rtde_driver/rtde/rtde_client.h | 5 ++-- .../include/ur_rtde_driver/ur/ur_driver.h | 2 +- ur_rtde_driver/launch/ur10_ros_control.launch | 2 ++ ur_rtde_driver/resources/rtde_recipe.txt | 5 ++++ ur_rtde_driver/src/main_plain_driver.cpp | 3 ++- ur_rtde_driver/src/ros/hardware_interface.cpp | 9 +++++++- ur_rtde_driver/src/rtde/rtde_client.cpp | 23 +++++++++++-------- ur_rtde_driver/src/ur/ur_driver.cpp | 5 ++-- 8 files changed, 37 insertions(+), 17 deletions(-) create mode 100644 ur_rtde_driver/resources/rtde_recipe.txt diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h index 66123b1..fda5956 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h +++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h @@ -51,7 +51,7 @@ class RTDEClient { public: RTDEClient() = delete; - RTDEClient(std::string robot_ip, comm::INotifier& notifier); + RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file); ~RTDEClient() = default; bool init(); bool start(); @@ -64,6 +64,7 @@ public: private: comm::URStream stream_; + std::vector recipe_; RTDEParser parser_; comm::URProducer prod_; comm::Pipeline pipeline_; @@ -73,7 +74,7 @@ private: constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; - std::vector readRecipe(); + std::vector readRecipe(const std::string& recipe_file); }; } // namespace rtde_interface diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index e5617e7..2919be9 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -46,7 +46,7 @@ public: * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot */ - UrDriver(const std::string& robot_ip, const std::string& script_file); + UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file); virtual ~UrDriver() = default; /*! diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 8fac166..506b5cc 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -19,6 +19,7 @@ + @@ -29,6 +30,7 @@ + diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_recipe.txt new file mode 100644 index 0000000..24588e3 --- /dev/null +++ b/ur_rtde_driver/resources/rtde_recipe.txt @@ -0,0 +1,5 @@ +timestamp +actual_q +actual_qd +speed_scaling +target_speed_fraction diff --git a/ur_rtde_driver/src/main_plain_driver.cpp b/ur_rtde_driver/src/main_plain_driver.cpp index 44eb4a2..55b9ff4 100644 --- a/ur_rtde_driver/src/main_plain_driver.cpp +++ b/ur_rtde_driver/src/main_plain_driver.cpp @@ -20,13 +20,14 @@ int main(int argc, char* argv[]) { std::string robot_ip = "192.168.56.101"; std::string script_filename = "urprog.urscript"; + std::string recipe_filename = "rtde_recipe.txt"; if (argc > 2) { robot_ip = argv[1]; } - UrDriver driver(robot_ip, script_filename); + UrDriver driver(robot_ip, script_filename, recipe_filename); while (true) { diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 00839dc..0e8349f 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -46,14 +46,21 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; std::string robot_ip = robot_hw_nh.param("robot_ip", "192.168.56.101"); std::string script_filename; + std::string recipe_filename; if (!robot_hw_nh.getParam("script_file", script_filename)) { ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given."); return false; } + if (!robot_hw_nh.getParam("recipe_file", recipe_filename)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("recipe_file") << " not given."); + return false; + } + ROS_INFO_STREAM("Initializing urdriver"); - ur_driver_.reset(new UrDriver(robot_ip, script_filename)); + ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename)); if (!root_nh.getParam("hardware_interface/joints", joint_names_)) { diff --git a/ur_rtde_driver/src/rtde/rtde_client.cpp b/ur_rtde_driver/src/rtde/rtde_client.cpp index 455c52a..fa64ceb 100644 --- a/ur_rtde_driver/src/rtde/rtde_client.cpp +++ b/ur_rtde_driver/src/rtde/rtde_client.cpp @@ -31,9 +31,10 @@ namespace ur_driver { namespace rtde_interface { -RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier) +RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& recipe_file) : stream_(robot_ip, UR_RTDE_PORT) - , parser_(readRecipe()) + , recipe_(readRecipe(recipe_file)) + , parser_(recipe_) , prod_(stream_, parser_) , pipeline_(prod_, PIPELINE_NAME, notifier) , max_frequency_(URE_MAX_FREQUENCY) @@ -83,11 +84,11 @@ bool RTDEClient::init() LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency_); if (protocol_version == 2) { - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency_, readRecipe()); + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency_, recipe_); } else { - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, readRecipe()); + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, recipe_); } stream_.write(buffer, size, written); return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); @@ -104,14 +105,16 @@ bool RTDEClient::start() rtde_interface::ControlPackageStart* tmp = dynamic_cast(package.get()); return tmp->accepted_; } -std::vector RTDEClient::readRecipe() +std::vector RTDEClient::readRecipe(const std::string& recipe_file) { std::vector recipe; - recipe.push_back("timestamp"); - recipe.push_back("actual_q"); - recipe.push_back("actual_qd"); - recipe.push_back("speed_scaling"); - recipe.push_back("target_speed_fraction"); + std::ifstream file(recipe_file); + std::string line; + while (std::getline(file, line)) + { + recipe.push_back(line); + } + recipe_ = recipe; return recipe; } diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 32aab99..dadba29 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -42,11 +42,12 @@ static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); -ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file) +ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, + const std::string& recipe_file) : servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03) { LOG_INFO("Initializing RTDE client"); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_)); + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file)); if (!rtde_client_->init()) {