Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

replace tcp_accuracy_checker with hash comparison

Browse Source 
This closes #34
This commit is contained in:
Felix Mauch
2019-06-14 14:35:50 +02:00
parent 34eb8dcf08
commit 7e6b203b67
13 changed files with 133 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ur_rtde_driver/CMakeLists.txt
View File

@@ -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})

1
ur_rtde_driver/include/ur_rtde_driver/primary/package_header.h
View File

@@ -38,6 +38,7 @@ namespace ur_driver
{
namespace primary_interface
{
static const int UR_PRIMARY_PORT = 30001;
enum class RobotPackageType : int8_t
{
DISCONNECT = -1,

2
ur_rtde_driver/include/ur_rtde_driver/primary/robot_state/kinematics_info.h
View File

@@ -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_;

5
ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
View File

@@ -41,7 +41,6 @@
#include <ur_controllers/scaled_joint_command_interface.h>
#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<UrDriver> ur_driver_;
hardware_interface::JointStateInterface js_interface_;
@@ -133,7 +134,7 @@ protected:
bool controller_reset_necessary_;
std::unique_ptr<TcpAccuracyChecker> tcp_accuracy_checker_;
std::string robot_ip_;
};
} // namespace ur_driver

55
ur_rtde_driver/include/ur_rtde_driver/ros/tcp_accuracy_checker.h → ur_rtde_driver/include/ur_rtde_driver/ur/calibration_checker.h
View File

@@ -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 <ur_rtde_driver/comm/pipeline.h>
#include <tf/transform_listener.h>
#include <thread>
#include <ur_rtde_driver/primary/robot_state/kinematics_info.h>
namespace ur_driver
{
class TcpAccuracyChecker
class CalibrationChecker : public comm::IConsumer<comm::URPackage<primary_interface::PackageHeader>>
{
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<comm::URPackage<primary_interface::PackageHeader>> product);
bool isChecked()
{
return checked_;
}
private:
tf::TransformListener tf_listener_;
tf::StampedTransform transform_;
std::string frame_a_;
std::string frame_b_;
std::unique_ptr<std::thread> 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

1
ur_rtde_driver/launch/ur_common.launch
View File

@@ -33,6 +33,7 @@
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>

2
ur_rtde_driver/launch/ur_control.launch
View File

@@ -9,6 +9,7 @@
<arg name="use_tool_communication"/>
<arg name="controller_config_file"/>
<arg name="robot_ip"/>
<arg name="kinematics_config"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default=""/>
@@ -26,6 +27,7 @@
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
<param name="robot_ip" type="str" value="$(arg robot_ip)"/>
<rosparam command="load" file="$(arg kinematics_config)" />
<param name="script_file" value="$(arg urscript_file)"/>
<param name="recipe_file" value="$(arg rtde_recipe_file)"/>
<param name="tf_prefix" value="$(arg tf_prefix)"/>

14
ur_rtde_driver/src/primary/robot_state/kinematics_info.cpp
View File

@@ -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<std::string> hash_fn;
return "calib_" + std::to_string(hash_fn(ss.str()));
}
} // namespace primary_interface
} // namespace ur_driver

34
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -31,6 +31,10 @@
#include <Eigen/Geometry>
#include <ur_rtde_driver/primary/package_header.h>
#include <ur_rtde_driver/primary/primary_parser.h>
#include <ur_rtde_driver/ur/calibration_checker.h>
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<std::string>("robot_ip", "192.168.56.101");
robot_ip_ = robot_hw_nh.param<std::string>("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<std::string>("tf_prefix", "");
ROS_INFO_STREAM("Checking if calibration data matches connected robot.");
std::string calibration_checksum = robot_hw_nh.param<std::string>("kinematics/hash", "");
checkCalibration(calibration_checksum);
program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("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<tf2_msgs::TFMessage>(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<ur_driver::primary_interface::PackageHeader> stream(robot_ip_,
ur_driver::primary_interface::UR_PRIMARY_PORT);
primary_interface::PrimaryParser parser;
comm::URProducer<ur_driver::primary_interface::PackageHeader> prod(stream, parser);
CalibrationChecker consumer(checksum);
comm::INotifier notifier;
comm::Pipeline<ur_driver::primary_interface::PackageHeader> 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

73
ur_rtde_driver/src/ros/tcp_accuracy_checker.cpp
View File

@@ -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 <ur_rtde_driver/ros/tcp_accuracy_checker.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
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

47
ur_rtde_driver/src/ur/calibration_checker.cpp Normal file
View File

@@ -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 <ur_rtde_driver/ur/calibration_checker.h>
namespace ur_driver
{
CalibrationChecker::CalibrationChecker(const std::string& expected_hash)
: expected_hash_(expected_hash), checked_(false)
{
}
bool CalibrationChecker::consume(std::shared_ptr<comm::URPackage<primary_interface::PackageHeader>> product)
{
auto kin_info = std::dynamic_pointer_cast<primary_interface::KinematicsInfo>(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