From fd6a9ee36507b13093342b830f102d1fed6dedd1 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 13 Jun 2019 15:05:01 +0200 Subject: [PATCH] Add a checker to inform the user about an uncalibrated robot. --- ur_rtde_driver/CMakeLists.txt | 3 + .../ur_rtde_driver/ros/hardware_interface.h | 3 + .../ur_rtde_driver/ros/tcp_accuracy_checker.h | 50 ++++++++++++++++ ur_rtde_driver/package.xml | 1 + ur_rtde_driver/src/ros/hardware_interface.cpp | 7 ++- .../src/ros/tcp_accuracy_checker.cpp | 60 +++++++++++++++++++ 6 files changed, 121 insertions(+), 3 deletions(-) create mode 100644 ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h create mode 100644 ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index 23279a5..9336caa 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED roscpp sensor_msgs std_srvs + tf tf2_geometry_msgs tf2_msgs trajectory_msgs @@ -41,6 +42,7 @@ catkin_package( industrial_msgs roscpp sensor_msgs + tf tf2_geometry_msgs tf2_msgs trajectory_msgs @@ -99,6 +101,7 @@ add_library(ur_rtde_driver src/rtde/rtde_client.cpp src/ur/ur_driver.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/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index dd620a6..9155f7f 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,6 +41,7 @@ #include #include "ur_rtde_driver/ur/ur_driver.h" +#include "ur_rtde_driver/ros/tcp_accuracy_checker.h" namespace ur_driver { @@ -131,6 +132,8 @@ protected: ros::Publisher program_state_pub_; bool controller_reset_necessary_; + + std::unique_ptr tcp_accuracy_checker_; }; } // 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/ros/tcp_accuracy_checker.h new file mode 100644 index 0000000..743a6eb --- /dev/null +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h @@ -0,0 +1,50 @@ +// 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-13 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED +#define UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED + +#include +#include + +namespace ur_driver +{ +class TcpAccuracyChecker +{ +public: + TcpAccuracyChecker() = delete; + TcpAccuracyChecker(const std::string& frame_a, const std::string& frame_b, const double desired_accuracy); + virtual ~TcpAccuracyChecker() = 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(); + + void asyncCheck(const uint32_t interval, const uint32_t num_checks); + +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_; +}; +} // namespace ur_driver +#endif // ifndef UR_RTDE_DRIVER_ROS_TCP_ACCURACY_CHECKER_H_INCLUDED diff --git a/ur_rtde_driver/package.xml b/ur_rtde_driver/package.xml index 8f2a569..a90a992 100644 --- a/ur_rtde_driver/package.xml +++ b/ur_rtde_driver/package.xml @@ -28,6 +28,7 @@ industrial_msgs roscpp sensor_msgs + tf tf2_msgs tf2_geometry_msgs trajectory_msgs diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 9de6c74..cd71c67 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -65,10 +65,10 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h return false; } - tcp_link_ = robot_hw_nh.param("tcp_link", "tool0"); + std::string tf_prefix = robot_hw_nh.param("tf_prefix", ""); program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); tcp_transform_.header.frame_id = "base"; - tcp_transform_.child_frame_id = "tool_0_controller"; + tcp_transform_.child_frame_id = "tool0_controller"; bool use_tool_communication = robot_hw_nh.param("use_tool_communication", "false"); std::unique_ptr tool_comm_setup; @@ -172,7 +172,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_)); fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle( - "wrench", tcp_link_, fts_measurements_.begin(), fts_measurements_.begin() + 3)); + "wrench", "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3)); // Register interfaces registerInterface(&js_interface_); @@ -184,6 +184,7 @@ 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; } diff --git a/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp b/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp new file mode 100644 index 0000000..6851caa --- /dev/null +++ b/ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp @@ -0,0 +1,60 @@ +// 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-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