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

Improved handling of invalid save paths

Browse Source
This commit is contained in:
Felix Mauch
2019-05-28 16:51:44 +02:00
parent 0cced4a6c3
commit a603e9bb97
1 changed files with 123 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

193
ur_calibration/src/calibration_correction.cpp
View File

@@ -44,88 +44,141 @@ public:
}
};
template <typename T>
T getRequiredParameter(const ros::NodeHandle& nh, const std::string& param_name)
class CalibrationCorrection
{
T ret_val;
if (nh.hasParam(param_name))
public:
CalibrationCorrection(const ros::NodeHandle& nh) : nh_(nh)
{
nh.getParam(param_name, ret_val);
}
else
{
throw ParamaterMissingException(nh.resolveName(param_name));
subfolder_ = nh_.param<std::string>("subfolder_name", "etc");
std::string output_package_name;
try
{
// The robot's IP address
robot_ip_ = getRequiredParameter<std::string>("robot_ip");
// Name with which the robot will be referenced. Will be used for the filename the calibration
// data is stored in. This can be any arbitrary name. If left empty, the robot's serial number
// will be used.
robot_name_ = getRequiredParameter<std::string>("robot_name");
// The resulting parameter file will be stored inside
// <output_package_name>/subfolder/<robot_name>_calibration.yaml
output_package_name = getRequiredParameter<std::string>("output_package_name");
}
catch (const ParamaterMissingException& e)
{
ROS_FATAL_STREAM(e.what());
exit(1);
}
package_path_ = ros::package::getPath(output_package_name);
if (package_path_ == "")
{
ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there.");
exit(1);
}
}
return ret_val;
}
virtual ~CalibrationCorrection() = default;
void run()
{
comm::URStream<PackageHeader> stream(robot_ip_, UR_PRIMARY_PORT);
primary_interface::PrimaryParser parser;
comm::URProducer<PackageHeader> prod(stream, parser);
CalibrationConsumer consumer;
comm::INotifier notifier;
comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
pipeline.run();
while (!consumer.isCalibrated())
{
ros::Duration(0.1).sleep();
}
calibration_data_.reset(new YAML::Node);
*calibration_data_ = consumer.getCalibrationParameters();
}
bool writeCalibrationData()
{
if (calibration_data_ == nullptr)
{
ROS_ERROR_STREAM("Calibration data not yet set.");
return false;
}
ROS_INFO_STREAM("Writing calibration data");
fs::path dst_path = fs::path(package_path_) / fs::path(subfolder_);
if (!fs::exists(dst_path))
{
try
{
fs::create_directory(dst_path);
}
catch (const boost::filesystem::filesystem_error& e)
{
ROS_ERROR_STREAM("Could not create " << dst_path << ". Reason: " << e.what());
return false;
}
}
fs::path output_filename = dst_path / fs::path(robot_name_ + "_calibration.yaml");
if (fs::exists(output_filename))
{
ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting.");
}
std::ofstream file(output_filename.string());
if (file.is_open())
{
file << *calibration_data_;
}
else
{
ROS_ERROR_STREAM("Failed writing the file. Do you have the correct rights?");
return false;
}
ROS_INFO_STREAM("Wrote output to " << output_filename);
return true;
}
private:
template <typename T>
T getRequiredParameter(const std::string& param_name) const
{
T ret_val;
if (nh_.hasParam(param_name))
{
nh_.getParam(param_name, ret_val);
}
else
{
throw ParamaterMissingException(nh_.resolveName(param_name));
}
return ret_val;
}
ros::NodeHandle nh_;
std::string robot_ip_;
std::string robot_name_;
std::string package_path_;
std::string subfolder_;
std::unique_ptr<YAML::Node> calibration_data_;
};
int main(int argc, char* argv[])
{
ros::init(argc, argv, "ur_calibration");
ros::NodeHandle nh("~");
std::string subfolder = nh.param<std::string>("subfolder_name", "etc");
std::string robot_ip, robot_name, output_package_name;
try
CalibrationCorrection my_calibration_correction(nh);
my_calibration_correction.run();
if (!my_calibration_correction.writeCalibrationData())
{
// The robot's IP address
robot_ip = getRequiredParameter<std::string>(nh, "robot_ip");
// Name with which the robot will be referenced. Will be used for the filename the calibration
// data is stored in. This can be any arbitrary name. If left empty, the robot's serial number
// will be used.
robot_name = getRequiredParameter<std::string>(nh, "robot_name");
// The resulting parameter file will be stored inside
// <output_package_name>/subfolder/<robot_name>_calibration.yaml
output_package_name = getRequiredParameter<std::string>(nh, "output_package_name");
}
catch (const ParamaterMissingException& e)
{
ROS_FATAL_STREAM(e.what());
ROS_ERROR_STREAM("Failed writing calibration data. See errors above for details.");
return -1;
}
std::string package_path = ros::package::getPath(output_package_name);
if (package_path == "")
{
ROS_FATAL_STREAM("Could not find package " << output_package_name << ". Cannot save output file there.");
return -1;
}
comm::URStream<PackageHeader> stream(robot_ip, UR_PRIMARY_PORT);
primary_interface::PrimaryParser parser;
comm::URProducer<PackageHeader> prod(stream, parser);
CalibrationConsumer consumer;
comm::INotifier notifier;
comm::Pipeline<PackageHeader> pipeline(prod, consumer, "Pipeline", notifier);
pipeline.run();
while (!consumer.isCalibrated())
{
ros::Duration(0.1).sleep();
}
pipeline.stop();
std::ofstream file;
fs::path dst_path = fs::path(package_path) / fs::path(subfolder);
if (!fs::exists(dst_path))
{
fs::create_directory(dst_path);
}
fs::path output_filename = dst_path / fs::path(robot_name + "_calibration.yaml");
if (fs::exists(output_filename))
{
ROS_WARN_STREAM("Output file " << output_filename << " already exists. Overwriting.");
}
file.open(output_filename.string());
YAML::Node calibration_parameters = consumer.getCalibrationParameters();
file << calibration_parameters;
file.close();
ROS_INFO_STREAM("Corrected calibration: " << std::endl << calibration_parameters);
ROS_INFO("Calibration correction done");
return 0;
}