diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index 85b328c..a02c035 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -95,8 +95,6 @@ protected: */ void publishPose(); - void checkCalibration(const std::string& checksum); - bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); std::unique_ptr ur_driver_; 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 aefa90d..98e64a2 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 @@ -52,7 +52,8 @@ public: * \param tool_comm_setup Configuration for using the tool communication */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, - std::function handle_program_state, std::unique_ptr tool_comm_setup); + std::function handle_program_state, std::unique_ptr tool_comm_setup, + const std::string& calibration_checksum = ""); /*! * \brief Constructs a new UrDriver object. * @@ -60,8 +61,9 @@ public: * \param script_file URScript file that should be sent to the robot */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, - std::function handle_program_state) - : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr{}) + std::function handle_program_state, const std::string& calibration_checksum = "") + : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr{}, + calibration_checksum) { } @@ -111,6 +113,8 @@ public: void startWatchdog(); + void checkCalibration(const std::string& checksum); + private: std::string readScriptFile(const std::string& filename); std::string readKeepalive(); @@ -129,6 +133,8 @@ private: bool reverse_interface_active_; uint32_t reverse_port_; std::function handle_program_state_; + + std::string robot_ip_; }; } // namespace ur_driver #endif // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index f113c2d..bde564b 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -31,10 +31,6 @@ #include -#include -#include -#include - namespace ur_driver { HardwareInterface::HardwareInterface() @@ -72,10 +68,6 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h std::string tf_prefix = robot_hw_nh.param("tf_prefix", ""); - ROS_INFO_STREAM("Checking if calibration data matches connected robot."); - std::string calibration_checksum = robot_hw_nh.param("kinematics/hash", ""); - checkCalibration(calibration_checksum); - program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); tcp_transform_.header.frame_id = "base"; tcp_transform_.child_frame_id = "tool0_controller"; @@ -137,12 +129,13 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h tool_comm_setup->setTxIdleChars(tx_idle_chars); } + std::string calibration_checksum = robot_hw_nh.param("kinematics/hash", ""); ROS_INFO_STREAM("Initializing urdriver"); try { ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename, std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1), - std::move(tool_comm_setup))); + std::move(tool_comm_setup), calibration_checksum)); } catch (ur_driver::ToolCommNotAvailable& e) { @@ -435,27 +428,6 @@ void HardwareInterface ::publishPose() } } -void HardwareInterface ::checkCalibration(const std::string& checksum) -{ - comm::URStream stream(robot_ip_, - ur_driver::primary_interface::UR_PRIMARY_PORT); - primary_interface::PrimaryParser parser; - comm::URProducer prod(stream, parser); - - CalibrationChecker consumer(checksum); - - comm::INotifier notifier; - - comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); - pipeline.run(); - - while (!consumer.isChecked()) - { - ros::Duration(1).sleep(); - } - ROS_DEBUG_STREAM("Got calibration information from robot."); -} - bool HardwareInterface::stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { if (isRobotProgramRunning()) diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 6be03ad..b3b885f 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -32,8 +32,11 @@ #include "ur_rtde_driver/ur/ur_driver.h" #include "ur_rtde_driver/exceptions.h" +#include "ur_rtde_driver/primary/primary_parser.h" #include +#include + namespace ur_driver { static const int32_t MULT_JOINTSTATE = 1000000; @@ -45,16 +48,17 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, std::function handle_program_state, - std::unique_ptr tool_comm_setup) + std::unique_ptr tool_comm_setup, const std::string& calibration_checksum) : servoj_time_(0.008) , servoj_gain_(2000) , servoj_lookahead_time_(0.03) , reverse_interface_active_(false) , handle_program_state_(handle_program_state) + , robot_ip_(robot_ip) { LOG_DEBUG("Initializing urdriver"); LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file)); + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, recipe_file)); if (!rtde_client_->init()) { @@ -66,6 +70,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc std::string local_ip = rtde_client_->getIP(); + LOG_INFO("Checking if calibration data matches connected robot."); + checkCalibration(calibration_checksum); + uint32_t reverse_port = 50001; // TODO: Make this a parameter uint32_t script_sender_port = 50002; // TODO: Make this a parameter @@ -205,4 +212,26 @@ std::string UrDriver::readKeepalive() return std::string(""); } } + +void UrDriver::checkCalibration(const std::string& checksum) +{ + comm::URStream stream(robot_ip_, + ur_driver::primary_interface::UR_PRIMARY_PORT); + primary_interface::PrimaryParser parser; + comm::URProducer prod(stream, parser); + + CalibrationChecker consumer(checksum); + + comm::INotifier notifier; + + comm::Pipeline pipeline(prod, consumer, "Pipeline", notifier); + pipeline.run(); + + while (!consumer.isChecked()) + { + ros::Duration(1).sleep(); + } + ROS_DEBUG_STREAM("Got calibration information from robot."); +} + } // namespace ur_driver