Skip to content

Commit

Permalink
Add throwing versions of ROS parameter loading functions (#21)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jdlangs
jdlangs authored May 18, 2020
1 parent 64602e3 commit 3604688
Show file tree
Hide file tree
Showing 2 changed files with 229 additions and 51 deletions.
69 changes: 69 additions & 0 deletions rct_ros_tools/include/rct_ros_tools/parameter_loaders.h
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,88 @@
#ifndef RCT_PARAMETER_LOADERS_H
#define RCT_PARAMETER_LOADERS_H

#include <Eigen/Geometry>

#include <rct_image_tools/modified_circle_grid_target.h>
#include <rct_optimizations/types.h>
#include <ros/node_handle.h>

namespace rct_ros_tools
{

/**
* \brief Thrown during errors in loading or parsing data files
*/
class BadFileException : public std::runtime_error
{
public:
BadFileException(const std::string& what) : std::runtime_error(what) {}
};

/**
* \brief Load a target from a ROS parameter
* \throws ros::InvalidParameterException
*/
rct_image_tools::ModifiedCircleGridTarget loadTarget(const ros::NodeHandle& nh, const std::string& key);

/**
* \brief Load a target from a ROS parameter. Returns false if an error occurs.
*/
bool loadTarget(const ros::NodeHandle& nh, const std::string& key, rct_image_tools::ModifiedCircleGridTarget& target);

/**
* \brief Load a target from a YAML file
* \throws rct_ros_tools::BadFileException
*/
rct_image_tools::ModifiedCircleGridTarget loadTarget(const std::string& path);

/**
* \brief Load a target from a YAML file. Returns false if an error occurs.
*/
bool loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target);

/**
* \brief Load camera intrinsics from a ROS parameter
* \throws ros::InvalidParameterException
*/
rct_optimizations::CameraIntrinsics loadIntrinsics(const ros::NodeHandle& nh, const std::string& key);

/**
* \brief Load a target from a ROS parameter. Returns false if an error occurs.
*/
bool loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr);

/**
* \brief Load camera intrinsics from a YAML file
* \throws rct_ros_tools::BadFileException
*/
rct_optimizations::CameraIntrinsics loadIntrinsics(const std::string& path);

/**
* \brief Load camera intrinsics from a YAML file. Returns false if an error occurs.
*/
bool loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics);

/**
* \brief Load a pose from a ROS parameter
* \throws ros::InvalidParameterException
*/
Eigen::Isometry3d loadPose(const ros::NodeHandle& nh, const std::string& key);

/**
* \brief Load a pose from a ROS parameter. Returns false if an error occurs.
*/
bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose);

/**
* \brief Load a pose from a YAML file
* \throws rct_ros_tools::BadFileException
*/
Eigen::Isometry3d loadPose(const std::string& path);

/**
* \brief Load a pose from a YAML file. Returns false if an error occurs.
*/
bool loadPose(const std::string& path, Eigen::Isometry3d& pose);

}
Expand Down
211 changes: 160 additions & 51 deletions rct_ros_tools/src/data_loader/parameter_loaders.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,90 +15,199 @@ static bool read(XmlRpc::XmlRpcValue& xml, const std::string& key, T& value)
return true;
}

bool rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key,
rct_image_tools::ModifiedCircleGridTarget& target)
rct_image_tools::ModifiedCircleGridTarget rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key)
{
XmlRpc::XmlRpcValue xml;
if (!nh.getParam(key, xml)) return false;
if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key);

int rows = 0;
int cols = 0;
double spacing = 0.0;

if (!read(xml, "rows", rows)) return false;
if (!read(xml, "cols", cols)) return false;
if (!read(xml, "spacing", spacing)) return false;
if (!read(xml, "rows", rows)) throw ros::InvalidParameterException(key + "/rows");
if (!read(xml, "cols", cols)) throw ros::InvalidParameterException(key + "/cols");
if (!read(xml, "spacing", spacing)) throw ros::InvalidParameterException(key + "/spacing");

return rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing);
}

target = rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing);
bool rct_ros_tools::loadTarget(const ros::NodeHandle& nh, const std::string& key,
rct_image_tools::ModifiedCircleGridTarget& target)
{
try
{
target = loadTarget(nh, key);
}
catch (ros::InvalidParameterException &ex)
{
ROS_ERROR_STREAM("Failed to load target parameter: " << ex.what());
return false;
}
return true;
}

bool rct_ros_tools::loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target)
rct_image_tools::ModifiedCircleGridTarget rct_ros_tools::loadTarget(const std::string &path)
{
YAML::Node n = YAML::LoadFile(path);
int rows = n["target_definition"]["rows"].as<int>();
int cols = n["target_definition"]["cols"].as<int>();
double spacing = n["target_definition"]["spacing"].as<double>(); // (meters between dot centers)
try
{
YAML::Node n = YAML::LoadFile(path);
int rows = n["target_definition"]["rows"].as<int>();
int cols = n["target_definition"]["cols"].as<int>();
double spacing = n["target_definition"]["spacing"].as<double>(); // (meters between dot centers)
return rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing);
}
catch (YAML::Exception &ex)
{
throw BadFileException(std::string("YAML failure: ") + ex.what());
}
}

target = rct_image_tools::ModifiedCircleGridTarget(rows, cols, spacing);
bool rct_ros_tools::loadTarget(const std::string& path, rct_image_tools::ModifiedCircleGridTarget& target)
{
try
{
target = loadTarget(path);
}
catch (ros::InvalidParameterException &ex)
{
ROS_ERROR_STREAM("Failed to load target from file: " << ex.what());
return false;
}
return true;
}

rct_optimizations::CameraIntrinsics rct_ros_tools::loadIntrinsics(const ros::NodeHandle &nh, const std::string &key)
{
XmlRpc::XmlRpcValue xml;
if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key);

rct_optimizations::CameraIntrinsics intr;
if (!read(xml, "fx", intr.fx())) throw ros::InvalidParameterException(key + "/fx");
if (!read(xml, "fy", intr.fy())) throw ros::InvalidParameterException(key + "/fy");
if (!read(xml, "cx", intr.cx())) throw ros::InvalidParameterException(key + "/cx");
if (!read(xml, "cy", intr.cy())) throw ros::InvalidParameterException(key + "/cy");

return intr;
}

bool rct_ros_tools::loadIntrinsics(const ros::NodeHandle& nh, const std::string& key,
rct_optimizations::CameraIntrinsics& intr)
{
XmlRpc::XmlRpcValue xml;
if (!nh.getParam(key, xml)) return false;
try
{
intr = loadIntrinsics(nh, key);
}
catch (ros::InvalidParameterException &ex)
{
ROS_ERROR_STREAM("Failed to load intrisic parameter: " << ex.what());
return false;
}
return true;
}

rct_optimizations::CameraIntrinsics temp_intr;
if (!read(xml, "fx", temp_intr.fx())) return false;
if (!read(xml, "fy", temp_intr.fy())) return false;
if (!read(xml, "cx", temp_intr.cx())) return false;
if (!read(xml, "cy", temp_intr.cy())) return false;
rct_optimizations::CameraIntrinsics rct_ros_tools::loadIntrinsics(const std::string& path)
{
try
{
YAML::Node n = YAML::LoadFile(path);

rct_optimizations::CameraIntrinsics intrinsics;
intrinsics.cx() = n["cx"].as<double>();
intrinsics.cy() = n["cy"].as<double>();
intrinsics.fx() = n["fx"].as<double>();
intrinsics.fy() = n["fy"].as<double>();
return intrinsics;
}
catch (YAML::Exception &ex)
{
throw BadFileException(std::string("YAML failure: ") + ex.what());
}
}

