From 7e6b203b67e58f3ea06ff5ffd37b4190f83df333 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Fri, 14 Jun 2019 14:35:50 +0200 Subject: [PATCH] replace tcp_accuracy_checker with hash comparison This closes #34 --- ur_calibration/src/calibration_consumer.cpp | 1 + ur_calibration/src/calibration_correction.cpp | 2 - ur_rtde_driver/CMakeLists.txt | 2 +- .../ur_rtde_driver/primary/package_header.h | 1 + .../primary/robot_state/kinematics_info.h | 2 + .../ur_rtde_driver/ros/hardware_interface.h | 5 +- .../calibration_checker.h} | 55 +++++++------- ur_rtde_driver/launch/ur_common.launch | 1 + ur_rtde_driver/launch/ur_control.launch | 2 + .../primary/robot_state/kinematics_info.cpp | 14 ++++ ur_rtde_driver/src/ros/hardware_interface.cpp | 34 ++++++++- .../src/ros/tcp_accuracy_checker.cpp | 73 ------------------- ur_rtde_driver/src/ur/calibration_checker.cpp | 47 ++++++++++++ 13 files changed, 133 insertions(+), 106 deletions(-) rename ur_rtde_driver/include/ur_rtde_driver/{ros/tcp_accuracy_checker.h => ur/calibration_checker.h} (52%) delete mode 100644 ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp create mode 100644 ur_rtde_driver/src/ur/calibration_checker.cpp diff --git a/ur_calibration/src/calibration_consumer.cpp b/ur_calibration/src/calibration_consumer.cpp index 6630f69..83be906 100644 --- a/ur_calibration/src/calibration_consumer.cpp +++ b/ur_calibration/src/calibration_consumer.cpp @@ -50,6 +50,7 @@ bool CalibrationConsumer::consume( calibration.correctChain(); calibration_parameters_ = calibration.toYaml(); + calibration_parameters_["kinematics"]["hash"] = kin_info->toHash(); calibrated_ = true; } return true; diff --git a/ur_calibration/src/calibration_correction.cpp b/ur_calibration/src/calibration_correction.cpp index d9d383a..f460b48 100644 --- a/ur_calibration/src/calibration_correction.cpp +++ b/ur_calibration/src/calibration_correction.cpp @@ -46,8 +46,6 @@ using namespace ur_driver; using namespace primary_interface; using namespace ur_calibration; -static const int UR_PRIMARY_PORT = 30001; - class ParamaterMissingException : public ros::Exception { public: diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index 1a2d1a1..976417c 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -98,8 +98,8 @@ add_library(ur_rtde_driver src/rtde/text_message.cpp src/rtde/rtde_client.cpp src/ur/ur_driver.cpp + src/ur/calibration_checker.cpp src/ur/tool_communication.cpp - src/ros/tcp_accuracy_checker.cpp ) target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h b/ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h index 55f5844..00cc4b5 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h +++ b/ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h @@ -38,6 +38,7 @@ namespace ur_driver { namespace primary_interface { +static const int UR_PRIMARY_PORT = 30001; enum class RobotPackageType : int8_t { DISCONNECT = -1, diff --git a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h index b1b697f..b6fb5ff 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h +++ b/ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h @@ -51,6 +51,8 @@ public: virtual bool parseWith(comm::BinParser& bp); virtual std::string toString() const; + std::string toHash() const; + vector6uint32_t checksum_; vector6d_t dh_theta_; vector6d_t dh_a_; 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 9155f7f..a4f0605 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 @@ -41,7 +41,6 @@ #include #include "ur_rtde_driver/ur/ur_driver.h" -#include "ur_rtde_driver/ros/tcp_accuracy_checker.h" namespace ur_driver { @@ -95,6 +94,8 @@ protected: */ void publishPose(); + void checkCalibration(const std::string& checksum); + std::unique_ptr ur_driver_; hardware_interface::JointStateInterface js_interface_; @@ -133,7 +134,7 @@ protected: bool controller_reset_necessary_; - std::unique_ptr tcp_accuracy_checker_; + std::string robot_ip_; }; } // namespace ur_driver diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h b/ur_rtde_driver/include/ur_rtde_driver/ur/calibration_checker.h similarity index 52% rename from ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h rename to ur_rtde_driver/include/ur_rtde_driver/ur/calibration_checker.h index 2b48c93..81b823d 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/calibration_checker.h @@ -20,44 +20,49 @@ /*!\file * * \author Felix Mauch mauch@fzi.de - * \date 2019-06-13 + * \date 2019-06-14 * */ //---------------------------------------------------------------------- +#ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED +#define UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED -#ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED -#define UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED +#include -#include -#include +#include namespace ur_driver { -class TcpAccuracyChecker +class CalibrationChecker : public comm::IConsumer> { public: - TcpAccuracyChecker() = delete; - TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b, const double desired_accuracy); - virtual ~TcpAccuracyChecker() = default; + CalibrationChecker(const std::string& expected_hash); + virtual ~CalibrationChecker() = default; - /*! - * \brief Performs a lookup between the configured frames and checks if the distance is smaller - * than the given tolerance. - * - * \returns True of accuracy is below the tolerance - */ - bool checkAccuracy(); + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } + virtual void onTimeout() + { + } - void asyncCheck(const uint32_t interval, const uint32_t num_checks); + virtual bool consume(std::shared_ptr> product); + + bool isChecked() + { + return checked_; + } private: - tf::TransformListener tf_listener_; - tf::StampedTransform transform_; - std::string frame_a_; - std::string frame_b_; - std::unique_ptr worker_thread_; - double desired_accuracy_; - double actual_accuracy_; + std::string expected_hash_; + bool checked_; }; } // namespace ur_driver -#endif // ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED + +#endif // ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch index abb058c..d591941 100644 --- a/ur_rtde_driver/launch/ur_common.launch +++ b/ur_rtde_driver/launch/ur_common.launch @@ -33,6 +33,7 @@ + diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch index 5f99b8f..7c98798 100644 --- a/ur_rtde_driver/launch/ur_control.launch +++ b/ur_rtde_driver/launch/ur_control.launch @@ -9,6 +9,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp index 884fd2b..2dc0f9f 100644 --- a/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp +++ b/ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp @@ -87,5 +87,19 @@ std::string KinematicsInfo::toString() const return os.str(); } + +std::string KinematicsInfo ::toHash() const +{ + std::stringstream ss; + for (size_t i = 0; i < 6; ++i) + { + ss << dh_theta_[i]; + ss << dh_d_[i]; + ss << dh_a_[i]; + ss << dh_alpha_[i]; + } + std::hash hash_fn; + return "calib_" + std::to_string(hash_fn(ss.str())); +} } // namespace primary_interface } // namespace ur_driver diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index cd71c67..c78cc03 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -31,6 +31,10 @@ #include +#include +#include +#include + namespace ur_driver { HardwareInterface::HardwareInterface() @@ -50,7 +54,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h { joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } }; joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; - std::string robot_ip = robot_hw_nh.param("robot_ip", "192.168.56.101"); + 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)) @@ -66,6 +70,11 @@ 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"; @@ -130,7 +139,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h ROS_INFO_STREAM("Initializing urdriver"); try { - ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename, + ur_driver_.reset(new UrDriver(robot_ip_, script_filename, recipe_filename, std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1), std::move(tool_comm_setup))); } @@ -184,7 +193,6 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "/tf", 100)); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); - tcp_accuracy_checker_.reset(new TcpAccuracyChecker("tool0", "tool0_controller", 1e-9)); return true; } @@ -394,4 +402,24 @@ 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."); +} } // namespace ur_driver diff --git a/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp b/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp deleted file mode 100644 index a7e5da8..0000000 --- a/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- - -// -- BEGIN LICENSE BLOCK ---------------------------------------------- -// Copyright 2019 FZI Forschungszentrum Informatik -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -- END LICENSE BLOCK ------------------------------------------------ - -//---------------------------------------------------------------------- -/*!\file - * - * \author Felix Mauch mauch@fzi.de - * \date 2019-06-13 - * - */ -//---------------------------------------------------------------------- - -#include -#include - -namespace ur_driver -{ -TcpAccuracyChecker::TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b, - const double desired_accuracy) - : frame_a_(frame_a), frame_b_(frame_b), desired_accuracy_(desired_accuracy), actual_accuracy_(0) -{ - worker_thread_.reset(new std::thread(&TcpAccuracyChecker::asyncCheck, this, 2, 10)); -} - -bool TcpAccuracyChecker::checkAccuracy() -{ - try - { - tf_listener_.waitForTransform(frame_a_, frame_b_, ros::Time(0), ros::Duration(120)); - tf_listener_.lookupTransform(frame_a_, frame_b_, ros::Time(0), transform_); - } - catch (tf::TransformException ex) - { - ROS_ERROR("TF lookup error error: %s", ex.what()); - } - - actual_accuracy_ = transform_.getOrigin().length(); - return actual_accuracy_ <= desired_accuracy_; -} - -void TcpAccuracyChecker::asyncCheck(const uint32_t interval, const uint32_t num_checks) -{ - ros::Rate r(1.0 / interval); - for (size_t i = 0; i < num_checks; ++i) - { - if (!checkAccuracy()) - { - ROS_ERROR_STREAM("Desired accuracy of " - << desired_accuracy_ << " between " << frame_a_ << " and " << frame_b_ - << " was violated. Actual difference: " << actual_accuracy_ << std::endl - << "This is critical! Your robot might not be at the expected position." << std::endl - << "Use the ur_calibration tool to extract the correct calibration from the robot and pass that " - "into the description. See [TODO Link to documentation] for details."); - } - r.sleep(); - } -} -} // namespace ur_driver diff --git a/ur_rtde_driver/src/ur/calibration_checker.cpp b/ur_rtde_driver/src/ur/calibration_checker.cpp new file mode 100644 index 0000000..2337801 --- /dev/null +++ b/ur_rtde_driver/src/ur/calibration_checker.cpp @@ -0,0 +1,47 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-06-14 + * + */ +//---------------------------------------------------------------------- + +#include + +namespace ur_driver +{ +CalibrationChecker::CalibrationChecker(const std::string& expected_hash) + : expected_hash_(expected_hash), checked_(false) +{ +} +bool CalibrationChecker::consume(std::shared_ptr> product) +{ + auto kin_info = std::dynamic_pointer_cast(product); + if (kin_info != nullptr) + { + // LOG_INFO("%s", product->toString().c_str()); + // + if (kin_info->toHash() != expected_hash_) + { + LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the " + "ur_calibration tool to extract the correct calibration from the robot and pass that into the " + "description. See [TODO Link to documentation] for details."); + } + else + { + LOG_INFO("Calibration checked successfully."); + } + + checked_ = true; + } + + return true; +} +} // namespace ur_driver