Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +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

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