intr = temp_intr;
bool rct_ros_tools::loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics)
{
try
{
intrinsics = loadIntrinsics(path);
}
catch (BadFileException &ex)
{
ROS_ERROR_STREAM("Failed to load intrinsics from file: " << ex.what());
return false;
}
return true;
}

bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key,
Eigen::Isometry3d& pose)
Eigen::Isometry3d rct_ros_tools::loadPose(const ros::NodeHandle &nh, const std::string &key)
{
XmlRpc::XmlRpcValue xml;
if (!nh.getParam(key, xml)) return false;
if (!nh.getParam(key, xml)) throw ros::InvalidParameterException(key);

pose = Eigen::Isometry3d::Identity();
double x, y, z, qx, qy, qz, qw;
if (!read(xml, "x", x)) return false;
if (!read(xml, "y", y)) return false;
if (!read(xml, "z", z)) return false;
if (!read(xml, "qx", qx)) return false;
if (!read(xml, "qy", qy)) return false;
if (!read(xml, "qz", qz)) return false;
if (!read(xml, "qw", qw)) return false;

pose.translation() = Eigen::Vector3d(x, y, z);
pose.linear() = Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
if (!read(xml, "x", x)) throw ros::InvalidParameterException(key + "/x");
if (!read(xml, "y", y)) throw ros::InvalidParameterException(key + "/y");
if (!read(xml, "z", z)) throw ros::InvalidParameterException(key + "/z");
if (!read(xml, "qx", qx)) throw ros::InvalidParameterException(key + "/qx");
if (!read(xml, "qy", qy)) throw ros::InvalidParameterException(key + "/qy");
if (!read(xml, "qz", qz)) throw ros::InvalidParameterException(key + "/qz");
if (!read(xml, "qw", qw)) throw ros::InvalidParameterException(key + "/qw");

Eigen::Isometry3d pose =
Eigen::Translation3d(Eigen::Vector3d(x, y, z)) *
Eigen::AngleAxisd(Eigen::Quaterniond(qw, qx, qy, qz));
return pose;
}

bool rct_ros_tools::loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose)
{
try
{
pose = loadPose(nh, key);
}
catch (ros::InvalidParameterException &ex)
{
ROS_ERROR_STREAM("Failed to load pose parameter: " << ex.what());
return false;
}
return true;
}

Eigen::Isometry3d rct_ros_tools::loadPose(const std::string& path)
{
try
{
YAML::Node n = YAML::LoadFile(path);

Eigen::Vector3d position;
position(0) = n["x"].as<double>();
position(1) = n["y"].as<double>();
position(2) = n["z"].as<double>();

Eigen::Quaterniond quat;
quat.w() = n["qw"].as<double>();
quat.x() = n["qx"].as<double>();
quat.y() = n["qy"].as<double>();
quat.z() = n["qz"].as<double>();

Eigen::Isometry3d pose = Eigen::Translation3d(position) * Eigen::AngleAxisd(quat);
return pose;
}
catch (YAML::Exception &ex)
{
throw BadFileException(std::string("YAML failure: ") + ex.what());
}
}

bool rct_ros_tools::loadPose(const std::string& path, Eigen::Isometry3d& pose)
{
YAML::Node n = YAML::LoadFile(path);
Eigen::Vector3d position;

position(0) = n["x"].as<double>();
position(1) = n["y"].as<double>();
position(2) = n["z"].as<double>();

double qw, qx, qy, qz;
qw = n["qw"].as<double>();
qx = n["qx"].as<double>();
qy = n["qy"].as<double>();
qz = n["qz"].as<double>();

pose = Eigen::Isometry3d::Identity();
pose.translation() = position;
pose.linear() = Eigen::Quaterniond(qw, qx, qy, qz).toRotationMatrix();
try
{
pose = loadPose(path);
}
catch (BadFileException &ex)
{
ROS_ERROR_STREAM("Failed to load pose from file: " << ex.what());
return false;
}
return true;
}

0 comments on commit 3604688

Please sign in to comment.